ROS1 TF 坐标系广播与监听编程实现

实现步骤

创建一程序订阅海龟相对于 world 的位置，即海龟发布的/turtle_name/pose 消息，接受到消息数据后，创建 tf 的广播器，并初始化 tf 数据，广播 world 与海龟坐标系之间的关系；

然后，创建一程序产生第 2 只海龟 turtle2，实时监听 tf 广播数据，依据 turtle2 与 turtle1 之间的坐标关系计算 turtle2 的速度，向 turtle2 发布速度控制指令，实现海龟 turtle2 跟踪 turtle1。

1. 创建功能包

创建的 learning_tf 包来进行代码存放和编译

cd ~/catkin_ws/src && catkin_create_pkg learning_tf roscpp rospy tf turtlesim

2. 创建代码并编译运行（C++）

如何实现一个 TF 广播器：

• 定义 TF 广播器（TransformBroadcaster）
• 创建坐标变换值
• 发布坐标变换（sendTransform）

在 learning_tf/src 新增 turtle_tf_broadcaster.cpp

源码内容如下：

/**
 * 该例程产生 tf 数据，并广播 world 与海龟的坐标关系
 */
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <turtlesim/Pose.h>

std::string turtle_name;

// 回调函数，传入参数为海龟的位置
void poseCallback(const turtlesim::PoseConstPtr& msg) {
  // 创建 tf 的广播器，将 turtle1 相对于 world ,turtle2 相对于 world 的坐标系发出去
  static tf::TransformBroadcaster br;
  // 初始化 tf 数据 填充坐标系之间的关系
  tf::Transform transform;
  transform.setOrigin(tf::Vector3(msg->x, msg->y, 0.0));
  tf::Quaternion q;
  // 设置四元数（旋转矩阵），传入 RPY 三个角的信息。这里只有 Z 方向有旋转关系
  q.setRPY(0, 0, msg->theta);
  transform.setRotation(q);
  // 广播 world 与海龟坐标系之间的 tf 数据
  br.sendTransform(tf::StampedTransform(transform, ros::Time::(), , turtle_name));
}

{
  
  ros::(argc, argv, );
  
   (argc != ) {
    ();
     ;
  }
  turtle_name = argv[];
  
  ros::NodeHandle node;
  ros::Subscriber sub = node.(turtle_name+, , &poseCallback);
  
  ros::();
   ;
}