ROS学习（22）TF变换

---

前言

  机器人本地和机器人的工作环境中存在大量的组件元素，在机器人设计和应用中都会涉及不同组件的位置和姿态，这就需要引入坐标系以及坐标变换的概念。
  坐标变换是机器人系统中常用的基础功能，ROS中的坐标变换系统由TF功能包维护。

一、TF功能包

  TF是一个让用户随时间跟踪多个坐标系的功能包，使用树形数据结构，根据时间缓冲并维护多个坐标系之间的坐标变换关系，帮助开发者在任意时间、在坐标系间完成点、向量等坐标的变换。
  TF可以在分布式系统中进行操作，一个机器人系统中所有的坐标变换关系，对于所有的节点组件都是可用的，所有订阅TF消息的节点都会缓冲一份所有坐标系的变换关系数据，所以这种结构不需要中心服务器来存储任何数据。
  想要使用TF功能包，总体来说需要两步：

1. 监听TF变换
   接收并缓存系统中发布的所有坐标变换数据，并从中查询所需要的坐标变换关系。
2. 广播TF变换
   向系统中广播坐标系之间的坐标变换关系。系统中可能会存在多个不同部分的TF变换广播，每个广播都可以直接将坐标系变换关系插入TF树中，不需要再进行同步。

二、TF工具

坐标系统涉及多个空间之间的变换，不容易进行抽象，所以TF提供了丰富的终端工具来帮助开发者调试和创建TF变换。

1、tf_monitor

1）用于打印TF树中所有坐标系的发布状态

rosrun tf tf_monitor

2）查看指定坐标系之间的发布状态

rosrun tf tf_monitor <source_frame> <target_frame>

2、tf_echo

用于查看指定坐标系之间的变换关系

rosrun tf tf_echo <source_frame> <target_frame>

3、static_transform_publisher

用于发布两个坐标系之间的静态坐标变换，这两个坐标系不发生相对位置变化。该工具需要设置坐标的偏移参数和旋转参数，发布频率以ms为单位。
命令有两种格式，如下：
1）旋转参数使用以弧度为单位的yaw/pitch/roll角度

rosrun tf static_transform_publisher x y x yaw pitch roll frame_id child_frame_id period_in_ms

2）旋转参数使用四元数

rosrun tf static_transform_publisher x y x qx qy qw frame_id child_frame_id period_in_ms

该命令还可以在launch文件中使用，如下：

<launch>
  <node pkg="tf" type="static_transform_publisher" name="link_broadcaster" args="1 0 0 0 0 0 1 link_parent link 100" />
<launch>

4、view_frames

view_frames是可视化的调试工具，可以生成pdf文件，显示TF树的信息。命令如下：

rosrun tf view_frames

查看pdf文件，可以使用如下命令：

evince frames.pdf

三、乌龟例程中的TF

主要用于理解TF的作用，并且熟悉上述TF工具的使用，功能包名为turtle_tf，功能包安装命令如下：

sudo apt-get install ros-kinetic-turtle-tf

运行turtle_tf功能包，命令如下：

roslaunch turtle_tf turtle_tf_demo.launch

打开键盘控制节点，命令如下：

rosrun turtlesim turtle_teleop_key

效果如下：

可以发现，出现了两只乌龟，使用键盘方向键控制一只乌龟移动，会发现另一只乌龟会跟随移动。
其TF树如下：

如上所示，当前系统中存在三个坐标系，时间坐标系world、乌龟坐标系turtle1和乌龟坐标系turtle2。
世界坐标系是该系统的基础坐标系，其它坐标系都相对该坐标系建立，所以world是TF树的根节点，而两只乌龟坐标系的原点就是乌龟在世界坐标系下的坐标位置。
可以通过如下命令，查看两只乌龟坐标系之间的变换关系：

rosrun tf tf_monitor turtle1 turtle2

效果如下：

四、乌龟跟随例程代码实现

现在要让turtle2跟随turtle1运动，等价于turtle2坐标系向turtle1坐标系移动，首先新建learning_tf功能包。

1、创建TF广播器

创建一个节点，主要用于发布乌龟坐标系与世界坐标系之间TF变换，turtle_tf_broadcaster.cpp内容如下：

#include 
#include 
#include 

std::string turtle_name;

void poseCallback(const turtlesim::PoseConstPtr& msg)
{
    // tf广播器
    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);

    // 将坐标变换插入TF树并发布坐标变换
    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];

    // 订阅乌龟的pose信息
    ros::NodeHandle node;
    ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);

    ros::spin();

    return 0;
};

2、创建TF监听器

创建一个节点，主要用于监听TF消息，从中获取turtle2相对于turtle1的坐标系的变换，从而控制turtle2的移动。turtle_tf_listener.cpp内容如下：

#include 
#include 
#include 
#include 

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

    ros::NodeHandle node;

    // 通过服务调用，产生第二只乌龟turtle2
    ros::service::waitForService("spawn");
    ros::ServiceClient add_turtle =
    node.serviceClient<turtlesim::Spawn>("spawn");
    turtlesim::Spawn srv;
    add_turtle.call(srv);

    // 定义turtle2的速度控制发布器
    ros::Publisher turtle_vel =
    node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);

    // tf监听器
    tf::TransformListener listener;
	//监听器会自动接收TF树的消息，并且缓存10秒
    ros::Rate rate(10.0);
    while (node.ok())
    {
        tf::StampedTransform transform;
        try
        {
            // 查找turtle2与turtle1的坐标变换
            listener.waitForTransform("/turtle2", "/turtle1", ros::Time(0), ros::Duration(3.0));
            listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform);
        }
        catch (tf::TransformException &ex) 
        {
            ROS_ERROR("%s",ex.what());
            ros::Duration(1.0).sleep();
            continue;
        }

        // 根据turtle1和turtle2之间的坐标变换，计算turtle2需要运动的线速度和角速度
        // 并发布速度控制指令，使turtle2向turtle1移动
        geometry_msgs::Twist vel_msg;
        vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(),
                                        transform.getOrigin().x());
        vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) +
                                      pow(transform.getOrigin().y(), 2));
        turtle_vel.publish(vel_msg);

        rate.sleep();
    }
    return 0;
};

3、实现乌龟跟随运动

编写start_demo_with_listener.launch文件，内容如下：

 <launch>
    <!-- 海龟仿真器 -->
    <node pkg="turtlesim" type="turtlesim_node" name="sim"/>

    <!-- 键盘控制 -->
    <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>

    <!-- 两只海龟的tf广播 -->
    <node pkg="learning_tf" type="turtle_tf_broadcaster"
          args="/turtle1" name="turtle1_tf_broadcaster" />
    <node pkg="learning_tf" type="turtle_tf_broadcaster"
          args="/turtle2" name="turtle2_tf_broadcaster" />

    <!-- 监听tf广播，并且控制turtle2移动 -->
    <node pkg="learning_tf" type="turtle_tf_listener"
          name="listener" />

 </launch>

运行如下命令：

roslaunch learning_tf start_demo_with_listener.launch 

效果如下：

打开键盘控制节点，命令如下：

rosrun turtlesim turtle_teleop_key

跟随效果如下：