ROS高级组件之 TF坐标变换 其一
1 资料
从本文开始，我们使用三个博客学习TF坐标变化，TF即transform，话说电影变形金刚的英文就是transformer。
本文的参考资料如下：
（1）《ROS机器人开发实践》胡春旭 第4章第二节
（2）右手坐标系：机器人中的xyz坐标系和右手定则
（3）欧拉角与四元数：
详解 欧拉角与四元数
四元数与欧拉角（RPY角）的相互转换
四元数与欧拉角（Yaw、Pitch、Roll）的转换
2 正文
2.1 右手坐标系
机器人和自动驾驶领域，所有的坐标系都是右手坐标系：伸出右手，从x轴正向向y轴正向握，大拇指的方向就是z轴正向。
2.2 欧拉角
在三维空间中，Roll、Pitch 和 Yaw 是描述物体旋转的三个基本角度，也被称为欧拉角。
欧拉角用来描述一个物体相对于自身原始坐标系的旋转变化，以飞机举例，其自身原始三维坐标系是：机身沿着x轴，机头朝向x轴正方向，根据右手定则，y轴在飞机的左侧，z轴在飞机的上面。
（1）Roll（横滚）：物体绕其本身的x轴旋转的角度。
（2）Pitch（俯仰）：物体绕其本身的y轴旋转的角度。
（3）Yaw（偏航）：物体绕其本身的z轴旋转的角度。
这里有一个组图，也可以帮助理解欧拉角：
2.3 欧拉角与四元数
欧拉角和四元数都用来描述物理在三维空间内的旋转，这里进行比较：
（1）欧拉角用三个旋转角度，直观清晰，但会有万向节锁死的问题。具体见：详解 欧拉角与四元数
（2）四元数描述一个物体的旋转，使用旋转向量 + 旋转角度的组合，向量用x,y,z表示，角度用w，即q=(x,y,z,w)，也就是四元数。而且其中的x,y,z是个复数，即：q=xi+yj+zk+w, i2 = j2 = k2 = -1。
（3）四元数有运算效率高，无万向节死锁问题，且方便进行插值运算等优点。但由于四元数无法直观从物理意义上理解，很抽象，因此，通常情况下，人们使用欧拉角描述旋转，需要运算时，再转换为四元数。
（4）四元数与欧拉角之间有一套固定的矩阵运算转换公式（这里不深究，也确实不好懂）：四元数与欧拉角（Yaw、Pitch、Roll）的转换
2.4 TF 软件包的由来
一个机器人通常有很多个坐标系：手部，脚部，头部，关节等等
一个自动驾驶车辆（也属于机器人系统）往往有很多个坐标系，比如：
（1）通用横轴墨卡托投影（UTM，GPS坐标一般会转化为这个坐标值）
（2）lidar坐标系：lidar的点云数据都是用这个坐标系描述的
（3）radar坐标系（多个）：同上
（4）相机坐标系（多个）：同上
（5）imu的坐标系：
（6）车体坐标系：原点位于后轴中心
（7）当地水平坐标系：ENU，俗称世界坐标系
自动驾驶车辆所有的传感器和GPS数据，都是相对于自身坐标系的值。使用时，需要转换到同一个坐标系，才能使用，一般选取ENU坐标系，不然就是散乱的无效数据。
通常情况下，两个三维坐标系的转换包括平移和旋转，平移关系比较简单，也就是两个坐标原点之间的移动关系；而旋转关系比较复杂，需要使用欧拉角和四元数。
机器人系统的三维坐标系的数据转换是非常复杂的，ros提供了 TF 包来解决这个棘手的问题，避免开发者手动进行坐标变换。
2.5 以turtle_tf为例学习 TF 包
ROS 提供了 turtle_tf 作为样例，供开发者学习 TF 软件包的基本原理。
（1）运行 turtle_tf 样例
roslaunch turtle_tf turtle_tf_demo.launch
rosrun turtlesim turtle_teleop_key
// 如果rviz无显示，看这个博客：https://www.codestudyblog.com/cs2201ai/40115063750.html
rosrun rviz rviz 'rospack find turtle_tf' /rviz/turtle_rviz.rviz
turtle_tf 设计了两个乌龟，操作者使用键盘控制第一个乌龟的移动，第二个乌龟会主动跟上去，如下图。
（2）turtle_tf 的三个坐标系
world坐标系：原点位于窗口的左下角，x轴从原点出发，朝向右侧；y轴从原点出发，朝向上侧；根据右手定则，z轴垂直于屏幕，朝向人；由于仿真环境是二位的，所以z轴不需要考虑。
turtle1坐标系：turtle1自身坐标系，turtle1沿着x轴，头部朝向x轴正方形，y轴在turtle1左侧，z轴在turtle1上面（不用考虑）
turtle2坐标系：turtle2自身坐标系，同turtle1
在rviz里，三个坐标系长这样（注意：两个截图并不是同一时刻）：
（3）TF 软件包管理坐标系的逻辑：TF 坐标系树
TF 包会把各个坐标系在内部组织成坐标系树。如 turtle_tf 包，坐标系树根节点是world坐标系，左右子节点是两个turtle的自身坐标系。两个turtle各自创建自己的 TF 广播器，把自身坐标系与world坐标系的tf数据广播出来，TF 包就能实时建立坐标系树。
执行下面的指令，导出turtle_tf 的TF 坐标系树：
// ubuntu20.04执行下面命令会崩溃，解决方式：https://github.com/ros/geometry/pull/193
rosrun tf view_frames
（4）有了 TF 坐标系树，可以通过创建 TF 监听器，实时获取turtle1 相对于turtle2 坐标系的平移和旋转关系。执行下面的指令：
rosrun tf tf_echo turtle2 turtle1
运行输出：
At time 1688002771.829
// 平移向量，也就是两个坐标原点的平移，即在turtle2坐标系下，turtle1的位置
- Translation: [-0.403, 0.072, 0.000]
// 四元数描述的旋转关系，即在turtle2坐标系下，turtle1的朝向
- Rotation: in Quaternion [0.000, 0.000, -0.089, 0.996]
// 弧度描述的欧拉角，因为是二维平面，只有yaw（绕z轴）角有值
in RPY (radian) [0.000, 0.000, -0.178]
// 角度描述的欧拉角
in RPY (degree) [0.000, 0.000, -10.174]
利用获取到的坐标变化关系，turtle2就能实时的跟着turtle1跑，具体的实现见 ROS高效进阶第一章 – ROS高级组件之 TF坐标变换 其二。
2.6 理解turtlesim::Pose和geometry_msgs/Twist
在 ROS高效入门系列中，我们写了很多样例，都用到了 turtlesim::Pose和geometry_msgs/Twist 消息。前者的topic名为：/turtle1/pose，用来描述turtle1的位置信息；后者的topic名为：/turtle1/cmd_vel，用来控制turtle1的运行。
由于之前没有坐标系的知识，我们对这两个消息的数据域理解并不深入，这里我们深入分析下
（1）turtlesim::Pose是一个定义了在world坐标系下，乌龟位置和朝向的消息类型。它包含如下五个字段：
x，y：是world坐标系下的位置
theta：是world坐标系下，乌龟相对于z轴的旋转；
如果乌龟头朝向world坐标系x轴正向，则theta为0；
逆时针旋转时，theta增加，最大为pi；
顺时针旋转时，theta减小，最小为-pi
linear_velocity：线速度，米每秒
angular_velocity：角速度，弧度每秒
（2）geometry_msgs/Twist的域是两个向量：
geometry_msgs/Vector3 linear // turtle相对于自身坐标系的线速度向量
float64 x // 沿x轴方向的线速度（m/s），正值为前进，负值为后退
float64 y // 沿y轴方向的线速度（m/s），由于turtle永远沿着自身坐标系x轴朝向正方向，不会侧向运动，因此此值一直为0
float64 z // 沿z轴方向的线速度（m/s），由于turtle位于二维平面内，不会上下运动，因此此值一直为0
geometry_msgs/Vector3 angular // turtle相对于自身坐标系的角速度向量
float64 x // 绕x轴的角速度（以弧度/秒为单位），由于turtle不会绕x轴旋转，因此为零
float64 y // 绕y轴的角速度（以弧度/秒为单位），由于turtle不会绕y轴旋转，因此为零
float64 z // 绕z轴的角速度（以弧度/秒为单位），表示turtlez左转（正值），右转（负值）
// 这条命令是让turtle1顺时针画圆，圆半径就是r = v / w = 2.0 m/s / 1.8 rad/s ~= 1.11 m
rostopic pub /turtlesim1/turtle1/cmd_vel geometry_msgs/Twist -r 1 -- '[2.0, 0.0, 0.0]' '[0.0, 0.0, -1.8]'
3 总结
这节我们从数学出发，系统梳理了三维坐标变化的知识，并利用 ros 自带的 turtle_tf 初步学习了 TF 软件包的原理。基于本节的知识，下节我们将手动实现一个 turtle_tf 样例，掌握 TF 库的接口调用，掌握 TF广播器和 TF监听器的编写方法，见 ROS高效进阶第一章 – ROS高级组件之 TF坐标变换 其二