不能從ar_track_alvar獲取frame_id

Question

我想獲取相機的姿勢並將其設置爲相對於世界框架。我想從msg中獲取frame_id，這樣我就可以動態地設置多個相機相對於世界框架。

我現在使用的是Asus Xtion pro，所以我用pr2_indiv_no_kinect.launch啓動了ar_track_alvar。

這是我做了什麼，

啓動文件，

<launch> 
    <arg name="marker_size" default="4.4" /> 
    <arg name="max_new_marker_error" default="0.08" /> 
    <arg name="max_track_error" default="0.2" /> 
    <arg name="cam_image_topic" default="/camera1/rgb/image_rect_color" /> 
    <arg name="cam_info_topic" default="/camera1/rgb/camera_info" />  
    <arg name="output_frame" default="/camera1_link" /> 

    <node name="ar_track_alvar" pkg="ar_track_alvar" type="individualMarkersNoKinect" respawn="false" output="screen" args="$(arg marker_size) $(arg max_new_marker_error) $(arg max_track_error) $(arg cam_image_topic) $(arg cam_info_topic) $(arg output_frame)" /> 
</launch> 

我的ROS節點，rosrun camera_tf_pose camera_tf_pose的

[ INFO] [1475225608.355125575]: req.header.frame_id . . . . . .. . .. 
[ INFO] [1475225608.355185064]: this gets printed . . 
[ INFO] [1475225608.454772325]: req.header.frame_id . . . . . .. . .. 
[ INFO] [1475225608.454802236]: this gets printed . . 
[ INFO] [1475225608.555007653]: req.header.frame_id . . . . . .. . .. 
[ INFO] [1475225608.555137160]: this gets printed . . 

輸出的

#include <ros/ros.h> 
#include <tf/transform_datatypes.h> 
#include <ar_track_alvar_msgs/AlvarMarkers.h> 
#include<tf/transform_broadcaster.h> 
#include <tf/transform_listener.h> 
#include<iostream> 
#include <string> 

void cb(ar_track_alvar_msgs::AlvarMarkers req) { 

    tf::TransformBroadcaster tf_br; 
    tf::TransformListener listener; 
    static tf::Transform transform; 

    if (!req.markers.empty()) { 
    tf::Quaternion q(req.markers[0].pose.pose.orientation.x, req.markers[0].pose.pose.orientation.y, req.markers[0].pose.pose.orientation.z, req.markers[0].pose.pose.orientation.w); 
    transform.setOrigin(tf::Vector3(ceil(req.markers[0].pose.pose.position.x), ceil(req.markers[0].pose.pose.position.y), ceil(req.markers[0].pose.pose.position.z))); 
    transform.setOrigin(tf::Vector3(req.markers[0].pose.pose.position.x, req.markers[0].pose.pose.position.y, req.markers[0].pose.pose.position.z)); 
    transform.setRotation(tf::Quaternion(req.markers[0].pose.pose.orientation.x, req.markers[0].pose.pose.orientation.y, req.markers[0].pose.pose.orientation.z, req.markers[0].pose.pose.orientation.w)); 

    try{ 

     // this doesn't prints the frame id. 
     ROS_INFO("req.header.frame_id . . . . . .. . .. ", req.header.frame_id.c_str()); 

     if(req.header.frame_id.compare("/camera1_link")) 
     { 
      ROS_INFO("this gets printed . . "); 
     // this works . . I mean string comparision returns true. 
     // I want to set frame_id to the tf tree. 

     // listener.waitForTransform(req.header.frame_id, "world", ros::Time::now(), ros::Duration(1.0)); 
     // tf_br.sendTransform(tf::StampedTransform(transform.inverse(), ros::Time::now(), "world", req.header.frame_id)); 

     } 

    } 
    catch (tf::TransformException ex){ 
     ROS_ERROR("%s",ex.what()); 
     ros::Duration(1.0).sleep(); 
    } 
    } 
} 

int main(int argc, char **argv) { 
    ros::init(argc, argv, "camera_tf_pose"); 
    ros::NodeHandle nh; 
    ros::Subscriber sub = nh.subscribe("ar_pose_marker", 1, &cb); 

    ros::spin(); 
    return 0; 

} 

輸出ROS話題回聲/ ar_pose_marker

markers: 
    - 
    header: 
     seq: 0 
     stamp: 
     secs: 1475225585 
     nsecs: 290621273 
     frame_id: /camera1_link 
    id: 14 
    confidence: 0 
    pose: 
     header: 
     seq: 0 
     stamp: 
      secs: 0 
      nsecs:   0 
     frame_id: '' 
     pose: 
     position: 
      x: 0.310138838061 
      y: -0.0777276835864 
      z: -0.00489581265903 
     orientation: 
      x: 0.158053463521 
      y: -0.431284842866 
      z: 0.021097283333 
      w: 0.888013170859 

當我取消以下行

，

// listener.waitForTransform(req.header.frame_id, "world", ros::Time::now(), ros::Duration(1.0)); 
     // tf_br.sendTransform(tf::StampedTransform(transform.inverse(), ros::Time::now(), "world", req.header.frame_id)); 

我得到以下輸出：

[ INFO] [1475225923.155792267]: req.header.frame_id . . . . . .. . .. 
[ INFO] [1475225923.155849162]: this gets printed . . 
Warning: Invalid argument passed to canTransform argument target_frame in tf2 frame_ids cannot be empty 
     at line 122 in /tmp/binarydeb/ros-indigo-tf2-0.5.13/src/buffer_core.cpp 
Warning: Invalid argument passed to canTransform argument target_frame in tf2 frame_ids cannot be empty 
     at line 122 in /tmp/binarydeb/ros-indigo-tf2-0.5.13/src/buffer_core.cpp 
Warning: Invalid argument passed to canTransform argument target_frame in tf2 frame_ids cannot be empty 
     at line 122 in /tmp/binarydeb/ros-indigo-tf2-0.5.13/src/buffer_core.cpp 
Warning: Invalid argument passed to canTransform argument target_frame in tf2 frame_ids cannot be empty 
     at line 122 in /tmp/binarydeb/ros-indigo-tf2-0.5.13/src/buffer_core.cpp 
Warning: Invalid argument passed to canTransform argument target_frame in tf2 frame_ids cannot be empty 
     at line 122 in /tmp/binarydeb/ros-indigo-tf2-0.5.13/src/buffer_core.cpp 

請幫我解決這個問題。在此先感謝

Answer 1

frame_id不打印，因爲您的ROS_INFO日誌行格式不正確。它應該是以下任一格式：

ROS_INFO("req.header.frame_id: %s", req.header.frame_id.c_str()); 
ROS_INFO_STREAM("req.header.frame_id " << req.header.frame_id.c_str()); 

（注意您缺少的％s）。