cant get frame_id of ar_track_alvar pose
I am trying to get pose of the camera and set it with respect to the world frame. I want to get the frame_id from msg, so that I can set multiple cameras with respect to world frame dynamically.
I am using Asus Xtion pro live, so I launch ar_track_alvar with pr2_indiv_no_kinect.launch.
This is what I have done,
Launch file,
<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>
My ros Node,
#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;
}
Output of 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 . .
Output of rostopic echo /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
when I uncomment the following lines,
// listener.waitForTransform(req.header.frame_id ...
Can you edit your question to add the runtime output?
@130s I have updated question