机器人算法—ROS TF坐标变换

1.TF基本概念

（1）什么是TF？
TF是Transformations Frames的缩写。在ROS中，是一个工具包，提供了坐标转换等方面的功能。
tf工具包，底层实现采用的是一种树状数据结构，根据时间缓冲并维护多个参考系之间的坐标变换关系，可以帮助程序员在任意时间，将点、向量等数据的坐标，在两个参考系中完成坐标变换。
（2）为什么需要坐标转换？
坐标转换最主要要解决的是位置和姿态问题。
（3）什么是位置？ 我在哪里说的就是位置问题。
（4）什么是姿态？我摆了个什么造型给你看，是横着的，竖着的，躺着的，这个就是姿态。
其实坐标转换解决的问题是 我在什么位置摆了个什么造型的问题。
其实描述一个物体的位置，我们有很多种方式。
例如我们描述一下月球的位置：

上图中，我们可以说月亮在地球的左上方xxx公里，也可以说在火星左下方xxx公里。

但是如果我们看看实时运行图时

我们不能简单的描述：
月亮在地球的左上方xxx公里，在火星左下方xxx公里。
因为他是实时运动的，这个时候我们要采用一定规则对坐标进行描述，还要实时动态的计算出来。
坐标描述规则就是位置和姿态。
位置的描述也都是采用相对的策略。
实时计算就是将两两相对的位置，转换为另外两两相对的位置。

2.TF使用的场景

机器人每个关节的传感和运动都是参考自身 以上面的机器人烹饪为例, 头部的视觉传感器(摄像头)参考自己, 计算出煎饼和锅距离自己的距离.
机器人的左手右手的运动又是根据自己的当前位置来控制空间坐标的移动.
如果想让机器人各个部件能够协同工作, 就要建立一种世界坐标系统, 可以让各个node节点坐标系的点和向量可以互相转换.

3.TF工作原理

坐标系
右手法则

姿态RPY
roll: 翻滚 pitch: 俯仰 yaw: 偏航

TF构成
ros中，tf工具包包含了三块内容：Broadcaster,Listener,TF转换工具
要弄清楚怎么使用TF工具包，首先我们要明确位置关系描述。
位置关系描述是通过相对坐标系来的。相对坐标系就是一个参照物，我们在这个参照物下，来说明自己的位置和姿态。

4.TF工作原理

ROS中提供的是TF转换工具。转换是通过两个部分来完成的。
Broadcaster负责向TF工具广播 参照物和自己的位置关系
Listener负责向TF工具查看 想要知道的两个物体间的相对坐标
TF工具底层是通过向量来去实现的。

5.案例一

TF开发流程

1. 建立Broadcaster，将小乌龟1坐标发送给TF工具

void callback(const turtlesim::Pose::ConstPtr &message, tf::TransformBroadcaster broadcaster) {
    tf::Transform transform;
    //设置位置
    transform.setOrigin(tf::Vector3(message->x, message->y, 0));
    //设置姿态
    tf::Quaternion quaternion;
    quaternion.setRPY(0, 0, message->theta);
    transform.setRotation(quaternion);
    broadcaster.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "turtle1"));
}

我们在小乌龟pose回调中，得到小乌龟相对于窗体的坐标信息(message)。
broadcaster在发送相对位置信息时，需要给定两个类型的数据：
在相对环境中的 坐标（origin），包含x,y,z坐标
在相对环境中的姿态 (rotation)，包含x,y,z方向的转动情况
在发送数据时，要标明谁相对谁的位置。参照物我们认为是父坐标。

2. 建立Broadcaster，将小乌龟2坐标发送给TF工具¶
   参考第1步骤

3. 建立Listener，通过查看TF工具获得小乌龟1和小乌龟2间的相对坐标

tf::TransformListener listener;
ros::Rate rate(10);
while (ros::ok()) {
    tf::StampedTransform transform;
    try {
        listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform);
    } catch (exception e) {
        ROS_INFO_STREAM(e.what());
        rate.sleep();
        continue;
    }
    tf::Vector3 &origin = transform.getOrigin();
    double x = origin.x();
    double y = origin.y();

    const tf::Quaternion &quaternion = transform.getRotation();
    double theta = quaternion.z();
}

转换监听器listener可以从TF工具中，获得想要的两个物体间的相对坐标。
前一个参数是作为参照物存在的，坐标和姿态都是(0,0,0)
第二个参数是相对于第一个参数的参照物的坐标和姿态。

Reference

1. TF工作原理