ROS入门五 TF坐标变换

坐标变换简介

坐标转换是空间实体的位置描述，是从一种坐标系统变换到另一种坐标系统的过程。通过建立两个坐标系统之间一一对应关系来实现。通常坐标转换有平移、缩放、旋转三个方面的转换。

A、B两个坐标系A坐标系下的位姿可以通过平移和旋转变换成B坐标系下的位姿这里的平移和旋转可以通过4×4的变换矩阵来描述。

TF功能包

是什么？

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

TF功能包干什么 ？

• 五秒钟之前 ，机器人头部坐 标系相对于全局标系的 关是什么样？
• 机器人夹取的物体相对于中心坐标系位置在哪里 ？
• 机器人中心坐标系相对于全局的位置在哪里 ？

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

TF坐标变换如何实现？

1. 监听TF变换

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

2. 广播TF变换

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

TF工具

1. tf_monitor
   tf_monitor工具的功能是打印TF树中所有坐标系的发布状态
   $ tf_monitor
   tf_monitor工具查看TF树中所有坐标系的发布状态

2. tf_echo
   tf_echo工具的功能是查看指定坐标系之间的变换关系
   $ 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_tra nsform_publisher x y z qx qy qz qw frame_id child_frame_id period_in_ms
   该命令不仅可以在终端中使用还可以在launch文件中使用

<launch>
<node pk g="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树的信息。
   $ rosrun tf view_frames
   使用如下命令或者使用pdf阅读器查看生成的pdf文件
   $ evince frames.pdf

乌龟例程中的TF

安装功能包turtle_tf

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

运行

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

$ roslaunch turtle_tf turtle_tf_demo.launch

乌龟仿真器打开后会出现两只小乌龟并且下方的小乌龟会自动向中心位置的小乌龟移动

打开键盘控制节点控制中心位置的小乌龟运行，另外一只乌龟总是会跟随我们控制的那只乌龟运行。

$ rosrun turtlesim turtle_teleop_key

查看这个例程中的TF树

$ rosrun tf view_frames

三个坐标系之间的变换关系

使用tf_echo工具在TF树中查找乌龟坐标系之间的变换关系

$ rosrun tf tf_echo turtle1 turtle2

通过rviz的图形界面显示三者之间的关系

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

实现TF的广播和监听功能

TF消息广播之后其他节点就可以监听该TF消息从而获取需要的坐标变换了。目前我们已经将乌龟相对于world坐标系的TF变换进行了广播接下来需要监听TF消息并从中获取turtle2相对于turtle1坐标系的变换从而控制turtle2移动。

创建TF广播

在learning_tf功能包src下创建名为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);

    // 发布坐标变换
    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;
};

创建TF监听器

在learning_tf功能包src下创建名为turtle_tf_listener.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);

    // 发布坐标变换
    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;
};

实现乌龟跟随运动

在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也跟随移动。

古月《ROS机器人开发实践》