Writing a tf broadcaster (C++)

生成软件包learning_tf

<pre name="code" class="html">catkin_create_pkg learning_tf tf roscpp rospy turtlesim

新建turtle_tf_broadcaster.cp文件，并且进行编译。

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

代码解释

#include <tf/transform_broadcaster.h>

tf包提供了一个实现机制TransformBroadcaster，来帮助使得发布变换更容易，为了使用TransformBroadcaster，我们需要包含这个tf/transform_broadcaster.h头文件

 static tf::TransformBroadcaster br;

我们生成了一个 TransformBroadcaster对象，用来发送坐标转换。

  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位置，添加了一个z系，但是设为0.0,即在rviz中也是在平面。

 transform.setRotation(q);

通过transform设置方位

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

/*定义了一个名称为br的TransformBroadcaster，即上面所提到的，是tf包中的，通过TransformBroadcaster来发布信息需要四点:
1.首先需要transform
2.需要给transform一个时间戳，这个时间戳是现在的时间
3.然后，我们需要将我们创建的link的父框架的名字传输过去，在这个例子中是world
4.最后，我们需要将我们创建的link的子框架的名字传输过去，在这里就是turtle本身
*/

 ros::init(argc, argv, "my_tf_broadcaster");

初始化ros,命名节点为my_tf_broadcaster

 turtle_name = argv[1];

乌龟的名字可以输入

 ros::NodeHandle node;

NodeHandle是与ROS系统交流的最主要的接入点，是一个句柄

 ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);

从master订阅某乌龟的“/pose”话题，当消息到来时，即当乌龟位置改变时产生新的消息时，ROS将会调用poseCallback

Running the broadcaster

打开 CMakeLists.txt，加入以下文本

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

编译。
新建 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>

运行launch文件，

roslaunch learning_tf start_demo.launch

从launch文件中启动了四个节点，分别是turtlesim包中的类型为turtlesim_node的节点sim，turtlesim中类型为turtle_teleop_key的节点teleop,还有刚才建的包learning_tf中类型为turtle_tf_broadcaster的两个节点turtle1_tf_broadcaster和turtle2_tf_broadcaster.

.当运行.launch文件后生成了一个乌龟turtle1,当利用键盘控制使乌龟移动时，乌龟的位置发生变化，并通过主题turtle1/pose来发布位置改变的消息，由于节点turtle1_tf_broadcaster订阅了这个主题，当这个主题有消息发布时（比如乌龟位置变化），则会调用poseCallback，正如上面分析所示，这个方法将会将消息反馈回来，反馈的消息将2D改为3D，反馈为Translation和Rotation,其中Rotation包括Quaternion和RPY. 这样，当乌龟移动时能够反馈位置信息并能够在终端看到，主要利用了订阅pose主题，并利用TransformBroadcaster来发布转换消息。

Checking the results

 rosrun tf tf_echo /world /turtle1

可以查看位移于旋转角度的变化。