ROS2与tf2入门教程-tf2和时间(Python)

说明：

介绍如何在lookup_transform函数使用timeout等待转换在 tf2 树上可用
lookup_transform函数增加了超时处理
同时也增加了超时的异常处理
函数lookup_transform() 有四个参数，前两个参数用于设置要进行坐标变换的目标坐标系和源坐标系，第三个参数用于设置查找哪个时刻的坐标变换（这里为当前时刻），第四个是可选参数，用于设置查找坐标变换的超时时长。
如果设置了timeout参数，则将会阻塞最多长达超时参数设定的持续时间（这里为1秒）。
lookup_transform() 函数实际上会阻塞直到两个小乌龟之间的坐标变换可用为止（这通常需要几毫秒）。
一旦达到超时设定的时长（在本示例中为1秒），如果坐标变换仍然不可用，就会引发异常。
但这个超时参数的设置实际上非常重要。
如果不设置timeout参数，由于没有等待用于查找坐标变换的时间，系统可能会提示坐标系不存在或者坐标变换消息不成功。
但超时参数也不能设置为太长的时长，即不能超过回调函数调用频率的倒数（例如频率为1hz时不能超时时长不能大于1秒，频率为10hz时不能大于0.1秒），否则会造成系统阻塞。

步骤：

新建turtle_tf2_listener_timeout.py

cd ~/tf2_ws/src/learning_tf2_py/learning_tf2_py
cp turtle_tf2_listener.py turtle_tf2_listener_timeout.py
vim turtle_tf2_listener_timeout.py

编辑turtle_tf2_listener_timeout.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 tf2_ros import LookupException, ConnectivityException, ExtrapolationException
from turtlesim.srv import Spawn
from rclpy.duration import Duration

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,
                        timeout=Duration(seconds=1.0))
                except (LookupException, ConnectivityException, ExtrapolationException):
                    self.get_logger().info('transform not ready')
                    raise
                    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_timeout.py

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

'turtle_tf2_listener_timeout = learning_tf2_py.turtle_tf2_listener_timeout:main',

新增为turtle_tf2_demo_timeout.launch.py

cd ~/tf2_ws/src/learning_tf2_py/launch
cp turtle_tf2_demo.launch.py turtle_tf2_demo_timeout.launch.py
vim turtle_tf2_demo_timeout.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='carrot1', 
      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_timeout',
      name='listener',
      parameters=[
          {'target_frame': LaunchConfiguration('target_frame')}
      ]
    ),   
  ])

新增为turtle_tf2_fixed_frame_demo_timeout.launch.py

cd ~/tf2_ws/src/learning_tf2_py/launch
cp turtle_tf2_fixed_frame_demo.launch.py turtle_tf2_fixed_frame_demo_timeout.launch.py
vim turtle_tf2_fixed_frame_demo_timeout.launch.py

内容如下:

import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node
from launch.substitutions import LaunchConfiguration

def generate_launch_description():
    target_frame = LaunchConfiguration('target_frame')
    
    declare_target_frame_cmd = DeclareLaunchArgument(
        'target_frame',
        default_value='turtle1',
        description='target_frame')

    demo_nodes = IncludeLaunchDescription(
      PythonLaunchDescriptionSource([os.path.join(
            get_package_share_directory('learning_tf2_py'), 'launch'),
            '/turtle_tf2_demo_timeout.launch.py']),
      launch_arguments={'target_frame': target_frame}.items()
    )

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

构建

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

加载工作空间

. ~/tf2_ws/install/local_setup.bash

测试:

启动turtle_tf2_demo_timeout.launch.py

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

因为没有发布坐标系carrot1, 导致没有实现变换,触发了异常,所以会出现报错信息
类似错误

tf2.LookupException: "carrot1" passed to lookupTransform argument source_frame does not exist. 
[ERROR] [turtle_tf2_listener_timeout-4]: process has died [pid 9789, exit code 1, cmd '/home/ubuntu/tf2_ws/install/learning_tf2_py/lib/learning_tf2_py/turtle_tf2_listener_timeout --ros-args -r __node:=listener --params-file /tmp/launch_params_bwkzormr'].

启动turtle_tf2_fixed_frame_demo_timeout.launch.py

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

等待5秒变换信息可用,超过5秒就会报异常
可以打印出来

$ ros2 run tf2_ros tf2_echo carrot1 turtle1
At time 1645499090.876287690
- Translation: [0.000, -2.000, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.000, 1.000]
At time 1645499091.875560192
- Translation: [0.000, -2.000, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.000, 1.000]
At time 1645499092.875898218
- Translation: [0.000, -2.000, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.000, 1.000]

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