ROS::TF2教程中的例程详解：Writing a tf2 listener (C++)

ROS::TF2教程
Writing a tf2 listener (C++)

详解如下：

#include <ros/ros.h>
//坐标变换守听支持，包含TransformListener类
#include <tf2_ros/transform_listener.h>
//几何消息：坐标变换消息
#include <geometry_msgs/TransformStamped.h>
//几何消息：改变几何
#include <geometry_msgs/Twist.h>
//龟龟模拟器变量，Spawn服务
#include <turtlesim/Spawn.h>

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

  //用创建对象的方式获取节点的句柄，同时开始运行本node
  //这里通过对象的生命周期来管理节点的开始和结束，当node开始时节点就处于运行状态，当运行到main尾部，node析构，节点就销毁了。
  //如果不想通过对象的生命周期来管理节点的开始和结束，可以通过ros::start()和ros::shutdown() 来自己管理节点。
  ros::NodeHandle node;

  //等待龟龟模拟器的spawn服务启动
  ros::service::waitForService("spawn");
  //在本节点上创建一个客户端，连接到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";
  //使用spawn服务在turtlrsim节点中产生新的龟龟，本例程中是“turtle2”。
  spawner.call(turtle);

  //使用Publisher类，创建一个turtle_vel对象，用来"turtle2/cmd_vel"消息。
  ros::Publisher turtle_vel = node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);

  //创建一个缓冲。
  tf2_ros::Buffer tfBuffer;
  //用刚创建的缓冲tfBuffer来初始化创建一个TransformListener类的对象tfListener用于守听Transform消息。
  tf2_ros::TransformListener tfListener(tfBuffer);

  //将节点的运行频率设置为10Hz。
  ros::Rate rate(10.0);

  //当节点可以达到运行条件，例如可以满足运行频率时，返回true。
  while (node.ok())
  {
    //节点运行内容：

    //当下述代码中的TransformListener类的对象transformStamped被创建, 对象就会守听tf2广播的所有坐标变换消息，
    //并将接收到的消息保存到缓冲区（tfBuffer）中，并且仅存储10秒。 
    
    //应该不断的对TransformListener类的对象进行轮询且不能中断，否则对象的缓冲区会被消息装满而导致缓冲区变为不可用状态
    //从而导致后续的轮询返回查询失败状态。 
    //一种常用 的方法是将TransformListener类的对象作为类的成员变量使其生命周期与类的对象统一。
    geometry_msgs::TransformStamped transformStamped;
    try{
      //尝试从listener的缓冲区中扒拉出有关"turtle2", "turtle1"的位姿相对关系的消息，并装入transformStamped对象中。
      //其中的参数如下：
      //参数1: "turtle2"用于变换运算的目标坐标系。
      //参数2: "turtle1"用于变换运算的原坐标系。
      //参数3: 变换运算的时间，亦即提前的时间。使用ros::Time(0)代表我们想用最近（最新）的坐标进行变换运算。
      //参数4（可选）：缓冲消息等待时间 (默认为=ros::Duration(0.0)，即不等待)
      transformStamped = tfBuffer.lookupTransform("turtle2", "turtle1", ros::Time(0));
    }
    catch (tf2::TransformException &ex) {
      //如果妹扒拉出来消息，显示警告消息
      ROS_WARN("%s",ex.what());
      //让本node暂停1秒。
      ros::Duration(1.0).sleep();
      continue;
    }

    //实例化“改变几何消息”对象vel_msg。
    geometry_msgs::Twist vel_msg;

    //利用"turtle2", "turtle1"的位姿相对关系求出相对角度，并且乘上系数4。这其实是一个简单的比例控制器。
    //将计算产生的值，填充到“改变几何消息”的vel_msg.angular.z变量中，即龟龟绕Z轴旋转的角速度使龟龟转头。
    vel_msg.angular.z = 4.0 * atan2(transformStamped.transform.translation.y, transformStamped.transform.translation.x);
    //利用"turtle2", "turtle1"的位姿相对关系求出相对距离，并且乘上系数0.5。
    //将计算产生的值，填充到“改变几何消息”的vel_msg.linear.x变量中，即龟龟绕延本体坐标系的x轴运动的速度，使龟龟朝前走。
    vel_msg.linear.x = 0.5 * sqrt(pow(transformStamped.transform.translation.x, 2) + pow(transformStamped.transform.translation.y, 2));
    
    //用上述代码计算所得的数据，发布"turtle2/cmd_vel"消息。如果龟龟2（turtle2）能接收到的话就会向着龟龟1运动。
    turtle_vel.publish(vel_msg);

    //在节点剩下的运行周期内休眠。
    rate.sleep();
  }
  return 0;
};