Note: This tutorial assumes you have a working knowledge of compiling programs in ROS and you have completed the Introduction to tf tutorial. |
Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags. |
Writing a tf broadcaster (C++)
Description: This tutorial teaches you how to broadcast coordinate frames of a robot to tf.Tutorial Level: BEGINNER
Next Tutorial: Writing a tf listener (C++)
tf is deprecated in favor of tf2. tf2 provides a superset of the functionality of tf and is actually now the implementation under the hood. If you're just learning now it's strongly recommended to use the tf2/Tutorials instead.
Contents
In the next two tutorials we will write the code to reproduce the demo from the tf introduction tutorial. After that, the following tutorials focus on extending the demo with more advanced tf features.
Before we get started, you need to create a new ros package for this project. In the sandbox folder, create a package called learning_tf that depends on tf, roscpp, rospy and turtlesim:
$ cd %YOUR_CATKIN_WORKSPACE_HOME%/src $ catkin_create_pkg learning_tf tf roscpp rospy turtlesim
Build your new package before you can roscd:
$ cd %YOUR_CATKIN_WORKSPACE_HOME%/ $ catkin_make $ source ./devel/setup.bash
$ roscd tutorials $ roscreate-pkg learning_tf tf roscpp rospy turtlesim $ rosmake learning_tf
How to broadcast transforms
This tutorial teaches you how to broadcast coordinate frames to tf. In this case, we want to broadcast the changing coordinate frames of the turtles, as they move around.
Let's first create the source files. Go to the package we just created:
$ roscd learning_tf
The Code
Go to src/ folder and fire up your favorite editor to paste the following code into a new file called src/turtle_tf_broadcaster.cpp.
https://raw.github.com/ros/geometry_tutorials/hydro-devel/turtle_tf/src/turtle_tf_broadcaster.cpp
1 #include <ros/ros.h>
2 #include <tf/transform_broadcaster.h>
3 #include <turtlesim/Pose.h>
4
5 std::string turtle_name;
6
7
8
9 void poseCallback(const turtlesim::PoseConstPtr& msg){
10 static tf::TransformBroadcaster br;
11 tf::Transform transform;
12 transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );
13 tf::Quaternion q;
14 q.setRPY(0, 0, msg->theta);
15 transform.setRotation(q);
16 br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
17 }
18
19 int main(int argc, char** argv){
20 ros::init(argc, argv, "my_tf_broadcaster");
21 if (argc != 2){ROS_ERROR("need turtle name as argument"); return -1;};
22 turtle_name = argv[1];
23
24 ros::NodeHandle node;
25 ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);
26
27 ros::spin();
28 return 0;
29 };
The Code Explained
Now, let's take a look at the code that is relevant to publishing the turtle pose to tf.
The tf package provides an implementation of a TransformBroadcaster to help make the task of publishing transforms easier. To use the TransformBroadcaster, we need to include the tf/transform_broadcaster.h header file.
10 static tf::TransformBroadcaster br;
Here, we create a TransformBroadcaster object that we'll use later to send the transformations over the wire.
Here we create a Transform object, and copy the information from the 2D turtle pose into the 3D transform.
15 transform.setRotation(q);
Here we set the rotation.
16 br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
This is where the real work is done. Sending a transform with a TransformBroadcaster requires four arguments.
- First, we pass in the transform itself.
Now we need to give the transform being published a timestamp, we'll just stamp it with the current time, ros::Time::now().
Then, we need to pass the name of the parent frame of the link we're creating, in this case "world"
- Finally, we need to pass the name of the child frame of the link we're creating, in this case this is the name of the turtle itself.
Note: sendTransform and StampedTransform have opposite ordering of parent and child.
Running the broadcaster
Now that we created the code, lets compile it first. Open the CMakeLists.txt file, and add the following line at the bottom:
rosbuild_add_executable(turtle_tf_broadcaster src/turtle_tf_broadcaster.cpp)
Build your package:
$ make
add_executable(turtle_tf_broadcaster src/turtle_tf_broadcaster.cpp) target_link_libraries(turtle_tf_broadcaster ${catkin_LIBRARIES})
Build your package; at the top folder of your catkin workspace:
$ catkin_make
If everything went well, you should have a binary file called turtle_tf_broadcaster in your devel/lib/learning_tf folder.
If so, we're ready to create a launch file for this demo. With your text editor, create a new file called start_demo.launch, and add the following lines:
<launch> <!-- Turtlesim Node--> <node pkg="turtlesim" type="turtlesim_node" name="sim"/> <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/> <!-- Axes --> <param name="scale_linear" value="2" type="double"/> <param name="scale_angular" value="2" type="double"/> <node pkg="learning_tf" type="turtle_tf_broadcaster" args="/turtle1" name="turtle1_tf_broadcaster" /> <node pkg="learning_tf" type="turtle_tf_broadcaster" args="/turtle2" name="turtle2_tf_broadcaster" /> </launch>
First, make sure you stopped the launch file from the previous tutorial (use ctrl-c). Now, you're ready to start your own turtle broadcaster demo:
$ roslaunch learning_tf start_demo.launch
You should see the turtle sim with one turtle.
Checking the results
Now, use the tf_echo tool to check if the turtle pose is actually getting broadcast to tf:
$ rosrun tf tf_echo /world /turtle1
This should show you the pose of the first turtle. Drive around the turtle using the arrow keys (make sure your terminal window is active, not your simulator window). If you run tf_echo for the transform between the world and turtle 2, you should not see a transform, because the second turtle is not there yet. However, as soon as we add the second turtle in the next tutorial, the pose of turtle 2 will be broadcast to tf.
12 transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) ); 13 tf::Quaternion q; 14 q.setRPY(msg->theta, 0, 0);
To actually use the transforms broadcast to tf, you should move on to the next tutorial about creating a tf listener (Python) (C++)
Feedback
If you have any comments on this tutorial please comment below:
kulnet-nat-2 | 3.0 | Very useful! only a thing: when I type "make", che compiler told me that there's a warning in transform.setRotation(tf::Quaternion(msg->theta, 0, 0)); turtle_tf_broadcaster.cpp:31: warning: ‘btQuaternion::btQuaternion(const btScalar&, const btScalar&, const btScalar&)’ is deprecated |
2010-02-10 15:41:35 (178 months ago) | ||
213 | 3.0 | good to understand and easy to follow |
2010-12-21 14:21:58 (167 months ago) | ||
193 | 3.0 | I miss a tool to inspect the frames available in the same way that 'rostopic list' shows the topic. I know 'rosrun tf view_frames' does this, but it is not very practical. Indeed, I'm using 'rosrun tf tf_monitor', but it generates a large output. I'd like to do something like 'rosrun tf tf_list' and see the frames available. They would appear once they are detected, so the list would be growing and with Ctrl+C we would stop the program. These would be the main difference wrt to 'rostopic list' behaviour. PS: You may use 'rosmake' instead of 'make' |
2011-03-04 11:03:15 (165 months ago) | ||
tiopsiv | 4.0 | XiXWqM <a href="http://rcwezvnfezwp.com/">rcwezvnfezwp</a>, [url=http://mhligxdiuurs.com/]mhligxdiuurs[/url], [link=http://obubvmbjqeyq.com/]obubvmbjqeyq[/link], http://wwzdeaekrcus.com/ |
2011-08-25 22:52:46 (159 months ago) | ||