ROS TF变换

静态坐标转换：机器人本体中心到雷达中心的转换。因为激光雷达可能没安装到机器人的中心。

动态坐标转换：机器人中心和里程计坐标的变换。机器人从起点出发后，里程计坐标相对于本体就会产生一个偏移，这个偏移随着机器人的运动不断改变。

odme:里程计坐标系。

base_link :机器人的基体坐标系，与机器人的中心点重合。

base_link坐标系原点一般为机器人的旋转中心，base_footprint坐标系原点为base_link原点在地面的投影。

base_laser :传感器坐标系，例如激光雷达传感器。

map:地图坐标系，固定坐标系（fixed frame），与机器人所在的世界坐标系一致。

TF是可以让用户随时间跟踪多个坐标系的功能包。使用树形数据结构，根据时间缓冲，维护多个坐标系之间的坐标变换关系。

TF功能包提供了存储、计算不同数据在不同参考系之间变换的功能，只需要告诉tf树这些参考系之间的变换公式即可。

所有订阅TF消息的节点都会缓冲一份所有坐标系的变换关系数据。

有一篇文章介绍了移动机器人的控制结构，并且介绍了里程计：

base_controller节点将订阅的cmd_vel信息通过串口或其它通信接口发送给下位机（嵌入式控制板）。

下位机中根据机器人运动学公式进行解算，将机器人速度转换为每个轮子的速度。

然后通过CAN总线（或其它总线接口）将每个轮子的转速发送给电机驱动板控制电机转动。

电机驱动板对电机转速进行闭环控制（PID控制），并统计单位时间内接收到的编码器脉冲数，计算出轮子转速。

base_controller节点将接收到的cmd_vel速度信息转换为自定义的结构体或union类型的数据（自定义的数据类型中可以包含校验码等其它信息），并通过串口发送控制速度信息（speed_buf）或读取机器人传回的速度信息 (speed_buf_rev)。

base_controller节点正确读取到底层（比如嵌入式控制板）传回的速度后进行积分，计算出机器人的估计位置和姿态，并将里程计信息和tf变换发布出去。

单独使用里程计来估计小车位置姿态的效果不是特别好，因为轮子打滑。那个文章的作者提到，使用多传感器重合的方法，可以使位姿信息更加精准。具体方法是：使用IMU或其它传感器来同时进行测量，使用扩展卡尔曼滤波对imu和里程计的数据进行融合。

静态坐标转换

新建一个launch文件。

launch文件内容：调用tf功能包下的static_transform_publisher节点，几个参数分别是x，y，z三个方向的距离变换，航向，俯仰，横滚三个角度的变换。

x轴方向偏移0.1米，z轴方向偏移0.2米，/tf1/tf2表示参数描述的是tf2相对tf1的变换关系。20表示发布间隔20毫秒。

<launch>
 <node pkg="tf" type="static_transform_publisher" name="static_transform"
 args="0.1 0.0 0.2 0.0 0.0 0.0 /tf1 /tf2 20">
 </node>
</launch>

终端输入：

@robot:~$ roscore

@ubuntu:~$ roslaunch test1 tf_transform.launch

@ubuntu:~$ rosrun rqt_tf_tree rqt_tf_tree

打开RVIZ进行相应设置，可看到tf2相对tf1在x轴和z轴的偏移。

动态坐标转换

新建tf_transform.py文件。

#!/usr/bin/env python
import rospy
import tf
import math
def tf_transform():
    rospy.init_node('tf_transform', anonymous=False)
    tf_broadcaster = tf.TransformBroadcaster()
    rate = rospy.Rate(10) # 10hz
    angle = 0.0
    rospy.loginfo('Start TF Transform')
    while not rospy.is_shutdown():
        x = math.cos(angle)*0.3
        y = math.sin(angle)*0.3
        z = 0.2
        quat = tf.transformations.quaternion_from_euler(0,0,angle)
        current_time = rospy.Time.now()
        tf_broadcaster.sendTransform((x,y,z),quat,current_time,'tf3','tf1')
        tf_broadcaster.sendTransform((x,y,z),(0,0,0,1),current_time,'tf4','tf1')
        angle += 0.01
        rate.sleep()
if __name__ == '__main__':
 tf_transform()

@ubuntu:~/catkin_ws$ cd src/test1/src/
@ubuntu:~/catkin_ws/src/test1/src$ chmod +x tf_transform.py
@ubuntu:~/catkin_ws/src/test1/src$ rosrun test1 tf_transform.py
[INFO] [1645266508.071635]: Start TF Transform

这个节点会持续发布两个坐标tf3和tf4，绕着tf1以0.3为半径做圆周运动。tf4相对于tf1只做位置变换不做角度变换。tf3在位置变换的同时做角度变换。