ROS2工具之坐标变换TF ！！！！！

什么是坐标变换（TF）

在机器人系统中，会经常性的使用到“相对位置关系”这一概念，比如：机器人自身不同部件的相对位置关系，机器人与出发点的相对位置关系，传感器与障碍物的相对位置关系，机器人组队中不同机器人之间的相对位置关系等等。

毋庸置疑的，相对位置关系在机器人系统中有着重要的意义，那么在ros2中如何表述、使用相对位置关系呢？那就不得不提到ros2中强大的工具-----坐标变换（TF）

例子：

A是机器人的摄像头模块，B是机器人的夹爪模块，右边是一个苹果。从A的坐标系出发，能够知道苹果的坐标。A与B的相对关系也是知道。那我们现在要如何知道苹果与夹爪B之间的相对位置关系呢？

我们把A坐标系下的苹果坐标跟A与B坐标系的相对关系作为输入，输入到TF中，得到的输出就是B坐标系下的苹果坐标.

坐标系相对关系主要有两种：

静态坐标系相对关系与动态坐标系相对关系。

以一个移动小车加装了机械臂与激光雷达，摄像头为例子。车辆上的雷达、摄像头，他们的相对关系是指两个坐标系之间的相对位置是固定不变的，这就是静态坐标系相对关系。机械臂的关节或夹爪是可以运动的，那么机械臂的关节或夹爪坐标系相对车辆底盘坐标系或不同车辆坐标系的相对关系就是一种动态关系，这就是动态坐标系相对关系。

静态坐标系实现：

方法一：使用命令行实现静态广播

无论是静态坐标系相对关系，还是动态坐标系相对关系，都可以使用c++或者python进行广播。于此同时，静态坐标系相对关系还有一个很好的，很简单广播方法，就是使用命令行实现。步骤如下：

（1）在 tf2_ros功能包中提供了一个名为static_transform_publisher的可执行文件，通过该文件可以直接广播静态坐标系关系，其使用语法如下。

ros2 run tf2_ros static_transform_publisher
但是这个命令必须要加上参数，否则会报错。正确示例如下：

ros2 run tf2_ros static_transform_publisher -- frame-id base_link
-- child-frame-id laser

该命令的参数要求如下，必须要的参数是子坐标系与父坐标系。可选的参数是，描述相对关系的参数，如平移关系参数，旋转关系参数，旋转关系参数可以用四元数参数来描述，也可以使用欧拉角参数进行描述。所以如果不设置可选参数的话，就意味着父子坐标系是重合的

（2）打开新终端，输入rviz2，打开可视化界面，查看坐标系

ros2 run tf2_ros static_transform_publisher --x -0.5 --y 0 --z 0.4 --yaw 0 --roll 0 --pitch 0 --frame-id base_link --child-frame-id camera     #--yaw 0 --roll 0 --pitch 0表示什么

方法二：使用c++实现静态广播

 （1）创建工作空间，创建src

（2）终端下进入工作空间的src目录，调用如下两条命令分别创建C++功能包

ros2 pkg create cpp03_tf_broadcaster --build-type ament_cmake --dependencies rclcpp tf2 tf2_ros geometry_msgs turtlesim --node-name demo01_tf_static_broadcaster_cpp   #python版本

作用是创建一个c++版本的cpp03_tf_broadcaster 功能包，并依赖于rclcpp tf2   tf2_ros geometry_msgs    turtlesim，并创建一个名为demo01_tf_static_broadcaster_cpp的节点

相比与手动创建节点，--node-name demo01_tf_static_broadcaster_cpp   有如下好处，打开demo01_tf_static_broadcaster_cpp源文件进行编写，并编译配置环境变量执行即可

（3）打开demo01_tf_static_broadcaster_cpp源文件进行编辑

/*  需求：编写静态坐标变换程序，执行时传入两个坐标系的相对位姿关系以及父子级坐标系id，程序运行发布静态坐标变换。步骤：1.包含头文件；2.判断终端传入的参数是否合法；3.初始化 ROS 客户端；4.定义节点类；4-1.创建静态坐标变换发布方；4-2.组织并发布消息。5.调用 spin 函数，并传入对象指针；6.释放资源。*/// 1.包含头文件；
#include <geometry_msgs/msg/transform_stamped.hpp>#include <rclcpp/rclcpp.hpp>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/static_transform_broadcaster.h>using std::placeholders::_1;// 4.定义节点类；
class MinimalStaticFrameBroadcaster : public rclcpp::Node
{
public:explicit MinimalStaticFrameBroadcaster(char * transformation[]): Node("minimal_static_frame_broadcaster"){// 4-1.创建静态坐标变换发布方；tf_publisher_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);this->make_transforms(transformation);}private:// 4-2.组织并发布消息。void make_transforms(char * transformation[]){// 组织消息geometry_msgs::msg::TransformStamped t;rclcpp::Time now = this->get_clock()->now();t.header.stamp = now;t.header.frame_id = transformation[7];t.child_frame_id = transformation[8];t.transform.translation.x = atof(transformation[1]);t.transform.translation.y = atof(transformation[2]);t.transform.translation.z = atof(transformation[3]);tf2::Quaternion q;q.setRPY(atof(transformation[4]),atof(transformation[5]),atof(transformation[6]));t.transform.rotation.x = q.x();t.transform.rotation.y = q.y();t.transform.rotation.z = q.z();t.transform.rotation.w = q.w();// 发布消息tf_publisher_->sendTransform(t);}std::shared_ptr<tf2_ros::StaticTransformBroadcaster> tf_publisher_;
};int main(int argc, char * argv[])
{// 2.判断终端传入的参数是否合法；auto logger = rclcpp::get_logger("logger");if (argc != 9) {RCLCPP_INFO(logger, "运行程序时请按照：x y z roll pitch yaw frame_id child_frame_id 的格式传入参数");return 1;}// 3.初始化 ROS 客户端；rclcpp::init(argc, argv);// 5.调用 spin 函数，并传入对象指针；rclcpp::spin(std::make_shared<MinimalStaticFrameBroadcaster>(argv));// 6.释放资源。rclcpp::shutdown();return 0;
}

（3）打开package.xml进行依赖项配置：
由于创建功能包时已经添加了依赖项，所以无需手动配置。CMakeLists.txt中也自动做好了配置

（4）编译，执行

方法三：使用python实现静态广播

 （1）创建工作空间，创建src

（2）终端下进入工作空间的src目录，调用如下两条命令分别创建Python功能包。

ros2 pkg create py03_tf_broadcaster --build-type ament_python --dependencies rclpy tf_transformations tf2_ros geometry_msgs turtlesim --node-name demo01_tf_static_broadcaster_py   #python版本

作用是创建一个pyhton版本的py03_tf_broadcaster功能包，并依赖于rclpy tf_transformations tf2_ros geometry_msgs turtlesim，并创建一个名为demo01_tf_static_broadcaster_py的节点

相比与手动创建节点，--node-name demo01_tf_static_broadcaster_py 有如下好处，打开demo01_tf_static_broadcaster_py源文件进行编写，并编译配置环境变量执行即可

（3）具体的步骤详见5.3.3 静态广播器（C++） · GitBookhttps://zhangzhiwei-zzw.github.io/ROS2%E5%AD%A6%E4%B9%A0/ROS2/di-5-zhang-ros2-gong-ju-zhi-zuo-biao-bian-huan/53-zuo-biao-xi-guang-bo/533-jing-tai-guang-bo-qi-ff08-c-++.html

静态广播器的使用，我们一般都是用命令行实现，所以c++与python不多赘述，有需要再自行学习

动态坐标系实现：

方法一：python实现

（1）功能包 py03_tf_broadcaster 的 py03_tf_broadcaster 目录下，新建 Python 文件 demo02_dynamic_tf_broadcaster_py.py，并编辑文件，输入如下内容：

"""  需求：编写动态坐标变换程序，启动 turtlesim_node 以及 turtle_teleop_key 后，该程序可以发布乌龟坐标系到窗口坐标系的坐标变换，并且键盘控制乌龟运动时，乌龟坐标系与窗口坐标系的相对关系也会实时更新。步骤：1.导包；2.初始化 ROS 客户端；3.定义节点类；3-1.创建动态坐标变换发布方；3-2.创建乌龟位姿订阅方；3-3.根据订阅到的乌龟位姿生成坐标帧并广播。4.调用 spin 函数，并传入对象；5.释放资源。
"""
# 1.导包；
from geometry_msgs.msg import TransformStamped  #用于构建包含时间戳、坐标系ID和变换数据（平移和旋转）的消息
import rclpy
from rclpy.node import Node
from tf2_ros import TransformBroadcaster  #负责将坐标变换消息发送到ROS 2系统中的核心工具
import tf_transformations   #提供用于处理三维旋转（如在欧拉角和四元数之间转换）的函数。
from turtlesim.msg import Pose  #来自turtlesim节点的消息类型，包含乌龟的位置（x, y）和朝向（theta）。# 3.定义节点类；
class MinimalDynamicFrameBroadcasterPy(Node):def __init__(self):super().__init__('minimal_dynamic_frame_broadcaster_py')# 3-1.创建动态坐标变换发布方；self.br = TransformBroadcaster(self)   #实例化一个TransformBroadcaster，用于发布坐标变换# 3-2.创建乌龟位姿订阅方；self.subscription = self.create_subscription(Pose,  #指定订阅者期望接收的消息数据类型是 geometry_msgs.msg.Pose'/turtle1/pose',   #指定监听的话题是/turtle1/poseself.handle_turtle_pose,   #乌龟位置更新时，调用回调函数self.handle_turtle_pose1)        #定义了一个大小为1的缓存队列，用于暂存来不及即时处理的消息# 3-3.根据订阅到的乌龟位姿生成坐标帧并广播。def handle_turtle_pose(self, msg):# 组织消息t = TransformStamped() # 创建TransformStamped消息对象t.header.stamp = self.get_clock().now().to_msg() # 设置时间戳.设置为当前时间，确保TF系统使用最新的位姿。t.header.frame_id = 'world' # 父坐标系t.child_frame_id = 'turtle1' # 子坐标系（乌龟自身的坐标系）t.transform.translation.x = msg.x # 设置x方向平移t.transform.translation.y = msg.y # 设置y方向平移t.transform.translation.z = 0.0   # z方向无移动q = tf_transformations.quaternion_from_euler(0, 0, msg.theta) # 将乌龟的朝向角（欧拉角）转换为四元数,使用四元数可以避免死锁问题t.transform.rotation.x = q[0] # 设置四元数x分量t.transform.rotation.y = q[1] # 设置四元数y分量t.transform.rotation.z = q[2] # 设置四元数z分量t.transform.rotation.w = q[3] # 设置四元数w分量# 发布消息self.br.sendTransform(t) # 广播变换消息.将构建好的变换消息广播出去，使ROS 2系统中的其他节点可以查询到turtle1相对于world的位置和姿态
。def main():# 2.初始化 ROS 客户端；rclpy.init()# 4.调用 spin 函数，并传入对象；node = MinimalDynamicFrameBroadcasterPy()try:rclpy.spin(node)except KeyboardInterrupt:pass# 5.释放资源。rclpy.shutdown()"""
队列长度是ROS 2中​​服务质量（QoS）​​ 策略的一部分
。将其设置为 1意味着消息队列只能容纳​​1条​​消息。它的工作方式如下：
​​当新消息到达时​​，如果回调函数 self.handle_turtle_pose正在忙碌地处理上一条消息，还未能接收新消息，那么这条新消息会被放入队列中等待。
​​当队列已满（即已有一条消息在等待）时，又来了第二条新消息​​：由于队列长度是1，​​最早进入队列的那条旧消息会被丢弃​​，新的消息会取代它的位置
。
这样做的目的是确保你的节点​​总是处理它能获取到的最新数据​​，而不是按顺序处理每一条可能已经“过时”的消息。这对于实时性要求高的应用（比如控制乌龟运动）非常有用，可以避免因处理速度跟不上发布速度而导致的严重延迟
。
"""

（2）进入package.xml配置依赖项： 由于在创建功能包的时候已经配置过了，所以无需手动配置

（3）配置入口程序，将您编写的 Python函数注册为可以通过命令行直接调用的节点可执行文件​​

entry_points={'console_scripts': ['demo01_static_tf_broadcaster_py = py03_tf_broadcaster.demo01_static_tf_broadcaster_py:main','demo02_dynamic_tf_broadcaster_py = py03_tf_broadcaster.demo02_dynamic_tf_broadcaster_py:main'],
},

（4）编译：colcon build --packages-select py03_tf_broadcaster

（5）执行：

. install/setup.bashros2 run turtlesim turtlesim_noderos2 run turtlesim turtle_teleop_keyros2 run py03_tf_broadcaster demo02_dynamic_tf_broadcaster_pyrviz2   #打开rviz看看坐标系相对关系，并配置 rviz2（Global Options 中的 Fixed Frame 设置为 world），通过键盘控制乌龟运动，最终执行结果与案例2类似。

方法二：c++实现

有需要再学

坐标点发布案例以及分析

案例：无人车上安装有激光雷达，现激光雷达扫描到一点状障碍物并且可以定位障碍物的坐标，请在雷达坐标系下发布障碍物坐标点数据，并在 rviz2 中查看发布结果。

上述案例，是一个简单的话题发布程序，在了解坐标点geometry_msgs/msg/PointStamped接口消息之后，直接通过话题发布方按照一定逻辑发布消息即可。

（1）功能包 py03_tf_broadcaster 的 py03_tf_broadcaster 目录下，新建 Python 文件 demo03_point_publisher_py.py，并编辑文件，输入如下内容：

（2）编写源码

"""  需求：发布雷达坐标系中某个坐标点相对于雷达（laser）坐标系的位姿。步骤：1.导包；2.初始化 ROS 客户端；3.定义节点类；3-1.创建坐标点发布方；3-2.创建定时器；3-3.组织并发布坐标点消息。4.调用 spin 函数，并传入对象；5.释放资源。
"""
# 1.导包；
from geometry_msgs.msg import PointStamped
import rclpy
from rclpy.node import Node# 3.定义节点类；
class MinimalPointPublisher(Node):#初始化函数def __init__(self):super().__init__('minimal_point_publisher_py')   #让MinimalPointPublisher子类调用其父类的构造函数，完成节点初始化# 3-1.创建坐标点发布方；self.pub = self.create_publisher(PointStamped, 'point', 10)# 3-2.创建定时器；self.timer = self.create_timer(1.0, self.on_timer)self.x = 0.1def on_timer(self):# 3-3.创建坐标点消息PointStamped，并填写发布ps = PointStamped()    #创建PointStamped接口类型的消息ps#根据接口类型填写消息细节ps.header.stamp = self.get_clock().now().to_msg()ps.header.frame_id = 'laser'self.x += 0.02ps.point.x = self.xps.point.y = 0.0ps.point.z = 0.2self.pub.publish(ps)   #发布PointStamped接口类型的消息psdef main():# 2.初始化 ROS 客户端；rclpy.init()# 4.调用 spin 函数，并传入对象；node = MinimalPointPublisher()rclpy.spin(node)# 5.释放资源。3-3.组织并发布坐标点消息。

（3）package.xml 无需修改，需要修改 setup.py 文件，将源码编译为可执行文件

（4）打开终端编译，配置环境变量，运行命令发布雷达（laser）相对于底盘（base_link）的静态坐标变换：
ros2 run py03_tf_broadcaster demo01_static_tf_broadcaster_py 0.4 0 0.2 0 0 0 base_link laser

（5）打开终端编译，配置环境变量，运行命令发布障碍物相对于雷达（laser）的坐标点：

ros2 run py03_tf_broadcaster demo03_point_publisher_py

（6）打开rviz2，设置根，打开tf

（7）打开pointstamped

坐标变换监听

监听可以实现坐标点在不同坐标系之间的变换，或者不同坐标系之间的相互变换。当然，前提是必须要广播出不同坐标系关系，而至于是静态广播或动态广播则无要求。

坐标系相互转换：

（1）在src下创建功能包：

ros2 pkg create py04_tf_listener --build-type ament_python --dependencies rclpy tf_transformations tf2_ros geometry_msgs --node-name demo01_tf_listener py

（2）编写源代码：

"""  需求：订阅 laser 到 base_link 以及 camera 到 base_link 的坐标系关系，并生成 laser 到 camera 的坐标变换。作用：这段代码实现了一个​​ROS 2坐标系变换监听器（TF Listener）​​，它的核心功能是动态查询并计算两个已知坐标系（laser和 camera）之间的直接变换关系，即使它们之间没有直接的广播关系步骤：1.导包；2.初始化 ROS 客户端；3.定义节点类；3-1.创建tf缓存对象指针；3-2.创建tf监听器；3-3.按照条件查找符合条件的坐标系并生成变换后的坐标帧。4.调用 spin 函数，并传入对象指针；5.释放资源。
"""# 1.导包；
import rclpy
from rclpy.node import Node
from tf2_ros.buffer import Buffer   # 核心: TF变换缓存区，存储所有坐标系关系，是一个不断更新的“坐标系地图数据库”
from tf2_ros.transform_listener import TransformListener   ## 核心: 监听器：订阅TF话题，监听所有坐标变换关系，并填充Buffer
from rclpy.time import Time# 3.自定义节点类；
class TFListenerPy(Node):def __init__(self):super().__init__("tf_listener_py_node_py")# 3-1.创建一个缓存对象，融合多个坐标系相对关系为一棵坐标树；self.buffer = Buffer()# 3-2.创建一个监听器，绑定一个buffer缓存对象，会将所有广播器广播的数据写入缓存；self.listener = TransformListener(self.buffer,self)# 3-3.创建定时器，每1.0秒触发一次on_timer回调函数self.timer = self.create_timer(1.0,self.on_timer)def on_timer(self):# 判断是否可以实现转换if self.buffer.can_transform("camera","laser",Time()):  #Time()或者Time(0)表示查询最新可用​​的变换。TF系统会返回缓存中时间戳最接近该请求时刻的变换数据。#ts是存储了指定时间点time（）下从源坐标系（laser）到目标坐标系（camera）的变换关系的接口消息，存储了父坐标系,子坐标系，与偏移量ts = self.buffer.lookup_transform("camera","laser",Time())  #​​lookup_transform方法​​：这是最关键的API调用。参数顺序是 ​​(目标坐标系，源坐标系，时间点)​​。它从 Buffer中查找在指定时间点time（）下从源坐标系（laser）到目标坐标系（camera）的变换，并且函数内部会基于TF树自动计算复合变换self.get_logger().info("-------转换后的数据-------")self.get_logger().info("转换的结果，父坐标系:%s,子坐标系:%s,偏移量:(%.2f,%.2f,%.2f)"% (ts.header.frame_id,ts.child_frame_id,ts.transform.translation.x,ts.transform.translation.y,ts.transform.translation.z))else:self.get_logger().info("转换失败......")def main():# 2.初始化ROS2客户端；rclpy.init()# 4.调用spain函数，并传入节点对象；rclpy.spin(TFListenerPy())# 5.资源释放。 rclpy.shutdown()if __name__ == '__main__':main()

（2）package.xml配置依赖项：无需配置，在创建功能包时已经配置好依赖项了

（3） 配置setup.py文件：

'demo01_tf_listener_py = py04_tf_listener.demo01_tf_listener_py:main'

（4）编译：

colcon build --packages-select py04_tf_listener

（5）配置环境变量

发布雷达（laser）相对于底盘（base_link）的静态坐标变换：

ros2 run py03_tf_broadcaster demo01_static_tf_broadcaster_py 0.4 0 0.2 0 0 0 base_link laser

再发布摄像头（camera）相对于底盘（base_link）的静态坐标变换：

ros2 run py03_tf_broadcaster demo01_static_tf_broadcaster_py -0.5 0 0.4 0 0 0 base_link camera

执行坐标系变换得到 laser 相对于 camera 的坐标：

ros2 run py04_tf_listener demo01_tf_listener_py

坐标点相互转换：

c++暂时不学

python版本，ros2暂无提供该功能

TF提供的一些高效工具

在 ROS2 的 TF 框架中除了封装了坐标广播与订阅功能外，还提供了一些非常好用的工具，这些工具主要涉及到两个功能包：tf2_ros和tf2_tools。

tf2_ros包中提供的常用节点如下：

• static_transform_publisher：该节点用于广播静态坐标变换；

• tf2_monitor：该节点用于打印所有或特定坐标系的发布频率与网络延迟；

• tf2_echo：该节点用于打印特定坐标系的平移旋转关系。

tf2_tools包中提供的节点如下：

• view_frames：该节点可以生成显示坐标系关系的 pdf 文件，该文件包含树形结构的坐标系图谱。

ros2 run tf2_ros tf2_monitor   #打印所有坐标系的发布频率与网络延迟ros2 run tf2_ros tf2_monitor camera laser   #打印指定坐标系的发布频率与网络延迟ros2 run tf2_ros tf2_echo world turtle1   #打印两个坐标系的平移旋转关系ros2 run tf2_tools view_frames   #以图形化的方式显示坐标系关系。将会生成 frames_xxxx.gv 与 frames_xxxx.pdf 文件，其中 xxxx 为时间戳。