Writing a tf broadcaster (C++)

生成软件包learning_tf

catkin_create_pkg learning_tf tf roscpp rospy turtlesim
新建turtle_tf_broadcaster.cp文件，并且进行编译。 
  
 
  
#include 
#include 
#include 

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包提供了一个实现机制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文件， 
  
 
  
 
  
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
 
  
 可以查看位移于旋转角度的变化。