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

一、编写广播器节点

继续使用之前创建的learning_tf2_cpp功能包，在src文件夹中创建今天需要用到的代码文件turtle_tf2_broadcaster.cpp，然后复制粘贴如下代码：
#include <geometry_msgs/msg/transform_stamped.hpp>

#include <rclcpp/rclcpp.hpp>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>
#include <turtlesim/msg/pose.hpp>

#include <memory>
#include <string>

using std::placeholders::_1;

class FramePublisher : public rclcpp::Node
{
public:
  FramePublisher()
  : Node("turtle_tf2_frame_publisher")
  {
    // Declare and acquire `turtlename` parameter
    this->declare_parameter<std::string>("turtlename", "turtle");
    this->get_parameter("turtlename", turtlename_);

    // Initialize the transform broadcaster
    tf_broadcaster_ =
      std::make_unique<tf2_ros::TransformBroadcaster>(*this);

    // Subscribe to a turtle{1}{2}/pose topic and call handle_turtle_pose
    // callback function on each message
    std::ostringstream stream;
    stream << "/" << turtlename_.c_str() << "/pose";
    std::string topic_name = stream.str();

    subscription_ = this->create_subscription<turtlesim::msg::Pose>(
      topic_name, 10,
      std::bind(&FramePublisher::handle_turtle_pose, this, _1));
  }

private:
  void handle_turtle_pose(const std::shared_ptr<turtlesim::msg::Pose> msg)
  {
    rclcpp::Time now = this->get_clock()->now();
    geometry_msgs::msg::TransformStamped t;

    // Read message content and assign it to
    // corresponding tf variables
    t.header.stamp = now;
    t.header.frame_id = "world";
    t.child_frame_id = turtlename_.c_str();

    // 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
    tf2::Quaternion q;
    q.setRPY(0, 0, msg->theta);
    t.transform.rotation.x = q.x();
    t.transform.rotation.y = q.y();
    t.transform.rotation.z = q.z();
    t.transform.rotation.w = q.w();

    // Send the transformation
    tf_broadcaster_->sendTransform(t);
  }
  rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr subscription_;
  std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
  std::string turtlename_;
};

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<FramePublisher>());
  rclcpp::shutdown();
  return 0;
}

1.1 代码解析

我们来分析一下代码。
this->declare_parameter<std::string>("turtlename", "turtle");
this->get_parameter("turtlename", turtlename_);
在FramePublisher类的构造函数中，首先声明了一个参数“turtlename”，并动态获取该参数的值，表示海龟的名字，比如turtle1和turtle2。
subscription_ = this->create_subscription<turtlesim::msg::Pose>(
topic_name, 10,
std::bind(&FramePublisher::handle_turtle_pose, this, _1));
接下来订阅话题turtleX/pose，并且绑定回调函数handle_turtle_pose。
rclcpp::Time now = this->get_clock()->now();
geometry_msgs::msg::TransformStamped t;

// Read message content and assign it to
// corresponding tf variables
t.header.stamp = now;
t.header.frame_id = "world";
t.child_frame_id = turtlename_.c_str();
紧接着创建一个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
tf2::Quaternion q;
q.setRPY(0, 0, msg->theta);
t.transform.rotation.x = q.x();
t.transform.rotation.y = q.y();
t.transform.rotation.z = q.z();
t.transform.rotation.w = q.w();
这里我们将海龟的坐标和朝向变换成坐标系的位姿变换。
// Send the transformation
tf_broadcaster_->sendTransform(t);
最后也是最重要的一步，广播tf，让其他节点都可以知道更新后的坐标系关系。

1.2 添加编译选项

打开包的CMakeLists.txt文件，添加代码文件的编译选项：
add_executable(turtle_tf2_broadcaster src/turtle_tf2_broadcaster.cpp)
ament_target_dependencies(
    turtle_tf2_broadcaster
    geometry_msgs
    rclcpp
    tf2
    tf2_ros
    turtlesim
)
还有安装路径：
install(TARGETS
    turtle_tf2_broadcaster
    DESTINATION lib/${PROJECT_NAME})

二、编写启动文件

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

2.1 添加依赖项

打开功能包的package.xml文件，添加如下依赖项：
<exec_depend>launch</exec_depend>
<exec_depend>launch_ros</exec_depend>
2.2 修改CMakeLists.txt
打开功能包的CMakeLists.txt文件，添加launch文件的安装路径：
install(DIRECTORY
    launch
    DESTINATION share/${PROJECT_NAME}
)
三、编译与运行
在编译之前，首先确定所有依赖项正常安装：
rosdep install -i --from-path src --rosdistro foxy -y
然后编译工作空间。
colcon build --packages-select learning_tf2_cpp
接下来就可以运行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的广播器，广播任意两个坐标系之间的位姿关系。