artrack_alvar에서 프레임 ID를 가져 오지 못합니다.

Question

카메라의 자세를 얻으 려하고 월드 프레임을 기준으로 설정하려고합니다. msg에서 frame_id를 가져 와서 월드 프레임과 관련하여 여러 개의 카메라를 동적으로 설정할 수 있습니다.

저는 Asus Xtion Pro live를 사용하고 있으므로 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; 

} 

출력 장미 주제 에코/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

ROS_INFO 로깅 라인의 형식이 잘못되어 있기 때문에 frame_id가 인쇄되지 않습니다. 다음 형식 중 하나 여야합니다.

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).