ROS2 第四讲 tf2

ROS2 第四讲 tf2

目录

• ROS2 第四讲 tf2
  • turtle demo
  • 静态broadcaster
  • 编写tf2 broadcaster
  • 编写tf2 listener
  • 添加坐标系
    • 代码解释:

turtle demo

安装turtle-tf2 demo:

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

以及可以转换四元数与欧拉角的包:

pip3 install transforms3d

我们来演示一下:

ros2 launch turtle_tf2_py turtle_tf2_demo.launch.py

会看到两只小乌龟，其中一只会向位于中心的小乌龟移动:

使用键盘控制下小乌龟，另一只仍会跟随:

ros2 run turtlesim turtle_teleop_key

这个demo 展示了tf2的功能。此例中，tf2库创建三个坐标系，分别是世界(或环境)坐标系，turtle1坐标系与turtle2坐标系。tf2库中的broadcaster发布turtle的坐标系，listener计算二者的相对位置，然后将基中一个小乌龟移向另一个。

ROS2提供了tf2_tools查看tf2工作图:

ros2 run tf2_tools view_frames.py

会显示:

[INFO] [1631335696.355017201] [view_frames]: Listening to tf data during 5 seconds...
[INFO] [1631335701.358101073] [view_frames]: Generating graph in frames.pdf file...
[INFO] [1631335701.359303339] [view_frames]: Result:tf2_msgs.srv.FrameGraph_Response(frame_yaml="turtle1: \n  parent: 'world'\n  broadcaster: 'default_authority'\n  rate: 62.705\n  most_recent_transform: 1631335701.357549\n  oldest_transform: 1631335696.349964\n  buffer_length: 5.008\nturtle2: \n  parent: 'world'\n  broadcaster: 'default_authority'\n  rate: 62.706\n  most_recent_transform: 1631335701.357533\n  oldest_transform: 1631335696.350012\n  buffer_length: 5.008\n")
/opt/ros/foxy/lib/tf2_tools/view_frames.py:75: YAMLLoadWarning: calling yaml.load() without Loader=... is deprecated, as the default Loader is unsafe. Please read https://msg.pyyaml.org/load for full details.
  data = yaml.load(result.frame_yaml)

然后在当前目录下，可以看到:

frames.gv
frames.pdf

打开frames.pdf:

可以看到，结构上world坐标系是两只小乌龟坐标系的父坐标系。上面还有一些接收到最老与最新坐系的时间，以及平均的接收频率。

使用tf2_echo时实查看两坐标系之间的转换:

# ros2 run tf2_ros tf2_echo [reference_frame] [target_frame]
ros2 run tf2_ros tf2_echo turtle2 turtle1

会显示类似:

At time 1631336659.214493548
- Translation: [0.015, 0.000, 0.000]
- Rotation: in Quaternion [0.000, 0.000, -0.206, 0.978]
At time 1631336660.222032437
- Translation: [0.008, 0.000, 0.000]
- Rotation: in Quaternion [0.000, 0.000, -0.208, 0.978]

使用rviz 查看:

ros2 run rviz2 rviz2 -d $(ros2 pkg prefix --share turtle_tf2_py)/rviz/turtle_rviz.rviz

使用键盘移动小乌龟时，会看到rviz中的坐标系会在rviz中移动。

静态broadcaster

创建package:

cd dev_ws/src/
ros2 pkg create --build-type ament_python learning_tf2_py
cd learning_tf2_py/learning_tf2_py/
vim static_turtle_tf2_broadcaster.py

在文件中编写:

import sys

from geometry_msgs.msg import TransformStamped

import rclpy
from rclpy.node import Node

from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster

import tf_transformations


class StaticFramePublisher(Node):
   """
   Broadcast transforms that never change.

   This example publishes transforms from `world` to a static turtle frame.
   The transforms are only published once at startup, and are constant for all
   time.
   """

   def __init__(self, transformation):
      super().__init__('static_turtle_tf2_broadcaster')

      self._tf_publisher = StaticTransformBroadcaster(self)

      # Publish static transforms once at startup
      self.make_transforms(transformation)

   def make_transforms(self, transformation):
      static_transformStamped = TransformStamped()
      static_transformStamped.header.stamp = self.get_clock().now().to_msg()
      static_transformStamped.header.frame_id = 'world'
      static_transformStamped.child_frame_id = sys.argv[1]
      static_transformStamped.transform.translation.x = float(sys.argv[2])
      static_transformStamped.transform.translation.y = float(sys.argv[3])
      static_transformStamped.transform.translation.z = float(sys.argv[4])
      quat = tf_transformations.quaternion_from_euler(
            float(sys.argv[5]), float(sys.argv[6]), float(sys.argv[7]))
      static_transformStamped.transform.rotation.x = quat[0]
      static_transformStamped.transform.rotation.y = quat[1]
      static_transformStamped.transform.rotation.z = quat[2]
      static_transformStamped.transform.rotation.w = quat[3]

      self._tf_publisher.sendTransform(static_transformStamped)


def main():
   logger = rclpy.logging.get_logger('logger')

   # obtain parameters from command line arguments
   if len(sys.argv) < 8:
      logger.info('Invalid number of parameters. Usage: \n'
                  '$ ros2 run learning_tf2_py static_turtle_tf2_broadcaster'
                  'child_frame_name x y z roll pitch yaw')
      sys.exit(0)
   else:
      if sys.argv[1] == 'world':
            logger.info('Your static turtle name cannot be "world"')
            sys.exit(0)

   # pass parameters and initialize node
   rclpy.init()
   node = StaticFramePublisher(sys.argv)
   try:
      rclpy.spin(node)
   except KeyboardInterrupt:
      pass

   rclpy.shutdown()

然后我们在package.xml中加入:

<exec_depend>geometry_msgs</exec_depend>
<exec_depend>rclpy</exec_depend>
<exec_depend>tf_transformations</exec_depend>
<exec_depend>tf2_ros</exec_depend>
<exec_depend>turtlesim</exec_depend>

在setup.py中的console_scripts中加入:

'static_turtle_tf2_broadcaster = learning_tf2_py.static_turtle_tf2_broadcaster:main',

然后:

cd dev_ws
rosdep install -i --from-path src --rosdistro foxy -y
colcon build --packages-select learning_tf2_py
. install/setup.bash

为看到我们确实成功发布静态转换，先打开:

ros2 topic echo /tf_static

然后:

ros2 run learning_tf2_py static_turtle_tf2_broadcaster mystaticturtle 0 0 1 0 0 0

会在ros2 topic echo /tf_static 所在窗口中显示类似如下信息:

transforms:
- header:
    stamp:
      sec: 1631355927
      nanosec: 346452972
    frame_id: world
  child_frame_id: mystaticturtle
  transform:
    translation:
      x: 0.0
      y: 0.0
      z: 1.0
    rotation:
      x: 0.0
      y: 0.0
      z: 0.0
      w: 1.0
---

实际开发过程中，其实并不需要自己来写这样的发布代码， 只需使用static_transform_publisher, 它既可以当作命令行工具使用，也可将其当作一个节点，然后加入到launch文件中，启动:

ros2 run tf2_ros static_transform_publisher x y z yaw pitch roll frame_id child_frame_id

或者(其中，qx,qy,qz,qw是四元数):

ros2 run tf2_ros static_transform_publisher x y z qx qy qz qw frame_id child_frame_id

或者加入到launch文件中:

from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
   return LaunchDescription([
      Node(
            package='tf2_ros',
            executable='static_transform_publisher',
            arguments = ['0', '0', '1', '0', '0', '0', 'world', 'mystaticturtle']
      ),
   ])

编写tf2 broadcaster

在dev_ws/src/learning_tf2_py/learning_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()

在package.xml及在setup.py分别加入:

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

 'turtle_tf2_broadcaster = learning_tf2_py.turtle_tf2_broadcaster:main',

然后创建launch文件:

cd dev_ws/src/learning_tf2_py/learning_tf2_py/
mkdir 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'}
            ]
        ),
    ])

然后再在setup.py的data_files中加入:

(os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*.launch.py'))),

然后:

rosdep install -i --from-path src --rosdistro foxy -y
colcon build --packages-select learning_tf2_py
. install/setup.bash
 ros2 launch learning_tf2_py turtle_tf2_demo.launch.py

使用键盘控制并查看发布的tf2信息:

ros2 run turtlesim turtle_teleop_key
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]

编写tf2 listener

学习如何访问tf2的坐标系变换.

在learning_tf2_py/learning_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()

在setup.py的console_scripts加入:

 'turtle_tf2_listener = learning_tf2_py.turtle_tf2_listener:main',

然后在重新编写turtle_tf2_demo.launch.py:

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
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'}
            ]
        ),
        # new added
        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')}
            ]
        ),
    ])

然后构建、运行:

cd dev_ws/src
rosdep install -i --from-path src --rosdistro foxy -y
colcon build --packages-select learning_tf2_py
. install/setup.bash
ros2 launch learning_tf2_py turtle_tf2_demo.launch.py 

使用teleop_key控制小乌龟:

ros2 run turtlesim turtle_teleop_key 

现在小乌龟的运动方式应该如之前的demo演示的样子了。

添加坐标系

由上面的frames.pdf的树结构明显地显示，tf2将坐标系之间的关系组织成树结构，即一个坐标系其只可有一个父坐标系，而其可以有多个子坐标系。因此，如果要加入一个新的坐标系， 需要以tf2树中的某一个坐标系为父坐标系。

首先，尝试增加一个固定的坐标系, 即让新坐标系相对其父节点不变(本例中是carrot1相对于turtle1), 让它成为小乌龟turtle2的目标:

from geometry_msgs.msg import TransformStamped

import rclpy
from rclpy.node import Node

from tf2_ros import TransformBroadcaster


class FixedFrameBroadcaster(Node):

   def __init__(self):
      super().__init__('fixed_frame_tf2_broadcaster')
      self.br = TransformBroadcaster(self)
      self.timer = self.create_timer(0.1, self.broadcast_timer_callback)

   def broadcast_timer_callback(self):
      t = TransformStamped()
      t.header.stamp = self.get_clock().now().to_msg()
      t.header.frame_id = 'turtle1'
      t.child_frame_id = 'carrot1'
      t.transform.translation.x = 0.0
      t.transform.translation.y = 2.0
      t.transform.translation.z = 0.0
      t.transform.rotation.x = 0.0
      t.transform.rotation.y = 0.0
      t.transform.rotation.z = 0.0
      t.transform.rotation.w = 1.0

      self.br.sendTransform(t)


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

   rclpy.shutdown()

代码解释:

t = TransformStamped()
t.header.stamp = self.get_clock().now().to_msg()
t.header.frame_id = 'turtle1'
t.child_frame_id = 'carrot1'
t.transform.translation.x = 0.0
t.transform.translation.y = 2.0
t.transform.translation.z = 0.0

将carrot1的父节点设为turtle1, 并将其位置设置为在turtle1沿y轴方向两米处。

在setup.py的console_scripts中加入:

  'fixed_frame_tf2_broadcaster = learning_tf2_py.fixed_frame_tf2_broadcaster:main',

再在learning_tf2_py/launch目录中编写launch文件turtle_tf2_fixed_frame_demo.launch.py:

import os

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource

from launch_ros.actions import Node


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

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

此launch文件在启动turtle_tf2_demo.launch.py后，又将我们刚刚创建的carrot1启动，请注意此句launch_arguments={'target_frame': 'carrot1'}.items(),将参数target_frame设置为carrot1, 这是很关键的一句， 因为此句， 小乌龟turtle2不再追求小乌龟turtle1而改为追寻carrot1。:

构建并启动:

cd dev_ws/
colcon build --packages-select learning_tf2_py
. install/setup.bash
ros2 launch learning_tf2_py turtle_tf2_fixed_frame_demo.launch.py

此处也可不用在.launch.py文件中加入launch_arguments...这句，但需要在启动时remap target_frame到carrort1,具体地:

ros2 launch learning_tf2_py turtle_tf2_fixed_frame_demo.launch.py target_frame:=carrot1 

然后在另一个终端使用:

ros2 run tf2_tools view_frames.py 

点击frames.pdf: