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_listener.py

• 打开文件

import math

from geometry_msgs.msg import Twist

import rclpy
from rclpy.node import Node

from tf2_ros import TransformException
from tf2_ros.buffer import Buffer
from tf2_ros.transform_listener import TransformListener

from turtlesim.srv import Spawn


class FrameListener(Node):

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

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

        self.tf_buffer = Buffer()
        self.tf_listener = TransformListener(self.tf_buffer, self)

        # Create a client to spawn a turtle
        self.spawner = self.create_client(Spawn, 'spawn')
        # Boolean values to store the information
        # if the service for spawning turtle is available
        self.turtle_spawning_service_ready = False
        # if the turtle was successfully spawned
        self.turtle_spawned = False

        # Create turtle2 velocity publisher
        self.publisher = self.create_publisher(Twist, 'turtle2/cmd_vel', 1)

        # Call on_timer function every second
        self.timer = self.create_timer(1.0, self.on_timer)

    def on_timer(self):
        # Store frame names in variables that will be used to
        # compute transformations
        from_frame_rel = self.target_frame
        to_frame_rel = 'turtle2'

        if self.turtle_spawning_service_ready:
            if self.turtle_spawned:
                # Look up for the transformation between target_frame and turtle2 frames
                # and send velocity commands for turtle2 to reach target_frame
                try:
                    now = rclpy.time.Time()
                    trans = self.tf_buffer.lookup_transform(
                        to_frame_rel,
                        from_frame_rel,
                        now)
                except TransformException as ex:
                    self.get_logger().info(
                        f'Could not transform {to_frame_rel} to {from_frame_rel}: {ex}')
                        return

                msg = Twist()
                scale_rotation_rate = 1.0
                msg.angular.z = scale_rotation_rate * math.atan2(
                    trans.transform.translation.y,
                    trans.transform.translation.x)

                scale_forward_speed = 0.5
                msg.linear.x = scale_forward_speed * math.sqrt(
                    trans.transform.translation.x ** 2 +
                    trans.transform.translation.y ** 2)

                self.publisher.publish(msg)
            else:
                if self.result.done():
                    self.get_logger().info(
                        f'Successfully spawned {self.result.result().name}')
                    self.turtle_spawned = True
                else:
                    self.get_logger().info('Spawn is not finished')
        else:
            if self.spawner.service_is_ready():
                # Initialize request with turtle name and coordinates
                # Note that x, y and theta are defined as floats in turtlesim/srv/Spawn
                request = Spawn.Request()
                request.name = 'turtle2'
                request.x = float(4)
                request.y = float(2)
                request.theta = float(0)
                # Call request
                self.result = self.spawner.call_async(request)
                self.turtle_spawning_service_ready = True
           else:
                # Check if the service is ready
                self.get_logger().info('Service is not ready')


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

    rclpy.shutdown()

• 赋执行权限

sudo chmod +x turtle_tf2_listener.py

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

'turtle_tf2_listener    = learning_tf2_py.turtle_tf2_listener:main',

• 打开名为turtle_tf2_demo.launch.py的启动文件

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

　- 修改后内容如下

from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration

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'}
        ]
    ),
    
    DeclareLaunchArgument(
      'target_frame', default_value='turtle1', 
      description='Target frame name.'
    ),

    Node(
      package='learning_tf2_py',
      executable='turtle_tf2_broadcaster',
      name='broadcaster2',
      parameters=[
          {'turtlename': 'turtle2'}
      ]
    ),
    Node(
      package='learning_tf2_py',
      executable='turtle_tf2_listener',
      name='listener',
      parameters=[
          {'target_frame': LaunchConfiguration('target_frame')}
      ]
    ),   
  ])

• 构建

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

• 加载工作空间

. ~/tf2_ws/install/local_setup.bash

测试：　

• 运行代码

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

• 应该看到带有两只海龟的海龟模拟

• 键盘控制

ros2 run turtlesim turtle_teleop_key

• 效果是，控制第一个海龟移动，第二个海龟会自动跟随第一个海龟移动

参考:

• 资料来源