ROS tf2 实现
概述
因为曾经考虑将 ROS 的tf2 移植到其他环境中使用。因此过了一下 ROS 的 tf2 代码。在这里做一下总结。 关于 tf2 的介绍可以查看 ROS 网站。
我们先从使用的方法作为入口来看。(这里只考虑tf，暂不考虑tf_static, 二者的不同也很好理解，后面再说)。 使用 tf2 主要有一下三步：
- 创建坐标变换发布器 Broadcaster，以某种方式不断发布坐标信息。坐标信息主要包括四要素: 父坐标，子坐标， 平移和旋转变换，时间。
- 需要获取坐标变换的地方创建坐标订阅器。该订阅器需要一个buffer，该buffer中会为每一个frame设置一个双向 队列, 队列按照时间顺序保存转换到 child_frame 的所有转换。默认保存10s内的变换。
- 需要使用变换的时候通过buffer调用lookupTransform，来查询两个坐标之间的某个时刻的变换。包含三个要素： 父坐标，子坐标，时刻。
实现
这里就从上面的三个部分来将实现过程。最后简单说一下 tf_static。
tf broadcaster
坐标变换发布器比较简单，就是组合父坐标和子坐标之间当前时刻的变换信息发布到 "/tf" 或者 "/tf_static" 话 题上去。注意这个消息包含四要素：父坐标，子坐标，平移和旋转变换，时间。
tf listener
在需要使用变换的地方实例化一个订阅器 TransformListener，并定义一个 tf2_ros::Buffer 类型的buffer，用来 保存一段时间的坐标变换信息。在订阅器 TransformListener 的构造函数中，会订阅两个topic: "/tf", "/tf_static"， 并根据你的需要，设置是否需要单独的线程来处理这些订阅(默认是开启单独一个线程来处理订阅的).
当收到这两个话题上来的变换信息时，会将起保存到buffer中去。这个buffer会为每个frame建立一个双向队列，保存 所有子坐标是该frame的变换。这些信息都是按照时间顺序保存的，越靠近begin位置的消息越是最新的。每次保存之后 都会清理一遍整个队列，将与最新的消息时间差超过10s的数据清除掉。
lookupTransform
当需要用到某两个坐标之间的变换时可以调用buffer的接口 lookupTransform 来查询。包含三个要素：父(source)坐标， 子(target)坐标，时刻(查询离该时刻最近的转换，如果为0表示取最新的一个转换)。
查询过程主要分两步，首先是确定两个坐标的共同时间，然后计算两个坐标之间的平移和旋转。
由于/tf中得到的可能是其他所有坐标到该坐标的转换。因此source到target的转换可能需要经过几个坐标才能计算出来。 因此共同时间的确定是选择source和target坐标经过的所有转换的时间的最小值。具体步骤是：
- 定义一个map，key是本次查找的frame的父坐标，value是该转换对应的时间戳。
- 从buffer中属于source frame的队列中选择时间最新的转换(这里只考虑查询时刻为0，即最近时刻的转换)，找到父坐标 和时间，写入map中。如果父坐标不是target frame，则继续在该父坐标的队列中找最新的一次转换的父坐标和时间，将 他们也写进map中。以此类推，直到父坐标是target frame或者找到root frame为止。
- 如果上面一直找到root frame也没有找到target frame，那就从target frame使用上面的方法查找，每次找到父坐标和 map中保存的父坐标比较，如果包含在其中，说明找到的就是source和target的共同祖先坐标。选择最小的时间作为共同 时间即可。
- 根据上面找到的信息计算平移和旋转即可。最后得到我们要的变换信息。
tf 和 tf_static
上面我们看出在接受到tf的转换之后在buffer中为子坐标设置双向队列。但是对于tf_static则不用设置双向队列，而是只 保存一条变换。也就是说tf_static不会保存一段时间的变换，而只是保存最近的一条变换信息。其实tf_static主要是用来 发布固定的坐标系之间的变换，因为这样的变换是不常变化的，没有必要设置队列来保存。
以上就是ros中tf主要的内容。