前边我们学习了如何在ROS2中创建一个tf2的静态广播器，机器人系统中还有很多位姿关系会变化的坐标系，这就需要我们在代码中动态广播坐标系的变换，本篇我们就来学习如何使用C++编写一个动态的广播器。

一、编写广播器节点

继续使用之前创建的learning_tf2_py功能包，在src文件夹中创建今天需要用到的代码文件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()

1.1 代码解析

我们来分析一下代码。
self.declare_parameter('turtlename', 'turtle')
self.turtlename = self.get_parameter(
    'turtlename').get_parameter_value().string_value
在FramePublisher类的初始化函数中，首先声明了一个参数“turtlename”，并动态获取该参数的值，表示海龟的名字，比如turtle1或者turtle2。
self .subscription = self.create_subscription(
    Pose,
    f'/{self.turtlename}/pose',
    self.handle_turtle_pose,
    1)
接下来订阅话题turtleX/pose，并且绑定回调函数handle_turtle_pose。
t.header.stamp = self.get_clock().now().to_msg()
t.header.frame_id = 'world'
t.child_frame_id = self.turtlename
紧接着创建一个tf消息TransformStamped，并且进行赋值：
（1）时间戳：使用当前时间，this->get_clock()->now()
（2）父坐标系：这里是world
（3）子坐标系：这里使用海龟的名字作为子坐标系
后续的回调函数中，就会不断更新word与turtleX坐标系之间的位姿关系了。
# 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)
最后也是最重要的一步，广播tf，让其他节点都可以知道更新后的坐标系关系。

1.2 添加依赖项

打开功能包的package.xml文件，添加以下依赖项：
<exec_depend>launch</exec_depend>
<exec_depend>launch_ros</exec_depend>

1.3 添加程序入口

打开功能包的setup.py文件，添加如下程序入口：
'console_scripts': [
    'turtle_tf2_broadcaster = learning_tf2_py.turtle_tf2_broadcaster:main',
],

二、编写启动文件

由于节点较多，这里我们来编写一个Launch文件，在功能包中创建launch文件夹，并在其中创建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'}
            ]
        ),
    ])
在这个Launch文件中，我们启动了来你两个节点，一个是turtlesim_node海龟仿真器节点，另一个是刚刚完成的turtle_tf2_broadcaster节点，所以可以猜测到后者将根据前者发布的pose话题，更新tf中海龟坐标系与世界坐标系之间的位姿关系。

2.1 更新setup.py

打开功能包的setup.py文件，更新如下Launch文件路径的内容：
import os
from glob import glob

....

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
接下来就可以运行Launch文件了。
. install/setup.bash
ros2 launch learning_tf2_cpp turtle_tf2_demo.launch.py
运行成功后，再新打开一个终端，并且启动键盘控制节点：
ros2 run turtlesim turtle_teleop_key
此时就可以通过键盘控制小海龟运动了。

再启动一个终端，通过tf2_echo检查tf是否正常广播了。
ros2 run tf2_ros tf2_echo world turtle1
终端中会不断刷新两个坐标系之间的位姿关系，控制海龟运动时，也可以看到数据在不断刷新。
好啦，本篇我们就学习了如何通过tf2的广播器，广播任意两个坐标系之间的位姿关系。