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.

/* File_name: turtle_tf_broadcaster.cpp
 * Node_Name: my_tf_broadcaster
 * Pkg_Name:  learning_tf
 * For Tutorials: Writing a tf broadcaster (C++) - ROS wiki
 * For ROS Hydro
 * Author: Sonic, CIGIT of CAS
 * Date: Ang 11, 2015

 */

#include 
#include 
#include 

std::string turtle_name;

void poseCallback(const turtlesim::PoseConstPtr& msg){ //input: Post Pointer msg
    static tf::TransformBroadcaster br;
    //tf::StampedTransform stf;
    tf::Transform transform;
    tf::Vector3 v;
    tf::Quaternion q;

    v.setValue(msg->x, msg->y, 0.0);
    q.setRPY(0, 0, msg->theta);
    transform.setOrigin( v );
    transform.setRotation( q );
    tf::StampedTransform stf(transform, ros::Time::now(), "world", turtle_name);  //Declare stf
    br.sendTransform(stf); //send tf
    //br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}

int main(int argc, char** argv){
  ros::init(argc, argv, "my_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;
};

加入到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:

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代码的说明：

/*
		// #include for ros
		// #include for tf::tf broadcaster
		// #include for listen to turtlesim/Pose
	    这个tf的broadcaster就是把pose信息接收，再发布到tf 
		
		*Declare a std::string: turtle_name;  //const std::string &child_frame_id
		
		//Define CallBack function for Pose message: poseCallback(...)
			*Declare tf/TransformBroadcaster: br;
			
			*Declare tf/Transform: 	 transform;
			*Declare tf/Vector3: 	 v;
			*Declare tf/Quaternion:  q;
			
			v.setValue(msg->x, msg->y, 0.0);
			q.setRPY(0, 0, msg->theta); 
			transform.setOrigin(v);
			transform.setRotation(q);
			
			*Declare tf/StampedTransform: stf(transform, ros::Time::now(), "world", turtle_name);
			          //StampedTransform (const tf::Transform &input, const ros::Time ×tamp, const std::string &frame_id, const std::string &child_frame_id)
			
			br.sendTransform(stf);
			
		//**CallBack function Conclusion:**
		    //msg -> Vector+Quaternion -> transform;
			//transform+TimeStamp+Frame_id+ChildFrame_id -> StampedTransform (The most IMPT!)
			//StampedTransform -> broadcaster
		
		//Define the main function:
			ros::init();
			* Declare ros::NodeHandle node;

			check the argc ?= 2
			turtle_name = argv[1];
			* Declare ros::Subscriber sub = node.subscribe(topic, 10, &poseCallback);
			
			ros::spin();
			return 0;			
*/