TF学习笔记2（建立一个发布者）

在接下来的两个教程中，我们将编写代码以重现tf介绍教程的Demo。之后，以下教程将重点放在使用更高级的tf功能扩展演示。

在开始之前，您需要为此项目创建一个新的ros包。

创建一个名为learning_tf的包，它依赖于tf，roscpp，rospy和turtlesim：

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

编译：

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

1. 如何发布坐标变换

本教程将教您如何将坐标发布到tf。

我们希望发布海龟四处移动时的变化坐标。

我们先创建源文件。转到我们刚刚创建的包：

 $ roscd learning_tf

1.1 代码

转到src /文件夹，并启动您喜欢的编辑器，将以下代码粘贴到名为src / turtle_tf_broadcaster.cpp的新文件中。

https://raw.github.com/ros/geometry_tutorials/hydro-devel/turtle_tf/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;
};

1.2 程序解释

现在，让我们来看看与将turtle pose发布到tf相关的程序。

#include <tf/transform_broadcaster.h>

tf包提供了TransformBroadcaster，以帮助简化发布转换的任务。

要使用TransformBroadcaster，我们需要包含tf / transform_broadcaster.h头文件。

static tf::TransformBroadcaster br;

我们创建一个TransformBroadcaster对象，稍后我们将使用该对象发送转换。

补充下接收到信息的类型：

setOrigin应为平移坐标；setRPY应为欧拉角；Quaternion应为四元数；

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变换中。

transform.setRotation(q);

在这里我们设置旋转。

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

使用TransformBroadcaster发送变换需要四个参数。

首先，我们传递变换本身。
现在我们需要为变换发布一个时间戳，我们只用当前时间标记它，ros :: Time :: now（）。
然后，我们需要传递我们正在创建链接的父坐标系的名称，在本例中为“world”
最后，我们需要传递我们正在创建链接的子坐标系的名称，在这种情况下，这是乌龟本身的名称。

注意：sendTransform和StampedTransform have opposite ordering of parent and child.

2. 运行发布者

现在我们创建了代码，让我们先编译它。打开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）。现在，您已准备好开始自己的海龟广播演示：

 $ roslaunch learning_tf start_demo.launch

3. 检查结果

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

 $ rosrun tf tf_echo / world / turtle1

这应该会告诉你第一只乌龟的姿势。

使用箭头键驱动乌龟（确保终端窗口处于活动状态，而不是模拟器窗口）。

如果你为世界和乌龟2之间的转换运行tf_echo，你不应该看到变换，因为第二只乌龟还没有。

但是，只要我们在下一个教程中添加第二只乌龟，乌龟2的姿势就会被广播到tf。

要实际使用转换广播到tf，您应该继续学习有关创建tf监听器的下一个教程（Python）（C ++）