ROS 2边学边练（33）-- 写一个静态广播（C++）

前言

        通过这一篇我们将了解并学习到如何广播静态坐标变换到tf2（由tf2来转换这些坐标系）。

        发布静态变换对于定义机器人底座与其传感器或非移动部件之间的关系非常有用。例如，在以激光扫描仪中心的坐标系中推理激光扫描测量数据是最简单的。

        这是一个独立的教程，涵盖了静态变换的基础知识，包含两个部分。在第一部分，我们将编写代码以将静态变换发布到tf2。在第二部分，我们将说明如何在tf2_ros中使用命令行工具static_transform_publisher。

        后面的接着两篇博文中（下一篇可能会穿插一篇有关ROS世界坐标系的博文），我们将重新学习下上一篇博文（《初识tf2》）的一些知识，在后面的其他博文将会涉及到tf2的更多高级功能。

动动手

创建功能包

        我们在之前创建的某个工作空间（比如ros2_ws）或新建一个工作空间内创建一个learning_tf2_cpp包(工作空间的src路径下)：

$ros2 pkg create --build-type ament_cmake --license Apache-2.0 --dependencies geometry_msgs rclcpp tf2 tf2_ros turtlesim -- learning_tf2_cpp

写个静态广播节点

        在learning_tf2_cpp/src下，执行如下命令下载官方为我们准备好的源文件：

$wget https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_cpp/src/static_turtle_tf2_broadcaster.cpp

        下载下来的static_turtle_tf2_broadcaster.cpp内容如下：

#include <memory>

#include "geometry_msgs/msg/transform_stamped.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2_ros/static_transform_broadcaster.h"

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

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

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

    t.header.stamp = this->get_clock()->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_static_broadcaster_->sendTransform(t);
  }

  std::shared_ptr<tf2_ros::StaticTransformBroadcaster> tf_static_broadcaster_;
};

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;
}

代码分析

#include "geometry_msgs/msg/transform_stamped.hpp"

        包含transform_stamped.hpp头文件是为了能访问TransformStamped消息类型，并将其发布到转换树当中去。

#include "rclcpp/rclcpp.hpp"

        包含rclcpp.hpp头文件，是为了使用rcl::Node类。

#include "tf2/LinearMath/Quaternion.h"
#include "tf2_ros/static_transform_broadcaster.h"

        包含Quaternion.h是为了利用四元数类中提供的方便转换欧拉角为四元数（或四元数转换为欧拉角）的功能，包含static_transform_broadcaster.h是为了使用TransformBroadcaster从而使得发布静态转换变得简单。

tf_static_broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);

this->make_transforms(transformation);

        StaticFramePublisher类的构造函数中初始化名称为static_turtle_tf2_broadcaster的节点。然后，创建StaticTransformBroadcaster，它将在启动时发送一个静态转换。 

geometry_msgs::msg::TransformStamped t;

t.header.stamp = this->get_clock()->now();
t.header.frame_id = "world";
t.child_frame_id = transformation[1];

        在这里，我们创建一个TransformStamped对象，它将是我们在填充后发送的消息载体。在传递实际的转换值之前，我们需要为其提供适当的元数据初始化一下。 

1.         t.header.stamp，这个好理解，将当前时间作为时间戳填充上；
2.         t.hear.frame_id，我们创建连接（link）的父坐标系名称，我们取名“world”；
3.         t.child_frame_id, 我们创建连接（link）的子坐标系名称；

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();

        这里是填充关于小海龟的6D(六个自由度)位姿数据（偏移及旋转）。

tf_static_broadcaster_->sendTransform(t);

         数据都准备好了之后，我们调用sendTransform函数将消息t（静态转换）发布广播出去。

更新package.xml和CMakeLists.txt

package.xml

<description>Learning tf2 with rclcpp</description>
<maintainer email="mike@qq.com">mike</maintainer>
<license>Apache License 2.0</license>

        根据实际情况手动修改。

CMakeLists.txt

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
)

        添加可执行程序，名字为static_turtle_tf2_broadcaster，我们在使用ros2 run命令时会使用到此生成的可执行程序。

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

        指定构建安装的路径（可执行程序生成的路径），方便调用程序按图索骥。

构建

        都搞定之后，我们就可以构建这个包了，但，养成好习惯，先检查依赖再构建。

$rosdep install -i --from-path src --rosdistro iron -y
$colcon build --packages-select learning_tf2_cpp

运行

        我们新开一个终端，先进入工作空间根路径然后source下环境：

$source install/setup.bash

        现在我们终于可以运行了，但我们需要在调用可执行文件时还需将位姿数据传进去，方便该节点填充消息（x,y,z,r,p,y->0,0,1,0,0,0）：

$ros2 run learning_tf2_cpp static_turtle_tf2_broadcaster mystaticturtle 0 0 1 0 0 0

上面的参数z为1，代表给mystaticturtle坐标系广播了一个离地1米高度的海龟位姿信息（让mystaticturtle坐标系朝着这个小目标前进）。

补充一点机器人学背景专业知识。

在ROS（Robot Operating System）中，6自由度（6DOF，即6 Degrees of Freedom）通常指的是一个刚体在空间中的完全定位所需的参数。这些参数描述了刚体在三维空间中的位置和姿态。具体来说，6自由度包括：

    三个位置参数（Position）：
        X轴位移（X-axis translation）
        Y轴位移（Y-axis translation）
        Z轴位移（Z-axis translation）

这三个参数描述了刚体在三维空间中的位置。

    三个姿态参数（Attitude）或方向参数（Orientation）：
        横滚角（Roll）
        俯仰角（Pitch）
        偏航角（Yaw）

这三个参数描述了刚体在三维空间中的方向或姿态。这些角度通常使用欧拉角、四元数或其他方式来表示。

在ROS中，这些参数经常用于描述传感器数据（如IMU、激光雷达等）、机器人末端执行器的位置、机器人的自身姿态等。例如，tf2库就用于处理这些变换，允许用户在不同坐标系之间转换点、向量和姿态。

当处理机器人学中的运动时，了解这些参数和它们如何影响机器人在空间中的位置和姿态是非常重要的。

位置参数（Position）

    X轴位移（X-axis translation）：
        这个参数描述了刚体在X轴方向上的位置。正值通常表示向右移动，负值表示向左移动。它决定了刚体在三维空间中的水平位置。
    Y轴位移（Y-axis translation）：
        这个参数描述了刚体在Y轴方向上的位置。正值通常表示向前移动，负值表示向后移动。它决定了刚体在三维空间中的前后位置。
    Z轴位移（Z-axis translation）：
        这个参数描述了刚体在Z轴方向上的位置。正值通常表示向上移动，负值表示向下移动。它决定了刚体在三维空间中的垂直位置。

姿态参数（Attitude）或方向参数（Orientation）

姿态参数通常用来描述刚体在三维空间中的方向或朝向。有多种方式可以表示姿态，但最常见的包括欧拉角和四元数。这里，我们简单解释欧拉角：

    横滚角（Roll）：
        这个参数描述了刚体绕其X轴的旋转。正值通常表示顺时针旋转，负值表示逆时针旋转。它描述了刚体在水平面上的旋转。
    俯仰角（Pitch）：
        这个参数描述了刚体绕其Y轴的旋转。正值通常表示向上抬头，负值表示向下低头。它描述了刚体在垂直平面内的旋转。
    偏航角（Yaw）：
        这个参数描述了刚体绕其Z轴的旋转。正值通常表示向右转动，负值表示向左转动。它描述了刚体在水平面上的方向改变。

需要注意的是，欧拉角存在万向锁问题，即在某些特定姿态下，横滚角和俯仰角会变得不确定，影响姿态描述的准确性。因此，在实际应用中，有时会更倾向于使用四元数或其他方法来表示姿态。

         我们再来检查一下主题/tf_static发布的静态转换消息(不知道是哪个主题可以ros2 topic list确认一下)。

$ros2 topic echo /tf_static

 如果一切顺利，我们将会看到下面的内容。

发布静态转换的正确方式

        除了我们上面编写构建的StaticTransformBroadcaster可以发布静态转换外，我们还可以利用tf2_ros工具（名字为static_transform_publisher）来实现同样的目的（实际情况就是直接使用现成的工具而不是自己去实现，上面的代码实现是为了让我们了解到其内部的实现，知己知彼嘛），我们可以直接在命令行调用此工具或者在launch文件里添加使用。

        使用以米为单位的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

        可以看出，除了6D参数外还有我们在自己编写的代码里面的父子坐标系名称。我们执行一下，效果与上面的是一样的。

 

 

        使用以米和四元数为单位的x/y/z偏移将静态坐标变换发布到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

补充四元数知识点：

四元素（Quaternion）是一种用于表示旋转的数学工具，在机器人学和计算机图形学等领域有广泛应用。它由一个实部和三个虚部组成，可以表示为（w, x, y, z）或（w, V），其中V是虚部，表示为向量（x, y, z）。四元素满足特定的数学规则，包括虚部的乘法规则，如i^2 = j^2 = k^2 = ijk = −1等。

在ROS（Robot Operating System）中，四元素常用于表示机器人的姿态。姿态描述了机器人在三维空间中的位置和朝向，而四元素是描述这种姿态的一种常用方式。ROS提供了处理四元素的库和工具，使得机器人软件能够方便地处理与姿态相关的任务。

在使用四元素时，需要注意其标准化问题。标准化四元素意味着将其长度（模长）调整为1，以确保其正确表示旋转。未标准化的四元素可能会导致错误的结果。

此外，ROS中的tf2系统也涉及到四元素的使用。tf2用于进行坐标旋转以及tf、msg两种四元素数据结构的变换，它使得在不同坐标系之间转换姿态变得简单高效。

综上所述，四元素是机器人学和计算机图形学中的重要工具，尤其在ROS中，它扮演着描述机器人姿态的关键角色。

  static_transform_publisher既可以直接在命令行使用又可以在launch文件里面使用，我们来看看它在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 = ['--x', '0', '--y', '0', '--z', '1', '--yaw', '0', '--pitch', '0', '--roll', '0', '--frame-id', 'world', '--child-frame-id', 'mystaticturtle']
        ),
    ])

        看起来是不是比较容易接受，但需要注意的是，除了参数--frame-id 和参数 --child-frame-id外的其他参数都是可选的，如果未指定某个特定选项，则会假定使用恒等变换（默认值）。

本篇完。