Writing a tf listener

In the previous subsection, we created a TF broadcaster to publish the pose of a turtle to TF. In this subsection, we will create a TF listener that will use that TF.

We will create a tf_tutorials/src/ turtle_tf_listener.cpp file and add the following source code to it:

#include <ros/ros.h> 
#include <tf/transform_listener.h> 
#include <geometry_msgs/Twist.h> 
#include <turtlesim/Spawn.h> 
 
int main(int argc, char** argv){ 
  ros::init(argc, argv, "tf_listener"); 
 
  ros::NodeHandle node; 
 
  ros::service::waitForService("spawn"); 
  ros::ServiceClient add_turtle = 
    node.serviceClient<turtlesim::Spawn>("spawn"); 
  turtlesim::Spawn srv; 
  add_turtle.call(srv); 
 
  ros::Publisher turtle_vel = 
    node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10); 
 
  tf::TransformListener listener; 
 
  ros::Rate rate(10.0); 
  while (node.ok()){ 
    tf::StampedTransform transform; 
    try{ 
      listener.lookupTransform("/turtle2", "/turtle1", 
                               ros::Time(0), transform); 
    } 
    catch (tf::TransformException &ex) { 
      ROS_ERROR("%s",ex.what()); 
      ros::Duration(1.0).sleep(); 
      continue; 
    } 
 
    geometry_msgs::Twist vel_msg; 
    vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(), 
                                    transform.getOrigin().x()); 
    vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) + 
                                  pow(transform.getOrigin().y(), 2)); 
    turtle_vel.publish(vel_msg); 
 
    rate.sleep(); 
  } 
  return 0; 
}; 

Here, we will create a TransformListener object which will start receiving TF transformations over the ROS communication network, and will buffer them for up to 10 seconds by default:

try{ 
      listener.lookupTransform("/turtle2", "/turtle1", 
                               ros::Time(0), transform); 
    } 

In the preceding line of source code, we queried the listener for a specific transformation that will take four arguments:

  • In the first two arguments, we will transform from the /turtle1 frame to the /turtle2 frame.
  • In the next argument, the time at which the transformation is required, providing ros::Time(0) will set up the latest available transform.
  • In the final argument, we will use the object to store the resulting transformation.
..................Content has been hidden....................

You can't read the all page of ebook, please click here login for view all page.
Reset
3.17.68.14