这片博客 教你 如何使用TF2 实现 监听 获得 一个 坐标变换

在上一篇博客 创建了一个TF2 的 广播者 ，来发布一个 乌龟的位置 到 TF2， 这个博客 将 创建 一个 TF2 的 监听者 来开始使用TF2.

1、如何创建一个TF2的监听者

1.1 代码

在learning_tf2 的功能包的src 文件夹下 创建一个新的cpp文件 命名为 turtle_tf2_listener.cpp.

整体代码如下：

#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::NodeHandle node;

  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);

  tf2_ros::Buffer tfBuffer;
  tf2_ros::TransformListener tfListener(tfBuffer);

  ros::Rate rate(10.0);
  while (node.ok()){
    geometry_msgs::TransformStamped transformStamped;
    try{
      transformStamped = tfBuffer.lookupTransform("turtle2", "turtle1",
                               ros::Time(0));
    }
    catch (tf2::TransformException &ex) {
      ROS_WARN("%s",ex.what());
      ros::Duration(1.0).sleep();
      continue;
    }

    geometry_msgs::Twist vel_msg;

    vel_msg.angular.z = 4.0 * atan2(transformStamped.transform.translation.y,
                                    transformStamped.transform.translation.x);
    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;
};

1.2 代码解释

=====================================================================

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

以上是需要包含的文件

tf2软件包提供了TransformListener的实现，以帮助简化接收转换的任务。 要使用TransformListener，需要包含tf2 / transform_listener.h头文件。

=====================================================================

  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);

turtle 相关 如此 则 创建 了 一个 乌龟 命名为：turtle2 初始位置 在 x=4，y=2 角度 为0

=====================================================================

  ros::Publisher turtle_vel =
    node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);

声明 速度 topic 发布 句柄 下面 讲 发布 速度控制的 消息 来控制 turtle2 运动

=====================================================================

重点

  tf2_ros::Buffer tfBuffer;
  tf2_ros::TransformListener tfListener(tfBuffer);

在这里，创建一个TransformListener对象。 创建侦听器后，它将开始通过网络接收tf2转换，并将其最多缓冲10秒钟。 TransformListener对象的作用域应为持久性，否则其缓存将无法填充，并且几乎每个查询都会失败。 一种常见的方法是使TransformListener对象成为类的成员变量。

tf2_ros::TransformListener tfListener(tfBuffer)

这样 tfListener 相当于 TransformListener 类的实例 ，tfBuffer 相当于其成员变量

=====================================================================

    geometry_msgs::TransformStamped transformStamped;

声明 TF2 的标准 消息类型 变量 ，用于存放 监听的TF2

=====================================================================

重点

    try{
      transformStamped = tfBuffer.lookupTransform("turtle2", "turtle1",
                               ros::Time(0));
    }
    catch (tf2::TransformException &ex) {
      ROS_WARN("%s",ex.what());
      ros::Duration(1.0).sleep();
      continue;
    }

这里是真正工作的地方，查询监听者一个指定的坐标变换。
通过 lookupTransform （） 函数 实现
这个函数有四个参数
lookupTransform 的 使用 说明

1. 希望 变换到的 坐标系
2. 从这个坐标系 （参数2），转换到目标坐标系（参数1）
3. 转化希望时间 ， ros::Time(0) 即为 得到最新的 可用坐标变换、
4. 超时前的时间 ， 可选 默认为 default=ros::Duration(0.0)

lookupTransform("turtle2", "turtle1",ros::Time(0)
这样即获得了 有 turtle1（坐标系） 到 turtle2 （坐标系）的 坐标变换

=====================================================================

    geometry_msgs::Twist vel_msg;

    vel_msg.angular.z = 4.0 * atan2(transformStamped.transform.translation.y,
                                    transformStamped.transform.translation.x);
    vel_msg.linear.x = 0.5 * sqrt(pow(transformStamped.transform.translation.x, 2) +
                                  pow(transformStamped.transform.translation.y, 2));
    turtle_vel.publish(vel_msg);

乌龟1 的位置 在 turtle1（坐标系） 的 原点
所以 transformStamped.transform.translation.x / y 就是 以 乌龟2 为 原点在 turtle2 （坐标系）下的 位置。

通过这个位置转化为 速度 控制量 来控制 乌龟2 的 运动 实现 乌龟2 跟着 乌龟 1 的效果

=====================================================================

2、运行这个监听者

在 CMakeLists.txt 中 加入 如下 代码：

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

编译成功的话

会 在工作空间 的这个路径下面 生成 turtle_tf2_listener 的 bin 文件

下面添加 launch 文件 给这个demo

  <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_tf2" type="turtle_tf2_broadcaster"
          args="/turtle1" name="turtle1_tf2_broadcaster" />


    <node pkg="learning_tf2" type="turtle_tf2_broadcaster"
          args="/turtle2" name="turtle2_tf2_broadcaster" />


    <node pkg="learning_tf2" type="turtle_tf2_listener"
          name="listener" />

  </launch>

运行

 $ roslaunch learning_tf2 start_demo.launch

会看到 有2 只 乌龟 在屏幕上 ， 乌龟1 不动 ，乌龟2 和乌龟1 初始位置不一样 ，一会 乌龟2 就到了 乌龟1 的地方

移动 乌龟 1

rosrun turtlesim turtle_teleop_key

乌龟2 会 跟着 乌龟 1 移动 最后 两者 位置 重合

3、完毕