ROS：坐标管理系统

一、机器人中的坐标变换

机器人运动学的核心，描述任意两个坐标系之中任意两个向量之间的变换，可以用一个4×4的变换矩阵（Transformation Matrices）来描述它的平移和旋转变化。
变换矩阵中有包括旋转矩阵（Rotation Matrix）的信息和位置移动（Translation）的信息。

二、TF功能包

2.1TF功能包简介

一个机器人中，可以有很多坐标系，我们需要去描述任意两个坐标系之间的关系，涉及到大量的矩阵运算。我们可以用ROS中的TF(Transform)功能包来解决问题。

TF功能包的特点：默认能记录10秒内机器人所有坐标系之间的位置关系。
如：
5秒钟之前，机器人头部坐标系相对于全局坐标系的关系是什么样的？
机器人夹取的物体相对于机器人中心坐标系的位置在哪里？
机器人中心坐标系相对于全局坐标系的位置在哪里？

2.2TF坐标变换实现

实现机制：
广播TF变换
监听TF变换

这有别于之前topic和service的机制。
在ROS Master启动后，启动TF后，会在后台维护一个名为“TF树（TF Tree）”的数据结构。所有的坐标系都是通过树形结构保存在这个树结构当中，当有结点想查询某两个坐标系之间的关系的话，直接可以查询这个TF Tree来得到。

2.3TF案例

比如这辆带激光雷达的车，车体是以base_link为坐标系的，激光雷达是以base_laser为坐标系的，可以看到base_laser是base_link向x轴平移了0.1m，向z轴平移了0.2m，y轴没有平移。
当base_laser测到离墙面的距离为0.3m，即向量(0.3,0,0)时，就可以根据图下方的TF tree进行坐标系之间的数据变换的运算，从而算出base_link的相对与测距点的相对向量(0.4,0,0.2)。

三、小海龟跟随实验

3.1打开小程序

roslaunch turtle_tf turtle_tf_demo.launch

注：第一次运行可能会报错
报错的原因可能是Python解释器的指向问题。
打开终端依次输入：

cd /usr/bin/
sudo rm -r python
sudo cp python3 python

打开后就会有两只海龟，直接用键盘操作一只海龟前进，另一只会跟过来。

3.2查看当前的TF树

查看当前的TF tree，查看一下坐标系之间的关系。

rosrun tf view_frames

注：noetic直接运行可能会报错，无法生成pdf文件。解决方案：
先打开它指向的那个view_frames文件的修改权限：

sudo chmod a+w /opt/ros/noetic/lib/tf/view_frames

打开，88行后加上

vstr = str(vstr)

加上后重新运行。
可在用户文件夹下生成一个pdf文件：

可以看到有3个坐标系，除了两个海龟自身的坐标系，还有个world坐标系。
这颗TF树展示了当前的坐标间的位置关系，turtle1和turtle2是相对world坐标系变化的。

3.3坐标相对位置关系可视化1（tf_echo）

想看两个海龟的相对变换关系，输入：

rosrun tf tf_echo turtle1 turtle2

我们接着操控海龟1移动，相对关系就发生了变化：

这里包含了Translation和Rotation的信息。
Translation表示了相对位移的信息（是一个3×1的向量）。
Rotation表示了旋转矩阵的信息（一个3×3的矩阵，但自由度为3），它又有两种表示方式：
1.RPY表示法(pitch俯仰角、yaw偏航角、roll翻滚角)
2.四元数法(Quaternion)
都可以表示旋转矩阵的信息

Rotation和Translation合起来可以构成变换矩阵

3.4坐标相对位置关系可视化2（rviz）

rviz命令

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

打开rviz界面。
上面Fixed Frame选 world。Add选添加TF，可以看到3个坐标系了。

控制海龟运动，坐标系发生改变，然后坐标系turtle2原点会靠近turtle1原点：


下图中左边变换矩阵的运算，其实就是坐标移动的本质。两个坐标系相对于world坐标系的变换的乘积可以求得两个坐标系相对的变换关系。

参考视屏：古月居ROS入门21讲