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

一、创建功能包

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

二、编写静态广播器节点

在创建好的功能包中创建learning_tf2_cpp/src文件夹，然后在src中创建一个名为static_turtle_tf2_broadcaster.cpp的代码文件，将如下代码复制粘贴进去。
#include <geometry_msgs/msg/transform_stamped.hpp>

#include <rclcpp/rclcpp.hpp>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/static_transform_broadcaster.h>

#include <memory>

using std::placeholders::_1;

class StaticFramePublisher : public rclcpp::Node
{
public:
  explicit StaticFramePublisher(char * transformation[])
  : Node("static_turtle_tf2_broadcaster")
  {
    tf_publisher_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);

    // Publish static transforms once at startup
    this->make_transforms(transformation);
  }

private:
  void make_transforms(char * transformation[])
  {
    rclcpp::Time now;
    geometry_msgs::msg::TransformStamped t;

    t.header.stamp = now;
    t.header.frame_id = "world";
    t.child_frame_id = transformation[1];

    t.transform.translation.x = atof(transformation[2]);
    t.transform.translation.y = atof(transformation[3]);
    t.transform.translation.z = atof(transformation[4]);
    tf2::Quaternion q;
    q.setRPY(
      atof(transformation[5]),
      atof(transformation[6]),
      atof(transformation[7]));
    t.transform.rotation.x = q.x();
    t.transform.rotation.y = q.y();
    t.transform.rotation.z = q.z();
    t.transform.rotation.w = q.w();

    tf_publisher_->sendTransform(t);
  }
  std::shared_ptr<tf2_ros::StaticTransformBroadcaster> tf_publisher_;
};

int main(int argc, char * argv[])
{
  auto logger = rclcpp::get_logger("logger");

  // Obtain parameters from command line arguments
  if (argc != 8) {
    RCLCPP_INFO(
      logger, "Invalid number of parameters\nusage: "
      "ros2 run learning_tf2_cpp static_turtle_tf2_broadcaster "
      "child_frame_name x y z roll pitch yaw");
    return 1;
  }

  // As the parent frame of the transform is `world`, it is
  // necessary to check that the frame name passed is different
  if (strcmp(argv[1], "world") == 0) {
    RCLCPP_INFO(logger, "Your static turtle name cannot be 'world'");
    return 1;
  }

  // Pass parameters and initialize node
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<StaticFramePublisher>(argv));
  rclcpp::shutdown();
  return 0;
}

2.1 代码解析

我们来分析一下代码。
#include <geometry_msgs/msg/transform_stamped.hpp>

#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/static_transform_broadcaster.h>
首先我们包含了transform_stamped.hpp这个头文件，后续会使用到其中坐标系的描述消息TransformStamped。然后还包含了tf2的两个头文件，其中tf2::Quaternion这个类会提供欧拉角和四元数的转换公式，static_transform_broadcaster.h中的StaticTransformBroadcaster则是用来实例化静态坐标变化广播器的。
tf_publisher_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);

this->make_transforms(transformation);
接下来，使用StaticFramePublisher的构造函数初始化节点，然后创建了一个广播器StaticTransformBroadcaster，用于广播静态的坐标变换信息。
rclcpp::Time now;
geometry_msgs::msg::TransformStamped t;

t.header.stamp = now;
t.header.frame_id = "world";
t.child_frame_id = transformation[1];
这里创建了一个TransformStamped对象，作为后续发送坐标变换信息的消息载体，在发布之前，我们还需要给它设置一下具体的消息数据：
（1）时间戳：这里使用了当前的时间rclcpp::Time
（2）父坐标系：这里是world
（3）子坐标系：这里使用运行终端传入的坐标系名称
t.transform.translation.x = atof(transformation[2]);
t.transform.translation.y = atof(transformation[3]);
t.transform.translation.z = atof(transformation[4]);
tf2::Quaternion q;
q.setRPY(
  atof(transformation[5]),
  atof(transformation[6]),
  atof(transformation[7]));
t.transform.rotation.x = q.x();
t.transform.rotation.y = q.y();
t.transform.rotation.z = q.z();
t.transform.rotation.w = q.w();
然后设置了海龟的姿态信息，包括平移和旋转。
tf_publisher_->sendTransform(t);
最后就是将设置好的静态坐标变换信息广播出去了。

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>
然后设置功能包的依赖项：
<depend>geometry_msgs</depend>
<depend>rclcpp</depend>
<depend>tf2</depend>
<depend>tf2_ros</depend>
<depend>turtlesim</depend>

2.3 修改CMakeLists.txt文件

再打开CMakeLists.txt文件，添加如下对依赖项的搜索设置：
find_package(geometry_msgs REQUIRED)
find_package(rclcpp REQUIRED)
find_package(tf2 REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(turtlesim REQUIRED)
然后添加对之前代码文件的编译设置：
add_executable(static_turtle_tf2_broadcaster src/static_turtle_tf2_broadcaster.cpp)
ament_target_dependencies(
   static_turtle_tf2_broadcaster
   geometry_msgs
   rclcpp
   tf2
   tf2_ros
   turtlesim
)
最后设置编译后的安装路径：
install(TARGETS
   static_turtle_tf2_broadcaster
   DESTINATION lib/${PROJECT_NAME})

三、编译和运行

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

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

以上我们通过代码实现了静态坐标系的广播，但是对于静态坐标系变换这种常用的功能，写代码还是有点复杂，接下来我们就来看下另外一种更为灵活的方法。
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中实现某两个相对静止坐标系之间变换关系的广播，通过C++代码编程或者static_transform_publisher节点均可实现类似的功能。