Writing a TF broadcaster

First, we will create a ros package in our workspace or copy the chapter3_tutorials/tf_tutorials file from GitHub:

$ catkin_create_pkg tf_tutorials tf roscpp rospy turtlesim 

Next, we will create the tf_tutorials/src/turtle_tf_broadcaster.cpp file and add the source code:

#include <ros/ros.h> 
#include <tf/transform_broadcaster.h> 
#include <turtlesim/Pose.h> 
 
std::string turtle_name; 
 
 
void poseCallback(const turtlesim::PoseConstPtr& msg){ 
  static tf::TransformBroadcaster br; 
  tf::Transform transform; 
  transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) ); 
  tf::Quaternion q; 
  q.setRPY(0, 0, msg->theta); 
  transform.setRotation(q); 
  br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name)); 
} 
 
int main(int argc, char** argv){ 
  ros::init(argc, argv, "tf_broadcaster"); 
  if (argc != 2){ROS_ERROR("need turtle name as argument"); return -1;}; 
  turtle_name = argv[1]; 
 
  ros::NodeHandle node; 
  ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback); 
 
  ros::spin(); 
  return 0; 
}; 

Here, we will create a TransformBroadcaster object that will be used to send the transformations over the ROS communication network. And, we will also create a Transform object and copy the information from the 2D turtle pose into the 3D transformation:

br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name)); 

In the preceding code snippet, the real work is done where Transform with TransformBroadcaster requires four arguments:

  • First, we will pass in transform itself, which was created in the preceding code snippet.
  • Next, we provide a timestamp to the transform being published. We'll just stamp it with the current time, ros::Time::now().
  • Then, we will provide the name of the parent frame of the link we have created, in this case, "world".
  • Finally, we will provide the name of the child frame of the link we have created, in this case, the name of the turtle itself.
sendTransform and StampedTransform have opposite ordering of parent and child.
..................Content has been hidden....................

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