机器人系统ros2-开发实践06-将静态坐标系广播到 tf2(Python)-定义机器人底座与其传感器或非移动部件之间的关系

发布静态变换对于定义机器人底座与其传感器或非移动部件之间的关系非常有用。例如，最容易推断激光扫描仪中心框架中的激光扫描测量结果。

1. 创建包

首先，我们将创建一个用于本教程和后续教程的包。调用的包learning_tf2_py将依赖于geometry_msgs、python3-numpy、rclpy、tf2_ros_py和turtlesim

cd ros2_study/src

ros2 pkg create --build-type ament_python --license Apache-2.0 -- learning_tf2_py

learning_tf2_py您的终端将返回一条消息，验证您的包及其所有必需文件和文件夹的创建。

2 编写静态广播节点

我们首先创建源文件。在src/learning_tf2_py/learning_tf2_py目录中输入以下命令来下载示例静态广播器代码：

wget https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_py/turtle_tf2_py/static_turtle_tf2_broadcaster.py

这个有点慢，可能要等一会才下好

下载后可查看：

static_turtle_tf2_broadcaster.py 代码内容如下：

import math
import sys

from geometry_msgs.msg import TransformStamped

import numpy as np

import rclpy
from rclpy.node import Node

from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster


def quaternion_from_euler(ai, aj, ak):
    ai /= 2.0
    aj /= 2.0
    ak /= 2.0
    ci = math.cos(ai)
    si = math.sin(ai)
    cj = math.cos(aj)
    sj = math.sin(aj)
    ck = math.cos(ak)
    sk = math.sin(ak)
    cc = ci*ck
    cs = ci*sk
    sc = si*ck
    ss = si*sk

    q = np.empty((4, ))
    q[0] = cj*sc - sj*cs
    q[1] = cj*ss + sj*cc
    q[2] = cj*cs - sj*sc
    q[3] = cj*cc + sj*ss

    return q


class StaticFramePublisher(Node):
    """
    Broadcast transforms that never change.

    This example publishes transforms from `world` to a static turtle frame.
    The transforms are only published once at startup, and are constant for all
    time.
    """

    def __init__(self, transformation):
        super().__init__('static_turtle_tf2_broadcaster')

        self.tf_static_broadcaster = StaticTransformBroadcaster(self)

        # Publish static transforms once at startup
        self.make_transforms(transformation)

    def make_transforms(self, transformation):
        t = TransformStamped()

        t.header.stamp = self.get_clock().now().to_msg()
        t.header.frame_id = 'world'
        t.child_frame_id = transformation[1]

        t.transform.translation.x = float(transformation[2])
        t.transform.translation.y = float(transformation[3])
        t.transform.translation.z = float(transformation[4])
        quat = quaternion_from_euler(
            float(transformation[5]), float(transformation[6]), float(transformation[7]))
        t.transform.rotation.x = quat[0]
        t.transform.rotation.y = quat[1]
        t.transform.rotation.z = quat[2]
        t.transform.rotation.w = quat[3]

        self.tf_static_broadcaster.sendTransform(t)


def main():
    logger = rclpy.logging.get_logger('logger')

    # obtain parameters from command line arguments
    if len(sys.argv) != 8:
        logger.info('Invalid number of parameters. Usage: \n'
                    '$ ros2 run learning_tf2_py static_turtle_tf2_broadcaster'
                    'child_frame_name x y z roll pitch yaw')
        sys.exit(1)

    if sys.argv[1] == 'world':
        logger.info('Your static turtle name cannot be "world"')
        sys.exit(2)

    # pass parameters and initialize node
    rclpy.init()
    node = StaticFramePublisher(sys.argv)
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass

    rclpy.shutdown()

2.1 代码解析：

第一行导入所需的包。首先，我们TransformStamped从导入geometry_msgs，它为我们将发布到转换树的消息提供了一个模板。

from geometry_msgs.msg import TransformStamped

然后rclpy导入，以便Node可以使用它的类。

import rclpy
from rclpy.node import Node

该tf2_ros包提供了一个StaticTransformBroadcaster使静态转换发布变得容易的方法。要使用StaticTransformBroadcaster，我们需要从tf2_ros模块导入它。

from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster

类StaticFramePublisher构造函数用 name 初始化节点static_turtle_tf2_broadcaster。然后StaticTransformBroadcaster创建 ，它将在启动时发送一个静态转换。

    def __init__(self, transformation):
        super().__init__('static_turtle_tf2_broadcaster')

        self.tf_static_broadcaster = StaticTransformBroadcaster(self)

        # Publish static transforms once at startup
        self.make_transforms(transformation)

在这里，我们创建一个TransformStamped对象，这将是我们在填充后发送的消息。在传递实际的转换值之前，我们需要为其提供适当的元数据。

---

• 我们需要给正在发布的转换一个时间戳，我们只需用当前时间来标记它，self.get_clock().now()

t.header.stamp = self.get_clock().now().to_msg()

• 然后我们需要设置我们正在创建的链接的父框架的名称

t.header.frame_id = 'world'

• 最后，我们需要设置我们正在创建的链接的子框架的名称

t.child_frame_id = transformation[1]

---

在这里，我们填充海龟的 6D 姿势（平移和旋转）。

t.transform.translation.x = float(transformation[2])
t.transform.translation.y = float(transformation[3])
t.transform.translation.z = float(transformation[4])
quat = quaternion_from_euler(
    float(transformation[5]), float(transformation[6]), float(transformation[7]))
    
t.transform.rotation.x = quat[0]
t.transform.rotation.y = quat[1]
t.transform.rotation.z = quat[2]
t.transform.rotation.w = quat[3]

最后，我们使用该sendTransform()函数广播静态变换。

self.tf_static_broadcaster.sendTransform(t)

2.2 更新package.xml

3.构建编译

最好rosdep在构建之前在工作区的根目录中运行以检查缺少的依赖项：

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

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

colcon build --packages-select learning_tf2_py

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

. install/setup.bash

4. 运行

现在运行static_turtle_tf2_broadcaster节点：

ros2 run learning_tf2_py static_turtle_tf2_broadcaster mystaticturtle 0 0 1 0 0 0

这设置了海龟姿势广播，使其mystaticturtle漂浮在距地面 1 米的高度。

tf_static我们现在可以通过回显主题来检查静态转换是否已发布

ros2 topic echo /tf_static

如果一切顺利你应该看到一个静态转换

transforms:
- header:
   stamp:
      sec: 1622908754
      nanosec: 208515730
   frame_id: world
child_frame_id: mystaticturtle
transform:
   translation:
      x: 0.0
      y: 0.0
      z: 1.0
   rotation:
      x: 0.0
      y: 0.0
      z: 0.0
      w: 1.0

上面代码展示如何StaticTransformBroadcaster用于发布静态转换的原理。在实际的开发过程中，您不必自己编写此代码，而应该使用专用tf2_ros工具来执行此操作。 tf2_ros提供了一个名为 的可执行文件static_transform_publisher，可以用作命令行工具或可以添加到启动文件中的节点来使用

例如：

使用以米为单位的 x/y/z 偏移和以弧度为单位的横滚/俯仰/偏航将静态坐标变换发布到 tf2。在我们的例子中，横滚/俯仰/偏航分别指的是绕 x/y/z 轴的旋转。

ros2 run tf2_ros static_transform_publisher --x x --y y --z z --yaw yaw --pitch pitch --roll roll --frame-id frame_id --child-frame-id child_frame_id

使用以米和四元数为单位的 x/y/z 偏移量将静态坐标变换发布到 tf2。

ros2 run tf2_ros static_transform_publisher --x x --y y --z z --qx qx --qy qy --qz qz --qw qw --frame-id frame_id --child-frame-id child_frame_id

static_transform_publisher被设计为手动使用的命令行工具，以及在launch文件中使用以设置静态转换

例如：

from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([
        Node(
            package='tf2_ros',
            executable='static_transform_publisher',
            arguments = ['--x', '0', '--y', '0', '--z', '1', '--yaw', '0', '--pitch', '0', '--roll', '0', '--frame-id', 'world', '--child-frame-id', 'mystaticturtle']
        ),
    ])

请注意，除了–frame-id和之外的所有参数–child-frame-id都是可选的