how to set dynamic tf between camera_link and world frame

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

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

edit retag flag offensive close merge delete

Comments

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

DavidN gravatar image DavidN  ( 2016-08-31 23:11:03 -0600 )edit