Robotics StackExchange | Archived questions

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 tfecho /world /cameralink

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

Asked by Mahe on 2016-08-27 12:33:23 UTC

Comments

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

Asked by DavidN on 2016-08-31 23:11:03 UTC

Answers