how to set dynamic tf between camera_link and world frame

asked 2016-08-27 12:34:43 -0500

Mahe gravatar image

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_listener.h>

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

   // 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));

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

  } // 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);
  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 . .

edit retag flag offensive close merge delete


Do you alrd have the tf from world to the marker? What is the output when you run your node?

DavidN gravatar imageDavidN ( 2016-08-31 23:11:03 -0500 )edit