ROS2与tf2入门教程-编写tf2静态广播器 (Python)

说明：

• 介绍如何将静态坐标系广播到 tf2

步骤：

• 创建工作空间tf2_ws

mkdir -p ~/tf2_ws/src

• 创建包,依赖tf_transformations rclpy tf2_ros geometry_msgs turtlesim

cd ~/tf2_ws/src       
ros2 pkg create --build-type ament_python learning_tf2_py --dependencies tf_transformations rclpy tf2_ros geometry_msgs turtlesim 

• 生成的目录树

~/tf2_ws/src/learning_tf2_py$ tree
.
├── learning_tf2_py
│   └── __init__.py
├── package.xml
├── resource
│   └── learning_tf2_py
├── setup.cfg
├── setup.py
└── test
    ├── test_copyright.py
    ├── test_flake8.py
    └── test_pep257.py

• 下载静态广播代码到包的script目录

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/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()

• 赋予执行权限

cd ~/tf2_ws/src/learning_tf2_py/static_turtle_tf2_broadcaster.py
sudo chmod +x static_turtle_tf2_broadcaster.py

• 增加入口节点,修改setup.py
• 修改后如下:

entry_points={
    'console_scripts': [
        'static_turtle_tf2_broadcaster = learning_tf2_py.static_turtle_tf2_broadcaster:main',
    ],

• 检查依赖项

cd ~/tf2_ws/
rosdep install -i --from-path src --rosdistro galactic -y

• 指定构建learning_tf2_py包,并使用symlink的方式

cd ~/tf2_ws/
colcon build --symlink-install --packages-select learning_tf2_py

• 加载工作空间

. ~/tf2_ws/install/local_setup.bash

测试:

• 新开终端,检查静态转换是否已发布

ros2 topic echo /tf_static

• 新开终端,运行节点

. ~/tf2_ws/install/local_setup.bash
ros2 run learning_tf2_py static_turtle_tf2_broadcaster mystaticturtle 0 0 1 0 0 0

• 结果如下

transforms:
- header:
   stamp:
      sec: 1622908754
      nanosec: 208515730
   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

• 这会为 mystaticturtle 设置海龟姿势广播，使其漂浮在地面以上 1 米处。
• 本教程旨在展示如何使用 StaticTransformBroadcaster 发布静态转换。
• 在您的实际开发过程中，您不必自己编写此代码，而应使用专用的 tf2_ros 工具来编写。
• tf2_ros 提供了一个名为 static_transform_publisher 的可执行文件，可以用作命令行工具或可以添加到启动文件的节点

tf2_ros工具

• 使用以米为单位的 x/y/z 偏移量和以弧度为单位的滚动/俯仰/偏航向 tf2 发布静态坐标变换。
• 在我们的例子中，roll/pitch/yaw 分别指的是围绕 x/y/z 轴的旋转。

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

• 使用以米和四元数为单位的 x/y/z 偏移量将静态坐标变换发布到 tf2

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

• static_transform_publisher被设计为手动使用的命令行工具
• 可通过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 = ['--x', '0', '--y', '0', '--z', '1', '--yaw', '0', '--pitch', '0', '--roll', '0', '--frame-id', 'world', '--child-frame-id', 'mystaticturtle']
      ),
   ])

• 请注意，除了--frame-id 和 --child-frame-id 之外的所有参数都是可选的；

参考:

• 资料来源