ROS学习之tf.2.编写一个TF广播器（C++）

ROS.tf2.编写一个tf广播器（C++）

声明：本教程来自于ROS.WIKI，本人在整理过程中可能出现一些错误和问题，有问题可以去查看官网版本也可以咨询本人

1.1在接下来的两篇教程中，我们将编写代码来重现tf入门教程中的演示。之后，以下教程将重点介绍使用更高级的tf功能扩展演示。在我们开始之前，您需要为此项目创建一个新的ros包。在沙箱文件夹中，创建一个名为learning_tf的包，该包依赖于tf，roscpp，rospy和turtlesim：

 $ cd %YOUR_CATKIN_WORKSPACE_HOME%/src

 $ catkin_create_pkg learning_tf tf roscpp rospy turtlesim

1.2建立新的软件包，然后才能使用roscd：

$ cd %YOUR_CATKIN_WORKSPACE_HOME%/

$ catkin_make

$ source./devel/setup.bash

2.1如何广播转换
本教程将告诉您如何向tf广播坐标帧。在这种情况下，我们希望在移动时播放海龟变化的坐标系。
我们先创建源文件。转到我们刚刚创建的包：

转到src/文件夹并启动您的编辑器，将以下代码粘贴到名为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;
  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));
}
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;
}

2.2代码解释

现在，我们对代码的关键部分进行解释。

(1).#include
<tf/transform_broadcaster.h>
tf软件包提供了TransformBroadcaster的实现，以帮助简化发布转换的任务。要使用TransformBroadcaster，我们需要包含tf
/ transform_broadcaster.h头文件。

(2).static
tf::TransformBroadcaster br;
在这里，我们创建一个TransformBroadcaster对象，稍后我们将使用它来通过电线发送变换。

(3).
   tf::Transform transform;
   transform.setOrigin(tf::Vector3(msg->x, msg->y, 0.0) );
   tf::Quaternion q;
   q.setRPY(0,0, msg->theta);

在这里，我们创建一个Transform对象，并将2D海龟姿势的信息复制到3D变换中。

(4).transform.setRotation（q）;
这里我们设置旋转。

(5).br.sendTransform（tf:: StampedTransform（transform，ros:: Time :: now（），“world”，turtle_name））;

这是真正的工作完成的地方。使用TransformBroadcaster发送变换需要四个参数。

首先，我们通过转换本身。

现在我们需要给发布的变换一个时间戳，我们会用当前时间戳ros:: Time :: now（）

然后，我们需要传递我们创建的链接的父框架的名称，“世界”

最后，我们需要传递我们创建的链接的子框架的名称，这是乌龟本身的名称

注意：sendTransform和StampedTransform与父母和子女的顺序相反。

3.1运行广播公司
现在我们创建了代码，让我们先编译它。
打开CMakeLists.txt文件，并在底部添加以下行：

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

进入你的的工作包

$ catkin_make

如果一切顺利，您的devel/ lib /learning_tf文件夹中应该有一个名为turtle_tf_broadcaster的二进制文件。如果是这样，我们准备为这个演示创建一个启动文件。

进入learning_tf创建一个launch文件夹

$ mkdir launch

使用文本编辑器创建一个名为start_demo.launch的新文件

$ vim 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）。现在，你已经准备好开始你自己的海龟播放器演示：

 $ roslaunch learning_tf start_demo.launch

你应该看到一只乌龟。

检查结果

现在，使用tf_echo工具来检查乌龟姿势是否实际上正在广播到tf：

$ rosrun tf tf_echo /world /turtle1

这应该告诉你第一只乌龟的姿势。使用箭头键在乌龟周围驱动（确保您的终端窗口处于活动状态，而不是您的模拟器窗口）。如果你运行tf_echo来进行世界和海龟2之间的转换，你不应该看到转换，因为第二只海龟还没有。但是，只要我们在下一个教程中添加第二只乌龟，乌龟2的姿势就会广播给tf。
要实际使用广播到tf的转换，您应该转到下一个关于创建tf侦听器的教程（C+