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

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]

参考: 

纠错,疑问,交流: 请进入讨论区点击加入Q群

获取最新文章: 扫一扫右上角的二维码加入“创客智造”公众号


标签: ros2与tf2入门教程