how to set dynamic tf between camera_link and world frame
I am new to ROS and I am trying to set tf between camera_link and world frame dynamically with alvar marker using TransformBroadcaster.
this is the code im trying,
#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>
void cb(ar_track_alvar_msgs::AlvarMarkers req) {
tf::TransformBroadcaster tf_br;
tf::TransformListener listener;
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);
tf::Matrix3x3 m(q);
double roll, pitch, yaw;
m.getRPY(roll, pitch, yaw);
// this prints correct pose of the camera with respect to the alvar marker
std::cout<<req.markers[0].pose.pose;
// this prints x,y,z 0,0,0 . .not showing correct camera pose
// ROS_INFO("x, y, z=%1.2f %1.2f %1.2f", req.markers[0].pose.pose.position.x, req.markers[0].pose.pose.position.y, 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) );
// orientation of the camera works is correct
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{
listener.waitForTransform("/camera_link", "/world", ros::Time::now(), ros::Duration(1.0));
// this not setting correct camera pose translation but orientation works
tf_br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "camera_link"));
}
catch (tf::TransformException ex){
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
}
} // if
}
int main(int argc, char **argv) {
ros::init(argc, argv, "ar_listener");
ros::NodeHandle nh;
// ROS_INFO("INSIDE main ar_listener . . . . .");
ros::Subscriber sub = nh.subscribe("ar_pose_marker", 1, cb);
ros::spin();
return 0;
}
output of rosrun tf tf_echo /world /camera_link
At time 1472318991.936 - Translation: [0.000, 0.000, 0.000] - Rotation: in Quaternion [1.000, -0.000, -0.000, 0.000] in RPY (radian) [3.142, 0.000, -0.000] in RPY (degree) [180.000, 0.000, -0.000]
Please help me how to achieve what I wanted.
Thanks in advance . .
Do you alrd have the tf from world to the marker? What is the output when you run your node?