机器人系统ros2-开发实践09-如何向 tf2 添加额外的坐标帧位置(Python)

在之前的教程中，我们通过编写tf2 广播器和tf2 监听器重新创建了乌龟演示。本教程将教您如何向转换树添加额外的固定和动态框架。事实上，在 tf2 中添加框架与创建 tf2 广播器非常相似，但此示例将向您展示 tf2 的一些附加功能。

对于许多与转换相关的任务，在局部框架内思考会更容易。例如，最容易推断激光扫描仪中心框架中的激光扫描测量结果。 tf2 允许您为系统中的每个传感器、链路或关节定义本地框架。从一帧转换到另一帧时，tf2 将处理引入的所有隐藏中间帧转换。

tf2树

tf2 建立了帧的树结构，因此不允许帧结构中出现闭环。这意味着一个框架只有一个父框架，但可以有多个子框架。目前，我们的 tf2 树包含三个帧：world、turtle1和turtle2。两个海龟框架是该world框架的子框架。如果我们想向 tf2 添加一个新框架，则现有的三个框架之一需要作为父框架，而新框架将成为其子框架。

比如在机械臂形态的机器人中，机器人安装的位置叫做基坐标系Base Frame，机器人安装位置在外部环境下的参考系叫做世界坐标系World Frame，机器人末端夹爪的位置叫做工具坐标系，外部被操作物体的位置叫做工件坐标系，在机械臂抓取外部物体的过程中，这些坐标系之间的关系也在跟随变化。

在移动机器人系统中，坐标系一样至关重要，比如一个移动机器人的中心点是基坐标系Base Link，雷达所在的位置叫做雷达坐标系laser link，机器人要移动，里程计会累积位置，这个位置的参考系叫做里程计坐标系odom，里程计又会有累积误差和漂移，绝对位置的参考系叫做地图坐标系map。

一层一层坐标系之间关系复杂，有一些是相对固定的，也有一些是不断变化的，看似简单的坐标系也在空间范围内变得复杂，良好的坐标系管理系统就显得格外重要。

关于坐标系变换关系的基本理论，在每一本机器人学的教材中都会有讲解，可以分解为平移和旋转两个部分，通过一个四乘四的矩阵进行描述，在空间中画出坐标系，那两者之间的变换关系，其实就是向量的数学描述。

1 编写固定帧广播器

在我们的海龟示例中，我们将添加一个新框架carrot1，它将成为turtle1.该框架将作为第二只乌龟的目标。

我们首先创建源文件。转到learning_tf2_py 我们在之前的教程中创建的包。在src/learning_tf2_py/learning_tf2_py目录中新建fixed_frame_tf2_broadcaster.py文件，然后用vccode 打开写入如下代码：

from geometry_msgs.msg import TransformStamped

import rclpy
from rclpy.node import Node

from tf2_ros import TransformBroadcaster


class FixedFrameBroadcaster(Node):

   def __init__(self):
       super().__init__('fixed_frame_tf2_broadcaster')
       self.tf_broadcaster = TransformBroadcaster(self)
       self.timer = self.create_timer(0.1, self.broadcast_timer_callback)

   def broadcast_timer_callback(self):
       t = TransformStamped()

       t.header.stamp = self.get_clock().now().to_msg()
       t.header.frame_id = 'turtle1'
       t.child_frame_id = 'carrot1'
       t.transform.translation.x = 0.0
       t.transform.translation.y = 2.0
       t.transform.translation.z = 0.0
       t.transform.rotation.x = 0.0
       t.transform.rotation.y = 0.0
       t.transform.rotation.z = 0.0
       t.transform.rotation.w = 1.0

       self.tf_broadcaster.sendTransform(t)


def main():
    rclpy.init()
    node = FixedFrameBroadcaster()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass

    rclpy.shutdown()

上面关键代码说明：

在这里，我们创建一个新的转换，从父级turtle1到新的子级carrot1。就框架而言，框架carrot1在 y 轴上偏移 2 米turtle1。

t = TransformStamped()

t.header.stamp = self.get_clock().now().to_msg()
t.header.frame_id = 'turtle1'
t.child_frame_id = 'carrot1'
t.transform.translation.x = 0.0
t.transform.translation.y = 2.0
t.transform.translation.z = 0.0

添加节点入口：

在src/learning_tf2_py，用vscode 打开setup.py

在console_scripts 括号之间添加以下行’：

'fixed_frame_tf2_broadcaster = learning_tf2_py.fixed_frame_tf2_broadcaster:main',

编写启动文件

src/learning_tf2_py/launch目录中创建一个名为的新文件turtle_tf2_fixed_frame_demo.launch.py，代码内容如下：

import os

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource

from launch_ros.actions import Node


def generate_launch_description():
    demo_nodes = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([os.path.join(
            get_package_share_directory('learning_tf2_py'), 'launch'),
            '/turtle_tf2_demo.launch.py']),
        )

    return LaunchDescription([
        demo_nodes,
        Node(
            package='learning_tf2_py',
            executable='fixed_frame_tf2_broadcaster',
            name='fixed_broadcaster',
        ),
    ])

此启动文件导入所需的包，然后创建一个demo_nodes变量来存储我们在上一教程的启动文件中创建的节点。

代码的最后一部分将carrot1使用我们的节点将我们的固定框架添加到turtlesim世界中fixed_frame_tf2_broadcaster。

Node(
    package='learning_tf2_py',
    executable='fixed_frame_tf2_broadcaster',
    name='fixed_broadcaster',
),

编译：

在工作区的根目录中运行rosdep以检查是否缺少依赖项。

rosdep install -i --from-path src --rosdistro humble -y

仍然在工作区的根目录中构建您的包：

colcon build --packages-select learning_tf2_py

打开一个新终端，导航到工作区的根目录，然后获取安装文件：

. install/setup.bash

运行

现在您可以启动海龟广播演示：

ros2 launch learning_tf2_py turtle_tf2_fixed_frame_demo.launch.py

这里跟上面教程一样，初始化两个乌龟，其中一个乌龟会向另外一个乌龟靠齐

新carrot1框架出现在转换树中

如果你驾驶第一只乌龟，你应该注意到，即使我们添加了一个新框架，其行为与之前的教程相比并没有改变。这是因为添加额外的帧不会影响其他帧，并且我们的侦听器仍在使用之前定义的帧。

因此，如果我们希望第二只乌龟而不是第一只乌龟跟随胡萝卜，我们需要更改 的值target_frame。这可以通过两种方式完成。一种方法是target_frame直接从控制台将参数传递给启动文件：

ros2 launch learning_tf2_py turtle_tf2_fixed_frame_demo.launch.py target_frame:=carrot1