TF坐标变换

1.常用数据类型
2.TF 编程
2.1 TF 广播器
2.2 TF 监听器
3.轻舟机器人tf坐标变换
3.1 TF树如下：
4.机器人坐标系问题
4.1举例：
4.2 TF中相关概念的介绍
4.3 TF中话题的发布

1.常用数据类型

参考 ROS Wiki 上面给出的 TF 数据类型，主要有以下 6 种基本类型，分别对应四元数，向量，点坐标，位姿和转模板。

2.TF 编程

任何使用 TF 包的时，都需要编写两个程序，分别用来监听 TF 变换和广播 TF 变换的功能，称之为 TF 监听器和 TF 广播器。
1》TF 监听器：接受缓存系统中发布的所有参考系坐标变换，并从中查询所需要的参考系变换。
2》TF 广播器：广播 TF 变换，向系统中广播参考系之间的坐标变换关系。系统中可能会存在多个不同部分的 tf 变化广播，但每个广播可以直接将参考系变换关系直接插入 tf树中，不需要再进行同步。

2.1 TF 广播器

实现功能：创建 TF 广播器，创建坐标变换值并发布实时发布坐标变换
编程思路：
1.初始化 ROS 节点，并订阅 turtle 的位置消息；
2.循环等待话题消息，接收到之后进入回调函数，该回调函数用以处理并发布坐标变换；
3.在该回调函数内部定义一个广播器；
4.根据接收到的小海龟的位置消息，创建坐标变换值
5.通过定义的广播器发布坐标变换。

2.2 TF 监听器

创建 TF 监听器，创建第二只海龟，监听坐标变换并发布运动控制指令使第二只海龟向第一只海龟运动
编程思路：
2. 初始化 ROS 节点，并向 MASTER 注册节点信息；
3. 通过服务调用产生第二只海龟；
4. 创建 turtle2 的速度控制发布器；
5. 创建 tf 监听器并监听 turtle2 相对于 turtle1 的坐标变换；
6. 根据坐标变换发布速度控制指令；

3.轻舟机器人tf坐标变换

3.1 TF树如下：

轻舟机器人上的主要传感器为 IMU 模块和激光雷达，两个传感器相对于车体位置都是静态的，所以示例程序采用 static_transform_publisher 工具的功能是发布两个参考系之间的静态坐标变换，两个参考系一般不发生相对位置变化。命令的格式如下：

以上两种命令格式，需要设置坐标的偏移和旋转参数，偏移参数都使用相对于 xyz 三轴的坐标位移，而旋转参数第一种命令格式使用以弧度为单位的 yaw/pitch/roll 三个角度（yaw是围绕 x 轴旋转的偏航角，pitch 是围绕 y 轴旋转的俯仰角，roll 是围绕 z 轴旋转的翻滚角），而第二种命令格式使用四元数表达旋转角度。发布频率以 ms 为单位，一般 100ms 比较合适。该命令不仅可以在终端中使用，还可以在 launch 文件中使用，使用方式如下：
根据以上静态 TF 发布方式，可将轻舟机器人 TF 变换写入 launch 文件中，底盘到激光雷达的 TF 变化如下：底盘到 IMU 的 TF 变化如下：

4.机器人坐标系问题

简单来说，我们知道在机器人工作过程中，机器人自身处于世界坐标系下，同时其自身又具备了一个局部坐标系，这两个坐标系的存在确保了我们能够计算得出机器人的位姿。

但是对于机器人本体来说，其内部同样存在着很多需要确定位姿的部分，而它们在不同时刻所处的状态或许也是不同的，如果以底部框架作为基座标，那么我们又该如何确定不同时刻机器人内部如激光雷达、抓手、关节、头部的位姿呢？最好的方法就是再在这些东西上建立各自的坐标系，并且通过坐标变换得到它们的位姿。
而 TF 的作用就是帮助你实现系统中任一个点在所有坐标系之间的坐标变换，任意给定系统中一个点的坐标，TF 能够帮你换算得出该点在其他坐标系下的坐标。

4.1举例：

以机器人为例，定义两个坐标系，一个坐标系以机器人移动平台的中心为原点，称为base_link参考系，另一个坐标系以激光雷达的中心为原点，称为base_laser 参考系。
我们要做的是利用tf坐标转换，将base_laser参考系与base_link参考系重合，以便于更准确的得知机器人自身位置继续进行下一步的工作。

4.2 TF中相关概念的介绍

ROS 中机器人模型包含大量的部件，每一个部件统称之为 link（比如手部、头部、某个关节、某个连杆）,每一个 link 上面对应着一个 frame（坐标系）, 用 frame 表示该部件的坐标系，frame 和 link 是绑定在一起的。

tf可以看成是一个话题**/tf**，话题中的消息保存的就是tf tree的数据结构格式。维护了整个机器人的甚至是地图的坐标转换关系。维持并更新机器人整个坐标系的话题是**/tf**，/tf话题表示的内容是整个机器人的tf树，而非仅仅是某两个坐标系的转换关系，这样的话，/tf话题是需要很多的节点来维护的，每一个节点维护两个frame之间的关系。

每两个相邻 frame 之间靠节点发布它们之间的位姿关系，这种节点称为 broadcaster。broadcaster 就是一个发布器 publisher,如果两个 frame 之间发生了相对运动，broadcaster 就会发布相关消息。

TF 库的目的是实现系统中任一个点在所有坐标系之间的坐标变换。也就是说，只要给定一个坐标系下的一个点的坐标，就能获得这个点在其他任意坐标系的坐标。

为了达到上述目的，就需要提供当前 ROS 系统的中任意两个坐标系的位姿变换关系。TF 树的特点是每个节点只要一个父节点，即采用每个坐标系都有一个父坐标系，可以有多个子坐标系的原则。

4.3 TF中话题的发布

Topic通信机制发布的parent 坐标系到child坐标系的变换，并不是让每一对父子坐标系都发布一个话题，实际上发布的唯一个话题是****/topic****，该话题集合了所有发布的父子坐标系的变换关系。——(也就是说TF机制并不是让每一对父子坐标系都发布一个话题，而是将所有的父子坐标系都集合到到一个话题上，该话题的消息中传递的数据是所有父子坐标系的变换关系，是父子坐标系变换关系的一个大数组)

使用tf的tflisener就可以监听从任意两个坐标系的变换。（前提是TF的树上能把这两个联通）

基本原理是，tfbroadcastor的类里有个publisher，tflisener的类里有个subscriber，一个发布叫**/tf的topic，一个订阅这个topic，传送的消息message里包含了每一对parent frameid和child frameid的信息。这个机制意味着，所有的tb会发布某一特定的parent到child的变换，而所有tl会收到所有的这些变换，然后tl利用一个tfbuffercore的数据结构维护一个完整的树结构及其状态。基于此，tl在使用这棵树时，会用lookuptransform或waitfortransform**来获得任意坐标系之间的变换。

这样即使只要是一个 tflisener（即只监听两个坐标系的变换关系），就要跟所有 tfbroadcastor 建立连接，不仅要接收**/tf** 上的整个 TF 树，还要负责搜索这棵树，找到一条变换的路径，然后通过变换矩阵相乘得到两个坐标系最终的变换关系。