【ROS2】初见ROS2中的TF2变换

Z-YQ

已于 2024-06-15 00:06:34 修改

阅读量1.4k

点赞数 22

分类专栏： RL & ROS2 Chronicles 文章标签：人工智能机器学习算法

于 2024-05-29 16:54:03 首次发布

本文链接：https://blog.csdn.net/weixin_41434829/article/details/139300158

版权

RL & ROS2 Chronicles 专栏收录该内容

2 篇文章 0 订阅

订阅专栏

1、TF2的使用场景

在ROS 2中，TF2（Transform Library）是一个用于管理和转换坐标系的库。TF2可以帮助机器人应用程序在多个坐标系之间进行变换和同步，从而确保传感器数据和运动控制指令在正确的坐标系下进行操作。TF2对于管理和转换机器人系统中的坐标系至关重要，以下是TF2的一些典型使用场景：

多传感器融合：

在机器人系统中，通常会使用多个传感器，如LiDAR、摄像头和IMU。这些传感器的数据通常需要转换到一个统一的坐标系下，以便进行数据融合和环境建模。TF2可以管理这些传感器的坐标系变换，确保数据在统一的参考系下进行处理。
导航和SLAM：

在自主导航和SLAM（同步定位与地图构建）中，机器人需要不断地在环境中定位自己，并构建环境地图。TF2可以帮助管理机器人自身的坐标系（如base_link）和地图坐标系（如map）之间的变换，从而实现精确的定位和导航。
机械臂控制：

对于机械臂的控制，TF2可以用于管理机械臂各个关节之间的坐标系变换。通过TF2，可以实时计算机械臂末端执行器（end-effector）的位置和姿态，进而实现精确的运动控制和路径规划。
视觉系统：

在机器人视觉系统中，摄像头采集到的图像数据需要转换到机器人的坐标系下，以便进行目标检测和识别。TF2可以帮助将摄像头坐标系的数据转换到机器人基座坐标系，从而进行更精确的视觉处理。
仿真与现实世界同步：

在仿真环境中，TF2可以用于管理虚拟传感器和虚拟机器人之间的坐标系变换。通过在仿真中使用TF2，可以方便地将仿真结果与现实世界的坐标系进行对比和同步。
多机器人系统：

在多机器人系统中，每个机器人都有自己的坐标系，且需要在彼此之间进行协调。TF2可以帮助管理这些机器人之间的相对位置和姿态变换，从而实现多机器人协作和任务分配。

在这些使用场景中，TF2通过其高效的坐标系管理和变换功能，为机器人系统提供了关键的基础支持，确保各类传感器数据和控制指令在正确的坐标系下进行有效的处理和执行。

2、TF2 的主要功能

TF2可以帮助机器人知道不同部位和传感器之间的位置关系，并且可以随着时间的变化进行更新。以下是 TF2 的主要功能：

管理多个坐标系：

想象一下，一个机器人有很多部件，每个部件都有自己的位置和方向，比如头部、手臂、摄像头等。TF2 就像一个大管家，管理这些部件的坐标系，知道每个部件在哪里，朝哪个方向。
处理时间戳：

机器人在不同时刻采集的数据可能不同，TF2 允许我们记录每个坐标变换发生的具体时间。这样，即使数据是从不同时间点获取的，也能正确地转换和使用这些数据。
构建树结构：

坐标系之间的关系可以想象成一棵树，比如机器人身体是根节点，手臂是分支，手掌是手臂的分支。这种树结构帮助我们理清楚各个坐标系之间的层级关系。
插值和外推：

有时候我们需要知道在某个特定时间点的坐标变换，但没有直接的变换数据。这时候，TF2 可以通过已有的数据进行插值（在两个已知点之间估算）或外推（根据已有趋势推测未来），来提供我们需要的变换。
广播变换：

当机器人某个部件的位置发生变化时，比如手臂移动了，TF2 会通过广播把这个信息告诉其他部件。这样，大家都能及时知道最新的坐标信息。
监听变换：

机器人上的各个部件和传感器需要知道其他部件的位置信息时，就像在听广播一样，TF2 会让它们“收听”到最新的坐标变换信息，方便它们进行协调和合作。

简单例子

假设你有一个机器人，有一个底座和一个可以旋转的手臂，手臂上有一个摄像头。

广播变换：当手臂旋转时，TF2 会告诉其他部件“手臂现在旋转到了新的角度”。

监听变换：摄像头需要知道手臂的位置和方向，这样它才能正确地计算出拍摄的图像在机器人坐标系中的位置。

通过 TF2，机器人可以有效地管理和使用各种坐标系的信息，确保各个部件能够协同工作，做出准确的动作和判断。

3、常用的 TF2 工具

在 TF2 中，有几个常用的工具帮助我们管理和使用坐标系。这些工具类似于日常生活中的一些助手，帮助我们完成各种任务。

tf2_ros.Buffer（缓冲区）：
- 什么是 Buffer？
  
  想象一下，你有一个记事本，用来记录各种坐标变换的信息。Buffer 就是这样的一个记事本，保存了所有的坐标变换数据。
- 它的作用是什么？
  
  当你需要知道某个时刻某个部件的位置时，可以翻开这个记事本，找到对应的记录。
tf2_ros.TransformBroadcaster（变换广播器）：
- 什么是 TransformBroadcaster？
  
  想象一下一个电台，负责向外广播信息。TransformBroadcaster 就是这样的电台，用来广播坐标变换的信息。
- 它的作用是什么？
  
  当机器人某个部件的位置发生变化时，这个工具会把变换信息广播出去，让所有需要这些信息的部件都能收到。
tf2_ros.TransformListener（变换监听器）：
- 什么是 TransformListener？
  
  想象一下一个收音机，负责接收和监听广播信息。TransformListener 就是这样的收音机，用来接收和处理变换信息。
- 它的作用是什么？
  
  机器人上的部件需要知道其他部件的位置时，这个工具会帮助它们接收广播的变换信息，进行相应的调整和操作。

好的，我来重新用通俗的语言解释ROS 2 tf2静态广播的作用，并提供一个示例代码。

4、TF2中所用到的消息类型

4.1 from geometry_msgs.msg import TransformStamped

TransformStamped 是 ROS（机器人操作系统）中的一个消息类型，用于表示在特定时间点两个坐标系之间的变换。它属于 geometry_msgs 包，该包包含常见几何原语的消息，如点、向量和姿态。

定义

TransformStamped 消息的定义如下：

Header header
string child_frame_id
geometry_msgs/Transform transform

以下是对各个字段的详细解释：

Header header:
- header 包含两个信息：时间戳和坐标系的父帧。
- Header 的具体定义为：
```
uint32 seq
time stamp
string frame_id
```
  - seq 是消息的序列号（通常不手动设置，由系统自动管理）。
  - stamp 是时间戳，表示变换的时间点。
  - frame_id 是坐标系的父帧 ID。
string child_frame_id:
- child_frame_id 是子坐标系的 ID，表示该变换的目标坐标系。
geometry_msgs/Transform transform:
- transform 包含变换的信息，表示从父帧到子帧的变换。
- Transform 的具体定义为：
```
geometry_msgs/Vector3 translation
geometry_msgs/Quaternion rotation
```
  - translation 是平移向量，表示从父坐标系原点到子坐标系原点的平移。
  - rotation 是四元数，表示从父坐标系到子坐标系的旋转。

4.2 from geometry_msgs.msg import PointStamped

geometry_msgs.msg.PointStamped 是 ROS（机器人操作系统）中定义的一种消息类型，用于表示带有时间戳和坐标框架标识的点。它在传输空间坐标信息时非常有用，尤其是在需要记录某个点在特定时间和坐标框架下的位置时。

`PointStamped` 消息的结构

PointStamped 消息包含两个主要部分：

std_msgs/Header header:
- header.stamp: 这个字段表示消息的时间戳，是 ros::Time 类型，记录了消息的发送时间。
- header.frame_id: 这个字段是一个字符串，表示坐标框架的名称。它用于指定点所在的坐标系。
geometry_msgs/Point point:
- point.x: 点在 X 轴上的坐标，类型为浮点数（float64）。
- point.y: 点在 Y 轴上的坐标，类型为浮点数（float64）。
- point.z: 点在 Z 轴上的坐标，类型为浮点数（float64）。

`PointStamped` 消息的定义

以下是 PointStamped 消息的定义：

std_msgs/Header header
geometry_msgs/Point point

header 是标准消息头，包含时间戳和坐标框架信息。
point 是一个三维点，包含 x、y、z 坐标。

geometry_msgs.msg.PointStamped 是 ROS 中用于表示带有时间戳和坐标框架的点的消息类型，常用于需要记录点在特定时间和坐标框架下的位置的场景。在实际应用中，可以使用该消息类型在不同坐标系之间传输空间坐标信息。

4.3 from tf2_geometry_msgs import do_transform_point

do_transform_point 函数来自 ROS 2 的 tf2_geometry_msgs 模块，用于使用指定的变换将一个点从一个坐标系转换到另一个坐标系。这个函数在你需要将一个点在一个坐标系中的位置转换到另一个坐标系中的位置时非常有用。

函数签名

do_transform_point(point, transform)

参数

point：类型为 geometry_msgs.msg.PointStamped，表示要进行变换的点。PointStamped 包含一个 geometry_msgs.msg.Point（点的位置）和一个 std_msgs.msg.Header（包含坐标系信息和时间戳）。
transform：类型为 geometry_msgs.msg.TransformStamped，表示从一个坐标系到另一个坐标系的变换。TransformStamped 包含一个 geometry_msgs.msg.Transform（平移和旋转信息）和一个 std_msgs.msg.Header（包含目标坐标系信息和时间戳）。

返回值

返回一个 geometry_msgs.msg.PointStamped，表示在目标坐标系中的点。

5、TF2树

什么是TF2树

在机器人系统中，tf2树是一个用来表示不同部件之间相对位置和姿态关系的结构。tf2树将各种坐标系（例如机器人底盘、激光雷达、相机等）连接在一起，形成一个树状结构，这样我们就可以在任何两个坐标系之间进行转换。

为什么需要TF2树

机器人通常有多个传感器和部件，每个部件都有自己的坐标系。为了让机器人能够理解这些传感器的数据并进行合理的操作，我们需要知道这些坐标系之间的相对位置和方向。tf2树帮助我们管理这些关系，并自动计算任意两个坐标系之间的转换。

TF2树的基本概念

坐标系（Frame）：一个坐标系是一个特定的参考点，通常与机器人或传感器的某个部分相关联。
变换（Transform）：一个变换描述了两个坐标系之间的相对位置和姿态。它包括平移（位置）和旋转（方向）。
父子关系（Parent-Child Relationship）：在tf2树中，每个坐标系都有一个父坐标系，这些父子关系形成了一棵树。

TF2树的工作原理

当我们广播或监听tf2变换时，这些变换信息被组织成一棵树。例如，假设我们有一个机器人底盘（base_link），其上安装了一个激光雷达（laser）和一个相机（camera）。tf2树可能看起来像这样：

base_link
 ├── laser
 └── camera

在这个例子中，base_link是根坐标系（父节点），laser和camera是其子坐标系。我们可以广播从base_link到laser和camera的变换。然后，当我们需要知道激光雷达数据在相机坐标系中的位置时，tf2树可以自动计算从laser到camera的变换。

TF2树的作用

管理和组织坐标系：tf2树帮助我们有条理地管理和组织机器人系统中的各个坐标系。
自动计算变换：tf2树可以自动计算任意两个坐标系之间的变换，简化了开发工作。
实时更新和查询：tf2树支持实时更新和查询变换信息，确保我们总是获取最新的坐标系关系。

示例代码

以下是一个简单的示例，展示如何在ROS 2中使用tf2树来广播和监听变换。

广播节点

import rclpy
from rclpy.node import Node
from tf2_ros.transform_broadcaster import TransformBroadcaster
from geometry_msgs.msg import TransformStamped
import tf_transformations

class TransformBroadcasterNode(Node):
    def __init__(self):
        super().__init__('transform_broadcaster_node')
        
        # 创建变换广播器
        self._broadcaster = TransformBroadcaster(self)
        
        # 创建定时器，每秒调用一次回调函数发布变换
        self.timer = self.create_timer(1.0, self.broadcast_transform)

    def broadcast_transform(self):
        # 创建一个TransformStamped消息来存储变换信息
        transform_stamped = TransformStamped()
        transform_stamped.header.stamp = self.get_clock().now().to_msg()
        transform_stamped.header.frame_id = 'base_link'
        transform_stamped.child_frame_id = 'laser'

        # 设置变换的平移部分，假设激光雷达在底盘前方20厘米，高度为10厘米
        transform_stamped.transform.translation.x = 0.2
        transform_stamped.transform.translation.y = 0.0
        transform_stamped.transform.translation.z = 0.1

        # 设置变换的旋转部分，这里假设没有旋转，即四元数(0, 0, 0, 1)
        quat = tf_transformations.quaternion_from_euler(0, 0, 0)
        transform_stamped.transform.rotation.x = quat[0]
        transform_stamped.transform.rotation.y = quat[1]
        transform_stamped.transform.rotation.z = quat[2]
        transform_stamped.transform.rotation.w = quat[3]

        # 发送变换
        self._broadcaster.sendTransform(transform_stamped)

def main(args=None):
    rclpy.init(args=args)
    node = TransformBroadcasterNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

监听节点

import rclpy
from rclpy.node import Node
from tf2_ros import TransformListener, Buffer
from geometry_msgs.msg import PointStamped
import tf2_geometry_msgs

class TransformListenerNode(Node):
    def __init__(self):
        super().__init__('transform_listener_node')

        # 创建一个Buffer对象来存储变换
        self.tf_buffer = Buffer()
        
        # 创建TransformListener来监听变换
        self.tf_listener = TransformListener(self.tf_buffer, self)
        
        # 模拟一个激光雷达数据点（在激光雷达坐标系下）
        self.laser_point = PointStamped()
        self.laser_point.header.frame_id = 'laser'
        self.laser_point.point.x = 1.0
        self.laser_point.point.y = 0.0
        self.laser_point.point.z = 0.0
        
        # 创建定时器，每秒调用一次回调函数
        self.timer = self.create_timer(1.0, self.on_timer)
        
    def on_timer(self):
        try:
            # 查询从'laser'到'base_link'的变换
            transform = self.tf_buffer.lookup_transform('base_link', 'laser', rclpy.time.Time())
            
            # 将激光雷达坐标系下的点转换到底盘坐标系下
            transformed_point = tf2_geometry_msgs.do_transform_point(self.laser_point, transform)
            
            # 打印转换后的点
            self.get_logger().info(f'Transformed point: ({transformed_point.point.x}, {transformed_point.point.y}, {transformed_point.point.z})')
        
        except Exception as e:
            self.get_logger().warn(f'Could not transform laser point: {e}')

def main(args=None):
    rclpy.init(args=args)
    node = TransformListenerNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

总结

通过tf2树，我们可以有效地管理机器人系统中的坐标系，简化坐标转换的工作，并确保我们能够实时获取和使用最新的坐标变换信息。这个示例代码展示了如何在ROS 2中广播和监听tf2变换，并利用这些变换进行坐标转换。

6、TF2静态广播

静态广播器用于定义那些不会改变的变换。例如，如果你的传感器是固定在机器人底盘上的，它们之间的相对位置和方向不会变化，就可以用静态广播器来描述这个变换。

作用

定义固定变换：如果某些部件之间的相对位置不会变化，可以用静态广播器一次性定义这个变换。
减少计算量：静态变换只需要广播一次，节省了计算资源和通信带宽。
简化系统：通过静态广播器，其他节点可以轻松获取这些固定变换，简化了开发工作。

示例代码

假设我们有一个机器人，激光雷达固定在底盘的前面，我们要定义底盘到激光雷达的固定变换。下面是一个用Python编写的ROS 2节点示例：

import rclpy
from rclpy.node import Node
from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster
from geometry_msgs.msg import TransformStamped
import tf_transformations

class StaticTransformPublisher(Node):
    def __init__(self):
        super().__init__('static_transform_publisher')
        
        # 创建静态变换广播器
        self._broadcaster = StaticTransformBroadcaster(self)

        # 创建一个TransformStamped消息来存储变换信息
        static_transform_stamped = TransformStamped()
        static_transform_stamped.header.stamp = self.get_clock().now().to_msg()
        static_transform_stamped.header.frame_id = 'base_link'
        static_transform_stamped.child_frame_id = 'laser'

        # 设置变换的平移部分，假设激光雷达在底盘前方20厘米，高度为10厘米
        static_transform_stamped.transform.translation.x = 0.2
        static_transform_stamped.transform.translation.y = 0.0
        static_transform_stamped.transform.translation.z = 0.1

        # 设置变换的旋转部分，这里假设没有旋转，即四元数(0, 0, 0, 1)
        quat = tf_transformations.quaternion_from_euler(0, 0, 0)
        static_transform_stamped.transform.rotation.x = quat[0]
        static_transform_stamped.transform.rotation.y = quat[1]
        static_transform_stamped.transform.rotation.z = quat[2]
        static_transform_stamped.transform.rotation.w = quat[3]

        # 发送静态变换
        self._broadcaster.sendTransform(static_transform_stamped)

def main(args=None):
    rclpy.init(args=args)
    node = StaticTransformPublisher()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

代码解释

初始化节点：StaticTransformPublisher是一个ROS 2节点，用于发布静态变换。
创建静态变换广播器：使用StaticTransformBroadcaster类创建一个静态变换广播器。
定义变换信息：创建一个TransformStamped对象，设置源坐标系（base_link）和目标坐标系（laser），以及它们之间的平移和旋转关系。这里假设激光雷达在底盘前方20厘米，高度为10厘米。
设置四元数：用tf_transformations.quaternion_from_euler函数将欧拉角转换为四元数，并赋值给变换的旋转部分。
发送变换：调用sendTransform方法将定义的静态变换广播出去。
运行节点：通过rclpy.spin方法使节点保持运行，持续广播变换。

这样其他节点就可以通过tf2查询底盘和激光雷达之间的变换关系，进行坐标转换和计算。

7、TF2广播和监听

TF2广播的主要作用包括：

管理坐标系：tf2帮助我们定义和管理不同部件的坐标系之间的关系。
实时更新变换：tf2可以广播动态变换，实时更新坐标系之间的关系，这对移动的机器人或动态环境非常重要。
简化开发：通过tf2广播，其他节点可以轻松获取和使用这些变换，简化了开发工作。

TF2监听的主要作用包括：

获取实时变换：tf2监听器实时接收并保存变换信息，确保我们能够获取最新的坐标变换。
简化坐标转换：监听器可以帮助我们在不同坐标系之间轻松转换坐标，简化了数据处理过程。
统一坐标系管理：通过tf2监听，我们可以将所有部件和传感器的数据统一到一个坐标系中进行处理，方便集成和分析。

广播和监听示例

我们将创建两个ROS 2节点，一个用于广播坐标变换，另一个用于监听这些变换并使用它们。

广播节点

这个节点会广播一个从base_link到laser的变换，表示激光雷达固定在机器人底盘前方20厘米，高度为10厘米的位置。

import rclpy
from rclpy.node import Node
from tf2_ros.transform_broadcaster import TransformBroadcaster
from geometry_msgs.msg import TransformStamped
import tf_transformations

class TransformBroadcasterNode(Node):
    def __init__(self):
        super().__init__('transform_broadcaster_node')
        
        # 创建变换广播器
        self._broadcaster = TransformBroadcaster(self)
        
        # 创建定时器，每秒调用一次回调函数发布变换
        self.timer = self.create_timer(1.0, self.broadcast_transform)

    def broadcast_transform(self):
        # 创建一个TransformStamped消息来存储变换信息
        transform_stamped = TransformStamped()
        transform_stamped.header.stamp = self.get_clock().now().to_msg()
        transform_stamped.header.frame_id = 'base_link'
        transform_stamped.child_frame_id = 'laser'

        # 设置变换的平移部分，假设激光雷达在底盘前方20厘米，高度为10厘米
        transform_stamped.transform.translation.x = 0.2
        transform_stamped.transform.translation.y = 0.0
        transform_stamped.transform.translation.z = 0.1

        # 设置变换的旋转部分，这里假设没有旋转，即四元数(0, 0, 0, 1)
        quat = tf_transformations.quaternion_from_euler(0, 0, 0)
        transform_stamped.transform.rotation.x = quat[0]
        transform_stamped.transform.rotation.y = quat[1]
        transform_stamped.transform.rotation.z = quat[2]
        transform_stamped.transform.rotation.w = quat[3]

        # 发送变换
        self._broadcaster.sendTransform(transform_stamped)

def main(args=None):
    rclpy.init(args=args)
    node = TransformBroadcasterNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

监听节点

这个节点会监听从base_link到laser的变换，并将激光雷达坐标系下的一个点转换到底盘坐标系中。

import rclpy
from rclpy.node import Node
from tf2_ros import TransformListener, Buffer
from geometry_msgs.msg import PointStamped
import tf2_geometry_msgs

class TransformListenerNode(Node):
    def __init__(self):
        super().__init__('transform_listener_node')

        # 创建一个Buffer对象来存储变换
        self.tf_buffer = Buffer()
        
        # 创建TransformListener来监听变换
        self.tf_listener = TransformListener(self.tf_buffer, self)
        
        # 模拟一个激光雷达数据点（在激光雷达坐标系下）
        self.laser_point = PointStamped()
        self.laser_point.header.frame_id = 'laser'
        self.laser_point.point.x = 1.0
        self.laser_point.point.y = 0.0
        self.laser_point.point.z = 0.0
        
        # 创建定时器，每秒调用一次回调函数
        self.timer = self.create_timer(1.0, self.on_timer)
        
    def on_timer(self):
        try:
            # 查询从'laser'到'base_link'的变换
            transform = self.tf_buffer.lookup_transform('base_link', 'laser', rclpy.time.Time())
            
            # 将激光雷达坐标系下的点转换到底盘坐标系下
            transformed_point = tf2_geometry_msgs.do_transform_point(self.laser_point, transform)
            
            # 打印转换后的点
            self.get_logger().info(f'Transformed point: ({transformed_point.point.x}, {transformed_point.point.y}, {transformed_point.point.z})')
        
        except Exception as e:
            self.get_logger().warn(f'Could not transform laser point: {e}')

def main(args=None):
    rclpy.init(args=args)
    node = TransformListenerNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

代码解释

广播节点

初始化节点：TransformBroadcasterNode是一个ROS 2节点，用于发布变换。
创建变换广播器：使用TransformBroadcaster类创建一个变换广播器。
设置定时器：每秒调用一次broadcast_transform方法，实时更新和广播变换。
定义变换信息：创建一个TransformStamped对象，设置源坐标系（base_link）和目标坐标系（laser），以及它们之间的平移和旋转关系。
发送变换：调用sendTransform方法将定义的变换广播出去。

监听节点

初始化节点：TransformListenerNode是一个ROS 2节点，用于监听变换。
创建Buffer对象：Buffer对象用于存储和管理变换信息。
创建TransformListener：TransformListener监听并填充Buffer对象中的变换信息。
模拟激光雷达数据点：创建一个PointStamped对象，表示激光雷达坐标系下的一个点。
设置定时器：每秒调用一次on_timer方法，尝试进行坐标变换。
查询变换：在on_timer方法中，使用lookup_transform方法查询从laser坐标系到base_link坐标系的变换。
进行坐标转换：使用tf2_geometry_msgs.do_transform_point方法将激光雷达坐标系下的点转换到底盘坐标系下。
打印转换结果：打印转换后的点的坐标。

通过这两个节点，我们展示了如何使用tf2广播和监听功能来管理和使用机器人系统中的坐标变换。

8、使用 TF2的复杂场景：多机器人协作导航

场景描述

假设我们有一个仓库环境，其中有两个协作机器人，分别是运输机器人和监控机器人。运输机器人负责搬运货物，而监控机器人负责实时监控环境，并向运输机器人提供导航帮助。这两个机器人需要通过TF2框架来共享和转换坐标信息，以实现协作导航。

具体实现

坐标系设置：
- 世界坐标系（world）： 仓库的全局坐标系，所有机器人和固定物体都相对于它定位。
- 运输机器人基坐标系（base_link_transporter）： 运输机器人本身的坐标系，跟随机器人的移动而变化。
- 监控机器人基坐标系（base_link_monitor）： 监控机器人本身的坐标系，也跟随机器人的移动而变化。
- 货物坐标系（cargo）： 货物的坐标系，初始位置固定，但搬运过程中相对运输机器人基坐标系变化。
数据流和坐标转换：
- 监控机器人通过摄像头或LiDAR扫描整个仓库环境，记录货物的坐标
- 监控机器人将货物的位置坐标发布到世界坐标系下。
- 运输机器人需要从世界坐标系中获取货物的位置，然后将其转换到自身的基坐标系（base_link_transporter），以便进行导航和搬运。

通俗解释

想象一下，这两个机器人就像仓库里的小助手。监控机器人像一个有鹰眼视角的哨兵，它能够实时看到整个仓库，并能准确指出货物的位置。运输机器人像一个勤劳的搬运工，它虽然视野有限，但能够准确地搬运货物。

坐标系的作用：
- 世界坐标系 就像仓库的总地图，所有的位置都是基于这个地图来描述的。
- 运输机器人的坐标系 就像搬运工自己的坐标，它只关心货物相对它自己的位置。
- 监控机器人的坐标系 也类似，它只关心自身位置和货物的位置。
合作过程：
- 哨兵（监控机器人）扫描仓库，发现了货物的位置，并在总地图上标记出来。
- 搬运工（运输机器人）从总地图上获取到货物的位置，然后将这个位置信息转换成自己能理解的方式，就好像哨兵告诉搬运工：“货物在你前方3米，右转2米的地方。”
- 搬运工根据这个信息导航到货物位置，成功完成搬运任务。

实现步骤

启动TF2变换树：
- 配置和启动每个机器人的TF2广播器，用于发布自身的基坐标系相对于世界坐标系的变换。
发布货物位置：
- 监控机器人通过传感器检测货物，并将货物的位置变换发布到世界坐标系下。
订阅并转换坐标：
- 运输机器人订阅货物在世界坐标系下的位置，将其转换为自身基坐标系下的位置，以便进行导航。

示例代码

监控机器人节点（发布货物位置）

python
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import PointStamped
from tf2_ros import TransformBroadcaster
from geometry_msgs.msg import TransformStamped

class MonitorRobot(Node):
    def __init__(self):
        super().__init__('monitor_robot')
        
        # 发布货物位置的话题
        self.cargo_publisher = self.create_publisher(PointStamped, 'cargo_position', 10)
        self.cargo_timer = self.create_timer(2.0, self.publish_cargo_position)
        
        # 创建TF2变换广播器
        self.br = TransformBroadcaster(self)
        
    def publish_cargo_position(self):
        # 假设监控机器人检测到的货物位置在监控机器人的坐标系下
        cargo_in_monitor = PointStamped()
        cargo_in_monitor.header.stamp = self.get_clock().now().to_msg()
        cargo_in_monitor.header.frame_id = 'base_link_monitor'
        cargo_in_monitor.point.x = 1.0
        cargo_in_monitor.point.y = 2.0
        cargo_in_monitor.point.z = 0.0
        
        # 将货物位置发布到世界坐标系下
        cargo_in_world = self.transform_to_world(cargo_in_monitor)
        self.cargo_publisher.publish(cargo_in_world)
        self.get_logger().info(f'Published cargo position in world frame: {cargo_in_world.point}')
        
        # 同时广播监控机器人相对于世界坐标系的变换
        self.broadcast_transform()
    
    def transform_to_world(self, point):
        # 创建世界坐标系下的点
        transformed_point = PointStamped()
        transformed_point.header.stamp = self.get_clock().now().to_msg()
        transformed_point.header.frame_id = 'world'
        
        # 假设监控机器人相对于世界坐标系的变换为平移 (3, 4, 0)
        transformed_point.point.x = point.point.x + 3.0
        transformed_point.point.y = point.point.y + 4.0
        transformed_point.point.z = point.point.z
        
        return transformed_point
    
    def broadcast_transform(self):
        t = TransformStamped()
        t.header.stamp = self.get_clock().now().to_msg()
        t.header.frame_id = 'world'
        t.child_frame_id = 'base_link_monitor'
        t.transform.translation.x = 3.0
        t.transform.translation.y = 4.0
        t.transform.translation.z = 0.0
        t.transform.rotation.w = 1.0
        self.br.sendTransform(t)

def main(args=None):
    rclpy.init(args=args)
    node = MonitorRobot()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

运输机器人节点（订阅货物位置并进行坐标转换）

python
import rclpy
from rclpy.node import Node
from tf2_ros import TransformListener, Buffer
from tf2_geometry_msgs import do_transform_point
from geometry_msgs.msg import PointStamped
from rclpy.time import Time

class TransportRobot(Node):
    def __init__(self):
        super().__init__('transport_robot')
        
        # 创建TF2缓冲区和监听器
        self.buffer = Buffer()
        self.listener = TransformListener(self.buffer, self)
        
        # 订阅货物位置的话题
        self.cargo_subscription = self.create_subscription(
            PointStamped,
            'cargo_position',
            self.handle_cargo_position,
            10
        )
        
    def handle_cargo_position(self, msg):
        try:
            # 查找从世界坐标系到运输机器人基坐标系的变换
            transform = self.buffer.lookup_transform('base_link_transporter', 'world', Time())
            
            # 将货物的位置变换到运输机器人的基坐标系
            cargo_in_base = do_transform_point(msg, transform)
            
            # 打印货物在运输机器人基坐标系中的位置
            self.get_logger().info(f'Cargo position in base_link_transporter: {cargo_in_base.point}')
        except Exception as e:
            self.get_logger().warn(f'Could not transform cargo position: {e}')

def main(args=None):
    rclpy.init(args=args)
    node = TransportRobot()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

世界坐标系节点（广播固定的世界坐标系变换）

python
import rclpy
from rclpy.node import Node
from tf2_ros import TransformBroadcaster
from geometry_msgs.msg import TransformStamped

class WorldFramePublisher(Node):
    def __init__(self):
        super().__init__('world_frame_publisher')
        
        # 创建TF2变换广播器
        self.br = TransformBroadcaster(self)
        
        # 定时广播变换
        self.timer = self.create_timer(1.0, self.broadcast_transform)
        
    def broadcast_transform(self):
        # 创建一个变换消息
        t = TransformStamped()
        
        # 设置变换的时间戳
        t.header.stamp = self.get_clock().now().to_msg()
        
        # 设置父坐标系和子坐标系
        t.header.frame_id = 'world'
        t.child_frame_id = 'base_link_transporter'
        
        # 设置变换的平移部分
        t.transform.translation.x = 0.0
        t.transform.translation.y = 0.0
        t.transform.translation.z = 0.0
        
        # 设置变换的旋转部分（四元数表示）
        t.transform.rotation.w = 1.0
        
        # 发送变换
        self.br.sendTransform(t)

def main(args=None):
    rclpy.init(args=args)
    node = WorldFramePublisher()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()