ROS进阶学习手记 9 – 关于坐标系统 tf

ROS tf 教程： 写一个tf 发布器

完成了tf的初步认识以后，我们将按照以下的大纲进行ROS_tf的学习：
ref: http://wiki.ros.org/tf/Tutorials重要的一个参考是  古月居的博客： http://blog.csdn.net/hcx25909/article/details/9255001      

1. 有关tf 的学习大纲

1. Writing a tf broadcaster (C++) This tutorial teaches you how to broadcast coordinate frames of a robot to tf.
2. Writing a tf listener (C++) This tutorial teaches you how to use tf to get access to frame transformations.
3. Adding a frame (C++) This tutorial teaches you how to add an extra fixed frame to tf.
4. Learning about tf and time (C++) This tutorial teaches you to use the waitForTransform function to wait for a transform to be available on the tf tree.
5. Time travel with tf (C++) This tutorial teaches you about advanced time travel features of tf

2. 开始本节

    这一节，我们要用C++写一个 tf broadcaster，先创建Package：

Before we get started, you need to create a new ros package for this project. In the sandbox folder, create a package calledlearning_tf that depends on tf,roscpp,rospy andturtlesim:

 $ 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

Let’s first create the source files. Go to the package we just created:

 $ roscd learning_tf

3. 写代码The Code

Go to src/ folder and fire up your favorite editor to paste the following code into a new file calledsrc/turtle_tf_broadcaster.cpp.

1. /* File_name: turtle_tf_broadcaster.cpp
2. * Node_Name: my_tf_broadcaster
3. * Pkg_Name:  learning_tf
4. * For Tutorials: Writing a tf broadcaster (C++) - ROS wiki
5. * For ROS Hydro
6. * Author: Sonic, CIGIT of CAS
7. * Date: Ang 11, 2015
8. 
9. */
10. 
11. #include <ros/ros.h>
12. #include <tf/transform_broadcaster.h>
13. #include <turtlesim/Pose.h>
14. 
15. std::string turtle_name;
16. 
17. void poseCallback(const turtlesim::PoseConstPtr& msg){ //input: Post Pointer msg
18. static tf::TransformBroadcaster br;
19. //tf::StampedTransform stf;
20. tf::Transform transform;
21. tf::Vector3 v;
22. tf::Quaternion q;
23. 
24. v.setValue(msg->x, msg->y, 0.0);
25. q.setRPY(0, 0, msg->theta);
26. transform.setOrigin( v );
27. transform.setRotation( q );
28. tf::StampedTransform stf(transform, ros::Time::now(), "world", turtle_name);  //Declare stf
29. br.sendTransform(stf); //send tf
30. //br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
31. }
32. 
33. int main(int argc, char** argv){
34. ros::init(argc, argv, "my_tf_broadcaster");
35. if (argc != 2){ROS_ERROR("need turtle name as argument"); return -1;};
36. turtle_name = argv[1];
37. 
38. ros::NodeHandle node;
39. ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);
40. 
41. ros::spin();
42. return 0;
43. };
44. 
![](https://csdnimg.cn/release/blogv2/dist/pc/img/newCodeMoreWhite.png)

加入到Eclipse编辑：

 Generate the Eclipse Files for dev. in Eclipse IDE

    $ cd ~/catkin_ws    //这实际上是Workspace的路径，catkin_ws = catkin workspace
    $ catkin_make --force-cmake -G"Eclipse CDT4 - Unix Makefiles"
    $ . ~/catkin_ws/devel/setup.bash

to generate the .project file and then run:

    $ awk -f $(rospack find mk)/eclipse.awk build/.project > build/.project_with_env && mv build/.project_with_env build/.project

4. 准备运行这个broadcaster

Now that we created the code, lets compile it first. Open the CMakeLists.txt file, and add the following line at the bottom:

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 at the path:

    ~/catkin_ws/src/learning_tf/launch

and add the following lines:

1. <launch>
2. <!-- Turtlesim Node-->
3. <node pkg="turtlesim" type="turtlesim_node" name="sim"/>
4. 
5. <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>
6. <!-- Axes -->
7. <param name="scale_linear" value="2" type="double"/>
8. <param name="scale_angular" value="2" type="double"/>
9. 
10. <node pkg="learning_tf" type="turtle_tf_broadcaster"
11. args="/turtle1" name="turtle1_tf_broadcaster" />
12. <node pkg="learning_tf" type="turtle_tf_broadcaster"
13. args="/turtle2" name="turtle2_tf_broadcaster" />
14. 
15. </launch>

5. Start your own tf broadcaster

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.

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

$ rosrun turtlesim turtle_teleop_key

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.To actually use the transforms broadcast to tf, you should move on to the next tutorial about creating a tf listener.

7. 关于这个broadcaster代码的说明：

1. /*
2. // #include for ros
3. // #include for tf::tf broadcaster
4. // #include for listen to turtlesim/Pose
5. 这个tf的broadcaster就是把pose信息接收，再发布到tf
6. 
7. *Declare a std::string: turtle_name;  //const std::string &child_frame_id
8. 
9. //Define CallBack function for Pose message: poseCallback(...)
10. *Declare tf/TransformBroadcaster: br;
11. 
12. *Declare tf/Transform: 	 transform;
13. *Declare tf/Vector3: 	 v;
14. *Declare tf/Quaternion:  q;
15. 
16. v.setValue(msg->x, msg->y, 0.0);
17. q.setRPY(0, 0, msg->theta);
18. transform.setOrigin(v);
19. transform.setRotation(q);
20. 
21. *Declare tf/StampedTransform: stf(transform, ros::Time::now(), "world", turtle_name);
22. //StampedTransform (const tf::Transform &input, const ros::Time ×tamp, const std::string &frame_id, const std::string &child_frame_id)
23. 
24. br.sendTransform(stf);
25. 
26. //**CallBack function Conclusion:**
27. //msg -> Vector+Quaternion -> transform;
28. //transform+TimeStamp+Frame_id+ChildFrame_id -> StampedTransform (The most IMPT!)
29. //StampedTransform -> broadcaster
30. 
31. //Define the main function:
32. ros::init();
33. * Declare ros::NodeHandle node;
34. 
35. check the argc ?= 2
36. turtle_name = argv[1];
37. * Declare ros::Subscriber sub = node.subscribe(topic, 10, &poseCallback);
38. 
39. ros::spin();
40. return 0;
41. */
![](https://csdnimg.cn/release/blogv2/dist/pc/img/newCodeMoreWhite.png)