tf2介绍

安装依赖

sudo apt-get install ros-foxy-turtle-tf2-py ros-foxy-tf2-tools

transforms3d（它提供四元数和欧拉角转换功能）

pip3 install transforms3d

运行结果

启动turtle_tf2_demo.launch.py文件

ros2 launch turtle_tf2_py turtle_tf2_demo.launch.py

键盘控制turtlesim

ros2 run turtlesim turtle_teleop_key

使用键盘控制乌龟，可以看到一只乌龟会跟随另一只乌龟。

使用view_frames

ros2 run tf2_tools view_frames

生成的文件

frames.pdf

使用tf2_echo(报告通过 ROS 广播的任意两帧之间的转换)

ros2 run tf2_ros tf2_echo [reference_frame] [target_frame]

turtle2框架相对于turtle1框架的变换

ros2 run tf2_ros tf2_echo turtle2 turtle1

可以在终端显示turtle2框架相对于turtle1框架的变换

tf2广播器(broadcaster)

先决条件

创建工作区间和功能包

创建一个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()

创建一个turtle_tf2_broadcaster.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='py_package',
            executable='turtle_tf2_broadcaster',
            name='broadcaster1',
            parameters=[
                {'turtlename': 'turtle1'}
            ]
        ),
    ])

添加依赖

修改setup.py文件

'console_scripts': [
    'turtle_tf2_broadcaster = py_package.turtle_tf2_broadcaster:main',
],
data_files=[
    ...
    (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*.launch.py'))),
],

修改package.xml文件

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

编译

colcon build
. install/setup.bash

运行结果

运行turtle_tf2_broadcaster.launch.py文件

(同时启动turtlesim节点和turtle_tf2_broadcaster节点)

ros2 launch learning_tf2_py turtle_tf2_broadcaster.launch.py

键盘控制turtlesim

ros2 run turtlesim turtle_teleop_key

使用tf2_echo工具检测海龟位姿是否广播到tf2

ros2 run tf2_ros tf2_echo world turtle1

可以在终端显示world框架相对于turtle1框架的变换

tf2侦听器(listener)

创建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()

创建turtle_tf2_listener.launch.py文件

from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([

        Node(
            package='py_package',
            executable='turtle_tf2_listener',
            name='listener',
            parameters=[
                {'target_frame': 'turtle2'}
            ]
        ),
    ])

添加依赖

修改setup.py文件

'console_scripts': [
    'turtle_tf2_listener = py_package.turtle_tf2_listener:main',
],

编译

colcon build
. install/setup.bash

运行结果

运行turtle_tf2_broadcaster.launch.py文件

ros2 launch learning_tf2_py turtle_tf2_broadcaster.launch.py

运行turtle_tf2_listener.launch.py文件

ros2 launch py_package turtle_tf2_listener.launch.py

键盘控制turtlesim

ros2 run turtlesim turtle_teleop_key

使用键盘控制turtle1,可以看到turtle2会跟随turtle1
这里turtle_tf2_broadcaster节点会广播(broadcaster) turtle1位姿
turtle_tf2_listener节点会监听(listener) turtle1位姿

参考链接

tf2 介绍

编写 tf2 广播器 (Python)

编写 tf2 侦听器 (Python)