ROS机器人开发实践
上QQ阅读APP看书，第一时间看更新
上一章目录下一章

4.2　TF坐标变换

坐标变换是机器人学中一个非常基础同时也是非常重要的概念。机器人本体和机器人的工作环境中往往存在大量的组件元素，在机器人设计和机器人应用中都会涉及不同组件的位置和姿态，这就需要引入坐标系以及坐标变换的概念。

图4-1　某位姿在A、B两个坐标系下的坐标变换

如图4-1所示的A、B两个坐标系，A坐标系下的位姿可以通过平移和旋转变换成B坐标系下的位姿，这里的平移和旋转可以通过4×4的变换矩阵来描述，其理论原理可以参考机器人学的理论教程。

坐标变换是机器人系统中常用的基础功能，ROS中的坐标变换系统由TF功能包维护。

4.2.1　TF功能包

TF是一个让用户随时间跟踪多个坐标系的功能包，它使用树形数据结构，根据时间缓冲并维护多个坐标系之间的坐标变换关系，可以帮助开发者在任意时间、在坐标系间完成点、向量等坐标的变换。

如图4-2所示，一个机器人系统通常有很多三维坐标系，而且会随着时间的推移发生变化，如世界坐标系（World Frame）、基坐标系（Base Frame）、机械夹爪坐标系（Gripper Frame）、机器人头部坐标系（Head Frame）等。TF可以时间为轴跟踪这些坐标系（默认10s之内），并且允许开发者请求如下类型的数据：

图4-2　机器人系统中繁杂的坐标系

·5秒钟之前，机器人头部坐标系相对于全局坐标系的关系是什么样的？

·机器人夹取的物体相对于机器人中心坐标系的位置在哪里？

·机器人中心坐标系相对于全局坐标系的位置在哪里？

TF可以在分布式系统中进行操作，也就是说，一个机器人系统中所有的坐标变换关系，对于所有的节点组件都是可用的，所有订阅TF消息的节点都会缓冲一份所有坐标系的变换关系数据，所以这种结构不需要中心服务器来存储任何数据。

想要使用TF功能包，总体来说需要以下两个步骤。

1）监听TF变换

接收并缓存系统中发布的所有坐标变换数据，并从中查询所需要的坐标变换关系。

2）广播TF变换

向系统中广播坐标系之间的坐标变换关系。系统中可能会存在多个不同部分的TF变换广播，每个广播都可以直接将坐标变换关系插入TF树中，不需要再进行同步。

4.2.2　TF工具

坐标系统虽然是一个基础理论，但是由于涉及多个空间之间的变换，不容易进行想象，所以TF提供了丰富的终端工具来帮助开发者调试和创建TF变换。

1.tf_monitor

tf_monitor工具的功能是打印TF树中所有坐标系的发布状态（见图4-3），也可以通过输入参数来查看指定坐标系之间的发布状态（见图4-4）。

$ tf_monitor

图4-3　使用tf_monitor工具查看TF树中所有坐标系的发布状态

$ tf_monitor <source_frame> <target_target>

图4-4　使用tf_monitor工具查看指定坐标系之间的发布状态

2.tf_echo

tf_echo工具的功能是查看指定坐标系之间的变换关系。命令的格式如下：

$ tf_echo <source_frame> <target_frame>

运行后的效果如图4-5所示。

图4-5　使用tf_echo工具查看指定坐标系之间的变换关系

3.static_transform_publisher

static_transform_publisher工具的功能是发布两个坐标系之间的静态坐标变换，这两个坐标系不发生相对位置变化。命令的格式如下：

$ static_transform_publisher x y z yaw pitch roll frame_id child_frame_id period_in_ms
$ static_transform_publisher x y z qx qy qz qw frame_id child_frame_id    period_in_ms

以上两种命令格式，需要设置坐标的偏移参数和旋转参数：偏移参数使用相对于x、y、z三轴的坐标位移；而旋转参数的第一种命令格式使用以弧度为单位的yaw/pitch/roll角度（yaw是围绕z轴旋转的偏航角，pitch是围绕y轴旋转的俯仰角，roll是围绕x轴旋转的翻滚角），第二种命令格式使用四元数表达旋转角度。发布频率以ms为单位。

该命令不仅可以在终端中使用，还可以在launch文件中使用，方法如下：

<launch>
<node pkg="tf" type="static_transform_publisher" name="link1_broadcaster" args="1 0 0 0 0 0 1 link1_parent link1 100" />
</launch>

4.view_frames

view_frames是可视化的调试工具，可以生成pdf文件，显示整棵TF树的信息。该命令的执行方式如图4-6所示：

$ rosrun tf view_frames

图4-6　使用view_frames工具生成TF树的信息

然后使用如下命令，或者使用pdf阅读器查看生成的pdf文件（见图4-7）。

$ evince frames.pdf

图4-7　可视化的TF树信息

除此之外，rviz中还提供了TF可视化显示的插件，关于rviz的具体内容将在本章后续详细讲解。

4.2.3　乌龟例程中的TF

接下来我们在乌龟仿真器中通过一个例程（turtle_tf）来理解TF的作用，并且熟悉以上学到的TF工具。该例程的功能包turtle_tf可以使用如下命令进行安装：

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

安装完成后就可以使用如下命令运行例程了：

$ roslaunch turtle_tf turtle_tf_demo.launch

乌龟仿真器打开后会出现两只小乌龟，并且下方的小乌龟会自动向中心位置的小乌龟移动（见图4-8）。

打开键盘控制节点，控制中心位置的小乌龟运行：

$ rosrun turtlesim turtle_teleop_key

另外一只乌龟总是会跟随我们控制的那只乌龟运行（见图4-9）。在这个例程中，TF是如何运用的呢？首先使用TF工具来看一下这个例程中的TF树是什么样的：

$ rosrun tf view_frames

如图4-10所示，在当前系统中存在三个坐标系：world、turtle1、turtle2。world是世界坐标系，作为系统的基础坐标系，其他坐标系都相对该坐标系建立，所以world是TF树的根节点。相对于world坐标系，又分别针对两只乌龟创建了两个乌龟坐标系，这两个坐标系的原点就是乌龟在世界坐标系下的坐标位置。

图4-8　乌龟仿真器的启动界面

图4-9　乌龟跟随移动

图4-10　乌龟跟随例程中的TF树

现在要让turtle2跟随turtle1运动，等价于turtle2坐标系需要向turtle1坐标系移动，这就需要知道turtle2与turtle1之间的坐标变换。三个坐标系之间的变换关系可以使用如下公式描述：

使用tf_echo工具在TF树中查找乌龟坐标系之间的变换关系（见图4-11）：

$ rosrun tf tf_echo turtle1 turtle2

图4-11　乌龟坐标系之间的变换关系

也可以通过rviz的图形界面更加形象地看到这三者之间的坐标关系（见图4-12）：

$ rosrun rviz rviz -d `rospack find turtle_tf`/rviz/turtle_rviz.rviz

图4-12　在rviz中显示坐标系之间的关系

得到turtle2与turtle1之间的坐标变换后，就可以计算两只乌龟间的距离和角度，即可控制turtle2向turtle1移动了。

接下来我们以这个例程为目标，学习如何实现TF的广播和监听功能。

4.2.4　创建TF广播器

首先，我们需要创建一个发布乌龟坐标系与世界坐标系之间TF变换的节点，实现源码learning_tf/src/turtle_tf_broadcaster.cpp的具体内容如下：

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <turtlesim/Pose.h>
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);
    // 发布坐标变换
    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;
};

以上代码的关键部分是处理乌龟pose消息的回调函数poseCallback，在广播TF消息之前需要定义tf：：TransformBroadcaster广播器，然后根据乌龟当前的位姿设置tf：：Transform类型的坐标变换，包含setOrigin设置的平移变换以及setRotation设置的旋转变换。

然后使用广播器将坐标变换插入TF树并进行发布，这里发布的TF消息类型是tf：：StampedTransform，不仅包含tf：：Transform类型的坐标变换、时间戳，而且需要指定坐标变换的源坐标系（parent）和目标坐标系（child）。

4.2.5　创建TF监听器

TF消息广播之后，其他节点就可以监听该TF消息，从而获取需要的坐标变换了。目前我们已经将乌龟相对于world坐标系的TF变换进行了广播，接下来需要监听TF消息，并从中获取turtle2相对于turtle1坐标系的变换，从而控制turtle2移动。实现源码learning_tf/src/turtle_tf_listener.cpp的详细内容如下：

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>
#include <turtlesim/Spawn.h>
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;
    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;
};

该节点首先通过服务调用产生乌龟turtle2，然后声明控制turtle2速度的Publisher。在监听TF消息之前，需要创建一个tf：：TransformListener类型的监听器，创建成功后监听器会自动接收TF树的消息，并且缓存10秒。然后在循环中就可以实时查找TF树中的坐标变换了，这里需要调用的是tf：：TransformListener中的两个接口：

·waitForTransform(const std：：string&target_frame，const std：：string&source_frame，const ros：：Time&time，const ros：：Duration&timeout)：给定源坐标系（source_frame）和目标坐标系（target_frame），等待两个坐标系之间指定时间（time）的变换关系，该函数会阻塞程序运行，所以要设置超时时间（timeout）：

·lookupTransform(const std：：string&target_frame，const std：：string&source_frame，const ros：：Time&time，StampedTransform&transform）：给定源坐标系（source_frame）和目标坐标系（target_frame），得到两个坐标系之间指定时间（time）的坐标变换（transform），ros：：Time(0)表示我们想要的是最新一次的坐标变换：

通过以上两个接口的调用，就可以获取turtle2相对于turtle1的坐标变换了。然后根据坐标系之间的位置关系，计算得到turtle2需要运动的线速度和角速度，并发布速度控制指令使turtle2向turtle1移动。

4.2.6　实现乌龟跟随运动

现在小乌龟跟随例程的所有代码都已经完成，下面来编写一个launch文件，使所有节点运行起来，实现源码learning_tf/launch/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>

然后运行该launch文件，就可以看到与之前例程类似的两只乌龟的界面了，在终端中通过键盘控制turtle1移动，turtle2也跟随移动。

通过这个例程的实现，我们学习了TF广播与监听的实现方法，在实际应用中会产生更多的坐标系，TF树的结构也会更加复杂，但是基本的使用方法依然相同。

上一章目录下一章