ROS2与tf2入门教程-编写tf2广播器 (Python)

说明：

• 介绍如何使用python编写tf2广播器

步骤：

• 下载示例广播代码

cd ~/tf2_ws/src/learning_tf2_py/learning_tf2_py
wget https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_py/turtle_tf2_py/turtle_tf2_broadcaster.py

• 打开文件

from geometry_msgs.msg import TransformStamped

import rclpy
from rclpy.node import Node

from tf2_ros import TransformBroadcaster

import tf_transformations

from turtlesim.msg import Pose


class FramePublisher(Node):

    def __init__(self):
        super().__init__('turtle_tf2_frame_publisher')

        # Declare and acquire `turtlename` parameter
        self.declare_parameter('turtlename', 'turtle')
        self.turtlename = self.get_parameter(
        'turtlename').get_parameter_value().string_value

        # Initialize the transform broadcaster
        self.br = TransformBroadcaster(self)

        # Subscribe to a turtle{1}{2}/pose topic and call handle_turtle_pose
        # callback function on each message
        self.subscription = self.create_subscription(
            Pose,
            f'/{self.turtlename}/pose',
            self.handle_turtle_pose,
            1)
        self.subscription

    def handle_turtle_pose(self, msg):
        t = TransformStamped()

        # Read message content and assign it to
        # corresponding tf variables
        t.header.stamp = self.get_clock().now().to_msg()
        t.header.frame_id = 'world'
        t.child_frame_id = self.turtlename

        # Turtle only exists in 2D, thus we get x and y translation
        # coordinates from the message and set the z coordinate to 0
        t.transform.translation.x = msg.x
        t.transform.translation.y = msg.y
        t.transform.translation.z = 0.0

        # For the same reason, turtle can only rotate around one axis
        # and this why we set rotation in x and y to 0 and obtain
        # rotation in z axis from the message
        q = tf_transformations.quaternion_from_euler(0, 0, msg.theta)
        t.transform.rotation.x = q[0]
        t.transform.rotation.y = q[1]
        t.transform.rotation.z = q[2]
        t.transform.rotation.w = q[3]

        # Send the transformation
        self.br.sendTransform(t)


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

    rclpy.shutdown()

• 赋执行权限

sudo chmod +x turtle_tf2_broadcaster.py

• 添加入口，修改setup.py在'console_scripts': [里添加

'turtle_tf2_broadcaster = learning_tf2_py.turtle_tf2_broadcaster:main',

建立launch文件

• 进入在launch文件夹

mkdir ~/tf2_ws/src/learning_tf2_py/launch
cd ~/tf2_ws/src/learning_tf2_py/launch
vim turtle_tf2_demo.launch.py

• 内容如下

from launch import LaunchDescription
from launch_ros.actions import Node


def generate_launch_description():
    return LaunchDescription([
        Node(
            package='turtlesim',
            executable='turtlesim_node',
            name='sim'
        ),
        Node(
            package='learning_tf2_py',
            executable='turtle_tf2_broadcaster',
            name='broadcaster1',
            parameters=[
                {'turtlename': 'turtle1'}
            ]
        ),
    ])

增加依赖：

• package.xml添加以下内容

<exec_depend>launch</exec_depend>
<exec_depend>launch_ros</exec_depend>

• setup.py添加以下内容

import os
from glob import glob
......
data_files=[
    ...
    (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*.launch.py'))),
],

• 检查缺少的依赖项

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

• 构建

colcon build --symlink-install --packages-select learning_tf2_py

• 加载工作空间

. ~/tf2_ws/install/local_setup.bash

测试：　

• 启动launc文件

. ~/tf2_ws/install/local_setup.bash
ros2 launch learning_tf2_py turtle_tf2_demo.launch.py

• 运行键盘控制

ros2 run turtlesim turtle_teleop_key

• 结果如下

• 使用tf2_echo工具检查海龟姿势是否真的被广播到 tf2

ros2 run tf2_ros tf2_echo world turtle1

• 结果如下

At time 1625137663.912474878
- Translation: [5.276, 7.930, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.934,     -0.357]
At time 1625137664.950813527
- Translation: [3.750, 6.563, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.934, -0.357]
At time 1625137665.906280726
- Translation: [2.320, 5.282, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.934, -0.357]
At time 1625137666.850775673
- Translation: [2.153, 5.133, 0.000]
- Rotation: in Quaternion [0.000, 0.000, -0.365, 0.931]

参考：　

• 资料来源