二 tf / Tutorials / Writing a tf broadcaster (C++)_sendtransform和stampedtransform具有父对象和子对象的相反顺序-CSDN博客

原文地址：http://wiki.ros.org/tf/Tutorials/Writing a tf broadcaster (C%2B%2B)

Writing a tf broadcaster (C++)

描述：本教程教您如何将机器人的坐标系广播到tf。
在接下来的两个教程中，我们将编写代码来从tf介绍教程中再现演示。之后，下面的教程将重点介绍如何用更高级的tf特性扩展演示。
在我们开始之前，您需要为这个项目创建一个新的ros包。在sandbox folder中，创建一个名为learning_tf的包，该包依赖于tf、roscpp、rospy和turtlesim：

 $ cd %YOUR_CATKIN_WORKSPACE_HOME%/src
 $ catkin_create_pkg learning_tf tf roscpp rospy turtlesim

在roscd之前构建您的新包：

 $ cd %YOUR_CATKIN_WORKSPACE_HOME%/
 $ catkin_make
 $ source ./devel/setup.bash

1 How to broadcast transforms

本教程教您如何向tf广播坐标帧。在这种情况下，我们希望广播turtle在移动时坐标系的变化。
让我们首先创建源文件。转到我们刚刚创建的包：

 $ roscd learning_tf

1.1 The Code

转到src/folder，启动您最喜欢的编辑器，将下面的代码粘贴到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 };

1.2 The Code Explained

现在，让我们看一下与向tf发布海龟姿态相关的代码。



   2 #include <tf/transform_broadcaster.h>
   3

tf包提供了TransformBroadcaster的实现，以帮助简化发布transforms的任务。要使用TransformBroadcaster，我们需要包括tf/transform_broadcaster.h头文件。

 10   static tf::TransformBroadcaster br;

这里，我们创建一个TransformBroadcaster对象，稍后我们将使用它通过连线发送transforms。

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

在这里，我们创建一个Transform object，并将信息从2D turtle pose复制到3D transform。

  15   transform.setRotation(q);

这里我们设置了旋转。

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

这就是真正工作的地方。使用TransformBroadcaster发送transform需要四个arguments。
(1)首先，我们传递transform本身。
(2)现在我们需要为发布的transform提供一个时间戳，我们只需要用当前时间ros::Time::.()对其进行标记。
(3)然后，我们需要传递正在创建的链接的父坐标系的名称，在本例中为“world”。
(4)最后，我们需要传递我们正在创建的链接的子坐标系的名称，在这种情况下，这是turtle本身的名称。
注意：sendTransform和StampedTransform的父和子顺序相反。

2 Running the broadcaster

现在我们已经创建了代码，让我们首先编译它。打开CMakeLists.txt文件，并在底部添加以下行：

add_executable(turtle_tf_broadcaster src/turtle_tf_broadcaster.cpp)
target_link_libraries(turtle_tf_broadcaster ${catkin_LIBRARIES})

构建您的包；在catkin工作空间的顶部文件夹中：

 $ catkin_make

如果一切顺利，那么在devel/lib/learning_tf文件夹中应该有一个名为turtle_tf_broadcaster的二进制文件。
如果是这样，我们准备为这个演示创建一个启动文件。使用文本编辑器，创建一个名为start_demo.launch的新文件，并添加以下行：

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

首先，确保您停止了前一个教程中的启动文件（使用ctrl-c）。现在，您已经准备好开始您自己的turtle广播演示：

 $ roslaunch learning_tf start_demo.launch

你应该看到turtlesim和一只turtle拼在一起。

3 Checking the results

现在，使用tf_echo工具来检查乌龟的姿态是否真的被广播到tf:

 $ rosrun tf tf_echo /world /turtle1

这应该能告诉你第一只turtle的姿态。使用箭头键驱动turtle（确保你的终端窗口是活动的，而不是你的模拟器窗口）。如果运行tf_echo来转换world和turtle 2，则不应该看到转换，因为第二只turtle还没有出现。然而，一旦我们在下一教程中添加了第二只乌龟，乌龟2的姿态将被广播到tf。
12 setOrigin(tf::Vector3(msg->x,msg->y,0.0);
13 tf::Quaternion q;
14 q.setRPY(msg->θ,0,0);
要实际使用转换广播到TF，您应该转到下一个关于创建TF侦听器（Python）（C++）的教程。