tf官方教程——小海龟跟随程序原理解读

小海龟跟随程序的功能包下载链接：turtle_tf2.zip - 蓝奏云

小海龟跟随程序主要通过监听tf变换和广播tf变换实现

1：广播tf变换：接收并缓存系统中发布的所有坐标变换数据（订阅两个海龟位姿话题——海龟坐标系与世界坐标系之间变换关系，最后将这个变换关系广播出去。）

2：监听 tf变换：接受广播的数据（每只海龟坐标系与世界坐标系之间变换关系），计算两只海龟坐标系之间的变换关系，就可以得到两只海龟之间的距离和角度，最终由此计算出海龟2的线速度和角速度并控制其移动，实现小海龟跟踪。

总结：海龟位姿话题的坐标是基于world世界坐标系的（话题topic是包含特定信息的载体）；而我们要实现海龟2对海龟1的跟踪，所需要的信息就是两个海龟坐标系frame之间转换，进而知道海龟2坐标系的原点在海龟1坐标系下面坐标，通过监听tf变换即可得到。

turtle_tf2_demo_cpp.launch代码解析（启动launch文件）

<launch>

  <!-- Turtlesim Node-->
  <node pkg="turtlesim" type="turtlesim_node" name="sim"/>  
  <!-- 打开了turtlesim里面的turtlesim_node,即打开海龟仿真器那个界面  -->

  <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/> 
  <!-- 打开控制海龟的键盘控制节点  -->

  <param name="scale_linear" value="2" type="double"/>   
  <param name="scale_angular" value="2" type="double"/>  

  <!-- 打开两个turtle_tf的广播器，用一个代码实现了两个海龟的tf坐标广播-->
  <node name="turtle1_tf2_broadcaster" pkg="turtle_tf2" type="turtle_tf2_broadcaster" respawn="false" output="screen" >
    <param name="turtle" type="string" value="turtle1" />
  </node>
  <node name="turtle2_tf2_broadcaster" pkg="turtle_tf2" type="turtle_tf2_broadcaster" respawn="false" output="screen" >
    <param name="turtle" type="string" value="turtle2" />
  </node>

  <!-- 打开一个turtle1_tf的监听器 -->
  <node name="turtle_pointer" pkg="turtle_tf2" type="turtle_tf2_listener" respawn="false" output="screen" >
  </node>

</launch>

turtle_tf2_broadcaster.cpp 代码解析（广播tf2变换）

#include <ros/ros.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/TransformStamped.h>
#include <turtlesim/Pose.h>

std::string turtle_name;

void poseCallback(const turtlesim::PoseConstPtr& msg) 
//回调函数输入参数为海龟位姿数据类型turtlesim::PoseConstPtr的变量
{
  static tf2_ros::TransformBroadcaster br;
  //实例化一个tf2_ros::TransformBroadcaster的br对象，将坐标变换通过话题发布出去 

  //用geometry_msgs::TransformStamped来实例化一个transformStamped结构体，
  //用这个结构体来装等会要发布出去的两个TF坐标关系的一些内容
  geometry_msgs::TransformStamped transformStamped;    
  
  transformStamped.header.stamp = ros::Time::now();    // 时间戳
  transformStamped.header.frame_id = "world";   //父坐标
  transformStamped.child_frame_id = turtle_name;   //子坐标

  // 调用transform的函数setOrigin()设置原点，tf::Vector3(msg->x, msg->y, 0.0)代表的含义是在子坐标系turtle_name下，父坐标系world原点的坐标是（msg->x, msg->y）
  transformStamped.transform.translation.x = msg->x;  
  transformStamped.transform.translation.y = msg->y;
  transformStamped.transform.translation.z = 0.0;

  //通过Quaternion这个类里面的成员函数setRPY（），将输入的欧拉角转为四元数
  //因为这个geometry_msgs::TransformStamped消息数据类型里面的姿态是以四元数的形式存在
  // 1.绕物体的z轴旋转，得到偏航角-yaw；
  // 2.绕旋转之后的y轴旋转，得到俯仰角-pitch；
  // 3.绕旋转之后的x轴旋转，得到滚转角-roll；
  // 4.setRPY()函数的参数roll(绕x轴），pitch(绕y轴），yaw(绕z轴）
  // 话题发布的海龟坐标为二维坐标加角度（相当于偏航角）
  tf2::Quaternion q;
     q.setRPY(0, 0, msg->theta);  
    

  transformStamped.transform.rotation.x = q.x();   //pose关系
  transformStamped.transform.rotation.y = q.y();
  transformStamped.transform.rotation.z = q.z();
  transformStamped.transform.rotation.w = q.w();

  //最后通过tf2_ros::TransformBroadcaster这个类里面的sendTransform()函数将两个TF坐标之间的变换关系（transformStamped结构体）给发送出去
  br.sendTransform(transformStamped);   
}

  //argc表示传入main()函数的参数个数，argv表示传入main函数的参数序列或者指针。并且argv的第一个输入参数一定是程序的名称。
  //所以说无论有无输入参数，这个argv都会输入一个程序名称
int main(int argc, char** argv){
  ros::init(argc, argv, "my_tf2_broadcaster");   //初始化ros节点，创建名字为"my_tf_broadcaster"节点

  ros::NodeHandle private_node("~");   //初始化ros节点句柄

  //通过两种方法给turtle_name赋值，参数服务器存在就直接参数服务器赋值，否则通过函数输入参数
  if (! private_node.hasParam("turtle"))   //查询参数服务器里面是否有turtle这个参数
    {
      //如果不存在就进入这个判断，如果argc不等于2（除了程序名字外是否还会仅有一个额外的参数） 
      就会输出这个报错的信息：需要海龟的名字作为输入参数，并且会退出
      if (argc != 2)
        {ROS_ERROR("need turtle name as argument"); return -1;};  
      turtle_name = argv[1];   //argv[0]里面是程序的名称，argv[1]里面是名字
    }
  else
    {
      private_node.getParam("turtle", turtle_name);   
      //参数服务器有的话直接读取并且将"turtle"这个值给到turtle_name这个变量 
    }
    
  ros::NodeHandle node;   //创建了第二个节点句柄

  //订阅了话题，订阅的话题名字是一个变量——turtle_name+"/pose"
  // 节点句柄调用subscribe设置接收的话题名字"turtle_name/pose"这里做了一个字符串拼接
  // 第一个参数指定订阅话题topic；第二个参数设置消息缓冲区的大小；第三个参数指定回调函数。 
  ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);   

  ros::spin();   
  //ros::spin()等待函数，程序一直卡在这里等待订阅话题的消息进来，不会在向下执行return0。
  // 当用户输入Ctrl+C或者ROS主进程关闭时退出，
  return 0;
};

 turtle_tf2_listener.cpp（监听tf变换）

#include <ros/ros.h>
#include <tf2_ros/transform_listener.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/Twist.h>
#include <turtlesim/Spawn.h>

int main(int argc, char** argv){
  ros::init(argc, argv, "my_tf2_listener");   //初始化ros节点
  ros::NodeHandle node;   //初始化ros句柄

  //通过服务的形式去生成海龟2，因为默认打开海龟仿真器只有一只海龟
  ros::service::waitForService("spawn");
  ros::ServiceClient spawner = 
    node.serviceClient<turtlesim::Spawn>("spawn");
  turtlesim::Spawn turtle;
  turtle.request.x = 4;
  turtle.request.y = 2;
  turtle.request.theta = 0;
  turtle.request.name = "turtle2";
  spawner.call(turtle);

  ros::Publisher turtle_vel =
    node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);    
    //发布一个话题，话题名字是海龟2的一个速度话题"turtle2/cmd_vel"。

  tf2_ros::Buffer tfBuffer;   //用tf2_ros::Buffer这个类来实例一个对象tfBuffer，因为监听tf2的消息，大部分工作都是通过tf2_ros::Buffer这个类成员函数来完成的
  tf2_ros::TransformListener tfListener(tfBuffer);   //然后在用tf2_ros::TransformListener这个类对这个tfBuffer进行构造和初始化（定义监听器）

  ros::Rate rate(10.0);   //用ros::Rate来控制海龟2话题的发布速度（10hz）
  while (node.ok()){
    geometry_msgs::TransformStamped transformStamped;   //实例化了一个消息数据类型的结构体，用来装载两个tf坐标之间的变换关系
    try{
      transformStamped = tfBuffer.lookupTransform("turtle2", "turtle1",   //通过tfBuffer的类成员函数lookupTransform来监听两只海龟之间的tf关系
                               ros::Time(0));   //使用缓存区最新的tf坐标数据
    }
    catch (tf2::TransformException &ex) {
      ROS_WARN("%s",ex.what());
      ros::Duration(1.0).sleep();
      continue;
    }

    geometry_msgs::Twist vel_msg;
    
    // atan2() 是已知一个角的正切值（也就是 y/x），求该角的弧度值。 
    // https://blog.csdn.net/weixin_46098577/article/details/116026828
    vel_msg.angular.z = 4.0 * atan2(transformStamped.transform.translation.y,
                                    transformStamped.transform.translation.x);
    // linear.x指向机器人前方，linear.y指向左方，linear.z垂直向上满足右手系，平面移动机器人常常linear.y和linear.z均为0
    //0.5为速度系数，为1则等于被跟随乌龟的速度，这里设为0.5，使其只有被跟随乌龟一半的速度，当然设置多少取决于你。
    vel_msg.linear.x = 0.5 * sqrt(pow(transformStamped.transform.translation.x, 2) +
                                  pow(transformStamped.transform.translation.y, 2));
    turtle_vel.publish(vel_msg);
    
    rate.sleep();
  }
  return 0;
};

在终端中运行以下代码即可实现例程:(tf功能包ros中自带，无需再下载编译所提供程序)

roslaunch turtle_tf2 turtle_tf2_demo_cpp.launch