ROS_TF:Writing a tf broadcaster (C++)

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.
在接下来的两个教程中，我们将编写代码，将tf介绍部分的实例重现。在这之后，接下来的教程将重点放在使用更高级的tf特性来拓展演示实例。

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:
在我们开始之前，你需要在工程(project)中建立一个新的ros包。在sandbox文件夹中，建立一个名为learning_tf的包，该包依赖于 ft,roscpp,rospy,tuitlesim.

 $ 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

1.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.
该教程教你如何将坐标系广播给tf。在这种情况下，我们希望当乌龟移动时，广播它们不断变换的坐标系。
Let’s first create the source files. Go to the package we just created:

 $ roscd learning_tf

1.1 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.

#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;    //定义tf转换关系
  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));
  //将转换关系，即相对于父坐标系的位置发送给tf库
}

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;
};

1.2 The Code Explained

Now, let’s take a look at the code that is relevant to publishing the turtle pose to tf.

#include <tf/transform_broadcaster.h>

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.
tf包提供了TransformBroadcaster实现，以帮助简化发布转换任务。为了使用TransformBroadcaster，我们需要包含一个头文件。

static tf::TransformBroadcaster br;

Here, we create a TransformBroadcaster object that we’ll use later to send the transformations over the wire.
此处，我们创建了一个TransformBroadcaster对象，稍后我们将使用该对象通过线路发送变换。

  tf::Transform transform;
  transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) ); //设置三维坐标点坐标
  tf::Quaternion q;
  q.setRPY(0, 0, msg->theta);

Here we create a Transform object, and copy the information from the 2D turtle pose into the 3D transform.
此处，我们创建了一个变换对象，并且2维乌龟位置信息复制到三维变换中。

transform.setRotation(q);

Here we set the rotation.
这里我们设置旋转。

 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.
这里才是真正的工作所在。通过TransformBroadcaster发布一个变换需要四个变量

1.First, we pass in the transform itself.
2.Now we need to give the transform being published a timestamp, we’ll just stamp it with the current time, ros::Time::now().
3.Then, we need to pass the name of the parent frame of the link we’re creating, in this case “world”
4.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.
1.变换本身
2.我们需要给正在被发送的变换一个时间戳，我们只需要用当前时间戳他，ros::Time::now()
3.我们需要发送我们创建的体系的父坐标系，此状况下是 world。
4.最后，我们需要发送我们创建的体系的子坐标系，此状况下是乌龟本身。

Note: sendTransform and StampedTransform have opposite ordering of parent and child.
注意： sendTransform and StampedTransform 中父级与子级相反。

2.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:

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.

3 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++)