ROS220-ROS2工具：坐标变换（三）坐标变换广播

简介

坐标系相对关系主要有两种：静态坐标系相对关系与动态坐标系相对关系：

• 静态坐标系相对关系 是指两个坐标系之间的相对位置是 固定不变 的，比如：车辆上的雷达、摄像头等组件一般是固定式的，那么雷达坐标系相对于车辆底盘坐标系或摄像头坐标系相对于车辆底盘坐标系就是一种静态关系。

• 动态坐标系相对关系 是指两个坐标系之间的相对位置关系是 动态改变 的，比如：车辆上机械臂的关节或夹爪、多车编队中不同车辆等都是可以运动的，那么机械臂的关节或夹爪坐标系相对车辆底盘坐标系或不同车辆坐标系的相对关系就是一种动态关系。

本章会主要介绍如何实现静态坐标变换广播与动态坐标变换广播。在此基础上，还会演示如何发布坐标点消息。

案例梳理

以下有两个案例需求：

1. 有一无人车，在无人车底盘上装有 固定式 的雷达与摄像头。车辆底盘、雷达与摄像头各对应一坐标系，且各坐标系的原点取其几何中心。现已知雷达坐标系相对于底盘坐标系的三维平移量分别为：x方向0.4米，y方向0米，z方向0.2米，无旋转。摄像头坐标系相对于底盘坐标系的三维平移量分别为：x方向-0.5米，y方向0米，z方向0.4米，无旋转。请 静态广播 雷达与底盘的坐标系相对关系与摄像头与底盘的坐标系相对关系，并在 rviz2 中查看广播的结果。
2. 启动 turtlesim_node，设该节点中窗体为一个世界坐标系(左下角为坐标系原点)，乌龟是另一个坐标系(乌龟可以通过键盘控制运动)，请 动态广播 乌龟坐标系与世界坐标系的相对关系。

在上述案例中，案例1需要使用到静态坐标变换，案例2则需要使用动态坐标变换，不论何种实现其关注的要素都有两个：

1. 如何广播坐标系相对关系；
2. 如何使用 rviz2 显示坐标系相对关系。

这两种实现的通用主要步骤如下：

1. 编写广播实现原文件；
2. 编辑配置文件；
3. 编译；
4. 运行；
5. 在 rviz2 中查看坐标系关系。

以编码方式实现静态坐标变换的流程与实现动态坐标变换的流程相类似，这两个案例我们会采用 C++ 和 Python 分别实现，二者都遵循上述实现流程。

静态广播不止编码

需要说明的是，静态广播器 除了可以以编码的方式实现外，在 tf2 中还内置了相关工具，可以无需编码，直接执行节点并传入表示坐标系相对关系的参数，实现静态坐标系关系的发布。对于静态广播而言也比较建议直接使用该方法。

而动态广播器 没有提供类似的工具。

准备工作

终端下进入工作空间的src目录，调用如下两条命令分别创建C++功能包和Python功能包。

ros2 pkg create cpp03_tf_broadcaster --build-type ament_cmake --dependencies rclcpp tf2 tf2_ros geometry_msgs turtlesim
ros2 pkg create py03_tf_broadcaster --build-type ament_python --dependencies rclpy tf_transformations tf2_ros geometry_msgs turtlesim

静态广播

1. 使用命令方式

在 tf2_ros功能包中提供了一个名为static_transform_publisher的可执行文件，通过该文件可以直接广播静态坐标系关系，其使用语法如下。

格式1

使用以米为单位的 x/y/z 偏移量和以弧度为单位的roll/pitch/yaw（可直译为滚动/俯仰/偏航，分别指的是围绕 x/y/z 轴的旋转）向 tf2 发布静态坐标变换。

ros2 run tf2_ros static_transform_publisher --x x --y y --z z --yaw yaw --pitch pitch --roll roll --frame-id frame_id --child-frame-id child_frame_id

例如使用：

ros2 run tf2_ros static_transform_publisher --frame-id base_link --child-frame-id laser --x 1.0

则会创建两个坐标系:

格式2

使用以米为单位的 x/y/z 偏移量和 qx/qy/qz/qw 四元数向 tf2 发布静态坐标变换。

ros2 run tf2_ros static_transform_publisher --x x --y y --z z --qx qx --qy qy --qz qz --qw qw --frame-id frame_id --child-frame-id child_frame_id

例如使用：

ros2 run tf2_ros static_transform_publisher --frame-id base_link --child-frame-id laser --x 1.0 --qz -0.247404 --qw 0.968912

则会创建两个坐标系:

注

① 在父子坐标系有偏移关系时，一般会生成一个箭头，由子级坐标系原点指向父级坐标系原点。其中生成的箭头为黄色箭体，粉色箭头。

② 在上述两种格式中除了用于表示父级坐标系的--frame-id和用于表示子级坐标系的--child-frame-id之外，其他参数都是可选的，如果未指定特定选项，那么将直接使用默认值。

例如，假设除 id 之外其他参数都使用默认值，

ros2 run tf2_ros static_transform_publisher --frame-id base_link --child-frame-id laser

则会创建两个原点和轴都重叠的坐标系:

③ 如何使用 Rviz2 查看坐标系关系? 👈

新建终端，通过命令 rviz2 打开 Rviz2 并配置相关插件查看坐标变换消息：

• 将 Global Options 中的 Fixed Frame 设置为 base_link；
• 点击 add 按钮添加 TF 插件；
• 勾选 TF 插件中的 show names。

之后右侧的 Grid 窗口中将以图形化的方式显示坐标变换关系。

在此基于 案例1，我们可以使用如下指令实现：

ros2 run tf2_ros static_transform_publisher --frame-id base_link --child-frame-id laser --x 0.4 --y 0.0 --z 0.2
ros2 run tf2_ros static_transform_publisher --frame-id base_link --child-frame-id camera --x -0.5 --y 0.0 --z 0.4

如下图：

则会创建如下三个坐标系：

2. 使用编码方式

Ⅰ.编写广播实现原文件

C++

功能包 cpp03_tf_broadcaster 的 src 目录下，新建 C++ 文件 demo01_static_tf_broadcaster.cpp，并编辑文件，输入如下内容：

/*  
  需求：编写静态坐标变换广播器，执行该程序时需传入两个坐标系的相对位姿关系以及父子级坐标系id，
       ros2 run 包 可执行程序 x y z roll pitch yaw frame_id child_frame_id
  步骤：
    1.包含头文件；
    2.判断终端传入的参数是否合法；
    3.初始化 ROS 客户端；
    4.定义节点类；
      4-1.创建静态坐标变换广播对象；
      4-2.从传入参数中组织并发布消息。
    5.调用 spin 函数，并传入对象指针；
    6.释放资源。

*/

// 1.包含头文件；
#include <geometry_msgs/msg/transform_stamped.hpp>

#include <rclcpp/rclcpp.hpp>
// 四元数转换用的库
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/static_transform_broadcaster.h>

using std::placeholders::_1;

// 4.定义节点类；
class TFStaticBroadcaster : public rclcpp::Node
{
public:
  explicit TFStaticBroadcaster(char * transformation[]): Node("tf_static_broadcaster_node_cpp")
  {
    // 4-1.创建静态坐标变换发布方；
    tf_publisher_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);
    // 4-2.组织并发布消息。（这边推荐另创建一函数）
    this->make_transforms(transformation);
  }

private:
  // 4-2.组织并发布消息。
  void make_transforms(char * transformation[])
  {
    // 组织消息
    geometry_msgs::msg::TransformStamped t;

    t.header.stamp = this->now(); // 时间戳
    t.header.frame_id = transformation[7]; // 父级坐标系
    t.child_frame_id = transformation[8]; // 子级坐标系

    t.transform.translation.x = atof(transformation[1]);
    t.transform.translation.y = atof(transformation[2]);
    t.transform.translation.z = atof(transformation[3]);

    // 创建四元数对象
    tf2::Quaternion q;
    q.setRPY(
      atof(transformation[4]),
      atof(transformation[5]),
      atof(transformation[6]));
    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);
    RCLCPP_INFO(get_logger(), "已成功发布广播！");
  }
  std::shared_ptr<tf2_ros::StaticTransformBroadcaster> tf_publisher_;
};

int main(int argc, char * argv[])
{
  // 2.判断终端传入的参数是否合法；
  auto logger = rclcpp::get_logger("logger");

  if (argc != 9) {
    RCLCPP_INFO(
      logger, "运行程序时请按照：x y z roll pitch yaw frame_id child_frame_id 的格式传入参数");
    return 1;
  }

  // 3.初始化 ROS 客户端；
  rclcpp::init(argc, argv);
  // 5.调用 spin 函数，并传入对象指针；
  rclcpp::spin(std::make_shared<TFStaticBroadcaster>(argv));
  // 6.释放资源。
  rclcpp::shutdown();
  return 0;
}

注：在以上代码中：

...
public:
  explicit TFStaticBroadcaster(char * transformation[]): Node("tf_static_broadcaster_node_cpp")
  ...

这里的 explicit 用于修饰类的构造函数，它可以禁止编译器对被修饰的构造函数执行 隐式类型转换 / 隐式构造 行为。

Python

功能包 py03_tf_broadcaster 的 py03_tf_broadcaster 目录下，新建 Python 文件 demo01_static_tf_broadcaster_py.py，并编辑文件，输入如下内容：

"""  
    需求：编写静态坐标变换广播器，执行该程序时需传入两个坐标系的相对位姿关系以及父子级坐标系id，
       ros2 run 包 可执行程序 x y z roll pitch yaw frame_id child_frame_id
    步骤：
        1.导包；
        2.判断终端传入的参数是否合法；
        3.初始化 ROS 客户端；
        4.定义节点类；
            4-1.创建静态坐标变换广播对象；
            4-2.从传入参数中组织并发布消息。
        5.调用 spin 函数，并传入节点对象；
        6.释放资源。

"""
# 1.导包；
import rclpy
from rclpy.node import Node
import sys
from rclpy.logging import get_logger
from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster
from geometry_msgs.msg import TransformStamped
from geometry_msgs.msg import Quaternion
import tf_transformations

# 4.自定义节点类；
class TFStaticBroadcasterPy(Node):
    def __init__(self, argv):
        super().__init__("tf_static_broadcaster_py_node_py")
        # 4-1.创建静态坐标变换广播对象；
        self.broadcaster = StaticTransformBroadcaster(self)
        # 4-2.从传入参数中组织并发布消息。
        self.make_transforms(argv)

    def make_transforms(self, argv):

        t = TransformStamped()
        # 组织消息
        t.header.stamp = self.get_clock().now().to_msg() # 设置时间戳
        t.header.frame_id = argv[7]
        t.child_frame_id = argv[8]

        t.transform.translation.x = float(argv[1])
        t.transform.translation.y = float(argv[2])
        t.transform.translation.z = float(argv[3])

        # 设置四元数
        q = tf_transformations.quaternion_from_euler(float(argv[4]),float(argv[5]),float(argv[6]))
        t.transform.rotation.x = q[0]
        t.transform.rotation.y = q[1]
        t.transform.rotation.z = q[2]
        t.transform.rotation.w = q[3]

        self.broadcaster.sendTransform(t)

        self.get_logger().info("已成功发布广播！")

def main():
    # 2. 判断终端传入的参数是否合法；
    if len(sys.argv) != 9:
        get_logger("rclpy").error("运行程序时请按照：x y z roll pitch yaw frame_id child_frame_id 的格式传入参数")
        return
    # 3.初始化ROS2客户端；
    rclpy.init()
    # 5.调用spain函数，并传入节点对象；
    rclpy.spin(TFStaticBroadcasterPy(sys.argv))
    # 6.资源释放。 
    rclpy.shutdown()

if __name__ == '__main__':
    main()

注：

这里的 import tf_transformations 为使用了 sudo apt install ros-jazzy-tf-transformations 所安装的依赖包实现欧拉角和四元数之间的相互转换。该依赖包通常适用于将 ROS 的代码移植到 ROS2 上时使用。 如果是直接使用 ROS2 进行项目开发，实际上更推荐使用 transforms3d 库以实现欧拉角和四元数之间的相互转换。在官方教程中，其甚至是使用的自定义函数。

Ⅱ.编辑配置文件

C++

在 CMakeLists.txt 中发布和订阅程序核心配置如下：

find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(tf2 REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(turtlesim REQUIRED)

add_executable(demo01_static_tf_broadcaster src/demo01_static_tf_broadcaster.cpp)
ament_target_dependencies(
  demo01_static_tf_broadcaster
  "rclcpp"
  "tf2"
  "tf2_ros"
  "geometry_msgs"
  "turtlesim"
)

install(TARGETS demo01_static_tf_broadcaster
  DESTINATION lib/${PROJECT_NAME})

Python

在 setup.py 中针对 entry_points 字段的 console_scripts 添加如下内容：

...
entry_points={
    'console_scripts': [
        'demo01_static_tf_broadcaster_py = py03_tf_broadcaster.demo01_static_tf_broadcaster_py:main'
    ],
},
...

Ⅲ.编译

终端中进入当前工作空间，编译功能包：

C++

colcon build --packages-select cpp03_tf_broadcaster

Python

colcon build --packages-select py03_tf_broadcaster

Ⅳ.运行

当前工作空间下，启动两个终端，终端1输入如下命令发布雷达（laser）相对于底盘（base_link）的静态坐标变换：

C++

. install/setup.bash 
ros2 run cpp03_tf_broadcaster demo01_static_tf_broadcaster 0.4 0 0.2 0 0 0 base_link laser

终端2输入如下命令发布摄像头（camera）相对于底盘（base_link）的静态坐标变换：

. install/setup.bash
ros2 run cpp03_tf_broadcaster demo01_static_tf_broadcaster -0.5 0 0.4 0 0 0 base_link camera

此时就可以看到：

Python

. install/setup.bash 
ros2 run py03_tf_broadcaster demo01_static_tf_broadcaster_py  0.4 0 0.2 0 0 0 base_link laser

终端2输入如下命令发布摄像头（camera）相对于底盘（base_link）的静态坐标变换：

. install/setup.bash
ros2 run cpp03_tf_broadcaster demo01_static_tf_broadcaster_py  -0.5 0 0.4 0 0 0 base_link camera

此时就可以看到：

Ⅴ.在 Rviz2 中查看坐标系关系

你可以参考 前文注释 中的相关内容以打开 Rviz2。

所以，不用自己做话题的发布吗?

虽然咱之前提到过说，“坐标变换底层上就是话题通信”，但是在书写坐标变换时，我们 不需要自己发布话题，其在源码上会帮我们自己生成。 他的默认值为 /tf_static，你也可以通过查看 tf2_ros::StaticTransformBroadcaster 或者 tf2_ros.static_transform_broadcaster 的源码以验证：

节点也不用吗？

确实。虽然“无论创建何种通信对象，都需要依赖于一个节点”，但是以 c++ 为例子：

...
    // 4-1.创建静态坐标变换发布方；
    tf_publisher_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);

这里只需要传入当前对象 this 即可。

动态广播

动态广播因为需要编写更多的代码逻辑，因此没有能够使用终端指令就可以直接设置的命令方法。

在代码配置流程上，其与前文所述的静态广播 所使用的相关流程类似，区别点在于其需要自己订阅节点，并依据订阅节点的动向而动态的发布数据。在订阅节点时，其使用的回调函数执行逻辑也更倾向于“回调一次函数，就组织一次消息，然后发布一次数据”。

Ⅰ.编写广播实现原文件

C++

功能包 cpp03_tf_broadcaster 的 src 目录下，新建 C++ 文件 demo02_dynamic_tf_broadcaster.cpp，并编辑文件，输入如下内容：

/*  
  需求：启动 turtlesim_node节点，编写程序，发布乌龟 (turtle1) 相对于窗体 (world) 的位资
  步骤：
    1.包含头文件；
    2.初始化 ROS 客户端；
    3.定义节点类；
      3-1.创建动态坐标变换广播对象；
      3-2.创建一个乌龟位姿订阅方
      3-3.回调函数中获取乌龟位姿，生成相对信息并发布。
    4.调用 spin 函数，并传入对象指针；
    5.释放资源。

*/

// 1.包含头文件；
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <turtlesim/msg/pose.hpp>

#include <rclcpp/rclcpp.hpp>
// 四元数转换用的库
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>


using std::placeholders::_1;

// 3.定义节点类；
class TFDynamicBroadcaster : public rclcpp::Node
{
public:
  explicit TFDynamicBroadcaster(): Node("tf_dynamic_broadcaster_node_cpp")
  {
    // 3-1.创建动态坐标变换广播对象；
    tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);

    // 3-2.创建一个乌龟位姿订阅方
    pose_sub_ = this->create_subscription<turtlesim::msg::Pose>("/turtle1/pose", 10, 
      std::bind(&TFDynamicBroadcaster::do_pose, this, std::placeholders::_1)
    );
  }

private:
  // 3-3.回调函数中获取乌龟位姿，生成相对信息并发布。
  void do_pose(const turtlesim::msg::Pose & pose)
  {  
    // 组织消息
    geometry_msgs::msg::TransformStamped ts;
    ts.header.stamp = this->now();
    ts.header.frame_id = "world";
    ts.child_frame_id = "turtle1";

    ts.transform.translation.x = pose.x;
    ts.transform.translation.y = pose.y;
    ts.transform.translation.z = 0.0;

    // 从欧拉角转换出四元数
    // 但是作为 2D 乌龟，只有 yaw，没有 pitch 和 row （定为0）
    tf2::Quaternion q;
    q.setRPY(0, 0, pose.theta);

    ts.transform.rotation.x = q.x();
    ts.transform.rotation.y = q.y();
    ts.transform.rotation.z = q.z();
    ts.transform.rotation.w = q.w();

    // 发布消息
    tf_broadcaster_->sendTransform(ts);
    RCLCPP_INFO(get_logger(), "已成功发布广播！");
  }

  std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
  rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr pose_sub_;
};

int main(int argc, char * argv[])
{

  // 2.初始化 ROS 客户端；
  rclcpp::init(argc, argv);
  // 4.调用 spin 函数，并传入对象指针；
  rclcpp::spin(std::make_shared<TFDynamicBroadcaster>());
  // 5.释放资源。
  rclcpp::shutdown();
  return 0;
}

Python

"""  
    需求：启动 turtlesim_node节点，编写程序，发布乌龟 (turtle1) 相对于窗体 (world) 的位资
    步骤：
        1.包含头文件；
        2.初始化 ROS 客户端；
        3.定义节点类；
            3-1.创建动态坐标变换广播对象；
            3-2.创建一个乌龟位姿订阅方
            3-3.回调函数中获取乌龟位姿，生成相对信息并发布。
        4.调用 spin 函数，并传入对象指针；
        5.释放资源。

"""
# 1.导包；
import rclpy
from rclpy.node import Node
from tf2_ros import TransformBroadcaster
from turtlesim.msg import Pose
from geometry_msgs.msg import TransformStamped
import tf_transformations


# 3.自定义节点类；
class TFDynamicBroadcasterPy(Node):
    def __init__(self):
        super().__init__("tf_dynamic_broadcaster_py_node_py")
        # 3-1.创建动态坐标变换广播对象
        self.broadcaster = TransformBroadcaster(self)
        # 3-2.创建一个乌龟位姿订阅方
        self.sub = self.create_subscription(
            Pose, "/turtle1/pose", self.do_pose, 10
        )
        # 3-3.回调函数中获取乌龟位姿，生成相对信息并发布。    
    def do_pose(self, pose):
        ts = TransformStamped()
        # 组织消息
        ts.header.stamp = self.get_clock().now().to_msg()
        ts.header.frame_id = "world"
        ts.child_frame_id = "turtle1"

        # 设置偏移量
        ts.transform.translation.x = pose.x;
        ts.transform.translation.y = pose.y;
        ts.transform.translation.z = 0.0;

        # 设置四元数
        q = tf_transformations.quaternion_from_euler(0.0, 0.0, pose.theta)
        ts.transform.rotation.x = q[0]
        ts.transform.rotation.y = q[1]
        ts.transform.rotation.z = q[2]
        ts.transform.rotation.w = q[3]

        # 发布消息
        self.broadcaster.sendTransform(ts)
        self.get_logger().info("正在发布广播中。。。")

def main():
    # 2.初始化ROS2客户端；
    rclpy.init()
    # 4.调用spain函数，并传入节点对象；
    rclpy.spin(TFDynamicBroadcasterPy())
    # 5.资源释放。 
    rclpy.shutdown()

if __name__ == '__main__':
    main()

Ⅱ.编辑配置文件

C++

在 CMakeLists.txt 中发布和订阅程序核心配置如下：

find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(tf2 REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(turtlesim REQUIRED)

...

add_executable(demo02_dynamic_tf_broadcaster src/demo02_dynamic_tf_broadcaster.cpp)

...

ament_target_dependencies(
  demo02_dynamic_tf_broadcaster
  "rclcpp"
  "tf2"
  "tf2_ros"
  "geometry_msgs"
  "turtlesim"
)

install(TARGETS demo01_static_tf_broadcaster demo02_dynamic_tf_broadcaster
  DESTINATION lib/${PROJECT_NAME})

Python

在 setup.py 中针对 entry_points 字段的 console_scripts 添加如下内容：

...
entry_points={
    'console_scripts': [
        ...
        'demo02_dynamic_tf_broadcaster_py = py03_tf_broadcaster.demo02_dynamic_tf_broadcaster_py:main'
    ],
},
...

Ⅲ.编译

终端中进入当前工作空间，编译功能包：

C++

colcon build --packages-select cpp03_tf_broadcaster

Python

colcon build --packages-select py03_tf_broadcaster

Ⅳ.运行

当前工作空间下，新建两个终端，终端1开启小海龟节点：

ros2 run turtlesim turtlesim_node

新建终端2，开启键盘控制节点：

ros2 run turtlesim turtle_teleop_key

新建终端3，开启动态广播：

C++

. install/setup.bash 
ros2 run cpp03_tf_broadcaster demo02_dynamic_tf_broadcaster

Python

. install/setup.bash 
ros2 run py03_tf_broadcaster demo01_static_tf_broadcaster_py  0.4 0 0.2 0 0 0 base_link laser

Ⅴ.在 Rviz2 中查看坐标系关系

在终端4开启 RVIZ2:

rviz2

你可以参考 前文注释 中的相关内容以调整 Rviz2 内部相关设置。

此时将鼠标焦点定在终端2就可以看到：