静态变换在机器人系统中非常有用，一般用来描述机器人基坐标系与相对其静止的其他坐标系之间的关系，比如雷达坐标系、相机坐标系等。本篇我们就来看一下，如何通过Python编程，实现静态坐标系变换的广播。

一、创建功能包

首先我们创建一个名为learning_tf2_py的功能包，需要依赖rclpy、tf2_ros、geometry_msgs和turtlesim这几个依赖项。
ros2 pkg create --build-type ament_python learning_tf2_py
终端中提示如下内容，说明功能包创建成功。

二、编写静态广播器节点

在创建好的功能包中找到learning_tf2_py/learning_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()

2.1 代码解析

我们来分析一下代码。
from geometry_msgs.msg import TransformStamped

from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster
import tf_transformations
首先我们导入了TransformStamped这个模块，后续会使用到其中坐标系的描述消息TransformStamped。然后还导入了tf2的两个模块，其中tf_transformations会提供欧拉角和四元数的转换公式，StaticTransformBroadcaster则是用来实例化静态坐标变化广播器的。
self._tf_publisher = StaticTransformBroadcaster(self)
self.make_transforms(transformation)
接下来，使用StaticFramePublisher的构造函数初始化节点，然后创建了一个广播器StaticTransformBroadcaster，用于广播静态的坐标变换信息。
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]
这里创建了一个TransformStamped对象，作为后续发送坐标变换信息的消息载体，在发布之前，我们还需要给它设置一下具体的消息数据：
（1）时间戳：这里使用了当前的时间get_clock().now()
（2）父坐标系：这里是world
（3）子坐标系：这里使用运行终端传入的坐标系名称
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)
最后就是将设置好的静态坐标变换信息广播出去了。

2.2 修改package.xml文件

打开功能包的package.xml文件，首先修改其中的基本描述信息：
<description>Examples of static transform broadcaster using rclcpp</description>
<maintainer email="huchunxu@guyuehome.com">Hu Chunxu</maintainer>
<license>Apache License 2.0</license>
然后设置功能包的依赖项：
<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>

2.3 添加程序入口

再打开setup.py文件，添加如下程序入口的设置：
'static_turtle_tf2_broadcaster = learning_tf2_py.static_turtle_tf2_broadcaster:main',

三、编译和运行

收下确定所有依赖项已经成功安装：
rosdep install -i --from-path src --rosdistro galactic -y
然后在工作空间的根目录下编译功能包：
colcon build --packages-select learning_tf2_py
编译成功后打开一个新终端，设置环境变量后即可运行静态坐标变换的节点了：
. install/setup.bash
ros2 run learning_tf2_py static_turtle_tf2_broadcaster mystaticturtle 0 0 1 0 0 0
 
以上命令会广播一个mystaticturtle坐标系相对于world坐标系，在z向上产生1m偏移的坐标系变换关系。
我们可以通过静态坐标系的话题来查看数据是否正常。
ros2 topic echo /tf_static
正常情况下，我们将看到：
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

四、静态坐标系变换更合适的广播方式

以上我们通过代码实现了静态坐标系的广播，但是对于静态坐标系变换这种常用的功能，写代码还是有点复杂，接下来我们就来看下另外一种更为灵活的方法。
tf2_ros功能包中已经提供的static_transform_publisher节点功能，使用以下命令方式即可实现任意两个坐标系的静态广播。
# 使用欧拉角描述姿态
ros2 run tf2_ros static_transform_publisher x y z yaw pitch roll frame_id child_frame_id
# 使用四元数描述姿态
ros2 run tf2_ros static_transform_publisher x y z qx qy qz qw frame_id child_frame_id
大家需要注意，其中涉及到的单位，平移使用的是m，旋转使用的是弧度。
此外，我们还可以在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']
      ),
   ])
好啦，本篇我们讲解了如何在ROS2中实现某两个相对静止坐标系之间变换关系的广播，通过Python代码编程或者static_transform_publisher节点均可实现类似的功能。