ROS 2边学边练（34）-- 写一个广播（C++）

前言

        上一篇我们体验了一下静态广播的例子流程，通过命令行方式传入所需的6D参数（死数据称之为静态）并广播给tf2系统，实际使用中，这些参数可是实打实实时生成的，所以需要动态处理这些实时数据，本篇即由此始，写一个实打实的（动态）广播节点。

动动手

        我们还是接着使用上篇创建的learning_tf2_cpp功能包。

编写广播节点

        进入该功能包的src路径，执行以下命令下载官方提供的turtle_tf2_broadcaster.cpp源代码：

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

源文件内容如下：

#include <functional>
#include <memory>
#include <sstream>
#include <string>

#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"

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

    // 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, std::placeholders::_1));
  }

private:
  void handle_turtle_pose(const std::shared_ptr<turtlesim::msg::Pose> msg)
  {
    geometry_msgs::msg::TransformStamped t;

    // Read message content and assign it to
    // corresponding tf variables
    t.header.stamp = this->get_clock()->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;
}

代码分析

turtlename_ = this->declare_parameter<std::string>("turtlename", "turtle");

        在此广播（发布者）节点的构造函数中，我们定义并获取了带一个参数的turtlename参数，可以赋值turtle1或turtle2或者其他你想取的任何名字。

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

         接着，我们订阅了海龟仿真的位姿主题turtleX/pose（消息类型msg/Pose），并绑定了处理该主题消息的回调函数（一旦接收到该主题的数据，该回调函数会自动对数据进行转换处理并将最终结果广播给tf2系统）。

geometry_msgs::msg::TransformStamped t;

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

        在回调函数中，我们填充初始化一些结构体成员变量，当前时间（从1970年1月1日00：00到当前时间的总秒数）赋给stamp作为该节点发布消息时的一个时间戳标签，父坐标系名为world，子坐标系由传入的参数决定（名字可随意取，此处为方便明了就用了小海龟的名字作为子坐标系名称）。

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

        由于小海龟仿真的例子是运行在2D世界（平面）中，平移运动中Z轴用不上，所以上面代码平移转换变量中z值都取为0，而旋转转换变量只有Z轴是可用的（平面上转圈，Z轴垂直于平面）。

// Send the transformation
tf_broadcaster_->sendTransform(t);

        最后，将转换（从world坐标系到turtleX坐标系的转换）结果广播发布给tf2系统中。

CMakeLists.txt

        进入到learning_tf2_cpp功能包的根路径，打开CMakeLists.txt，将下面内容复制到里面（节点可执行文件的最终名字为turtle_tf2_broadcaster）：

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文件启动节点，在包的launch文件夹下新建名为turtle_tf2_demo：

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'}
            ]
        ),
    ])

代码分析

from launch import LaunchDescription
from launch_ros.actions import Node

        首先导入需要的模块，launch在ROS和ROS 2中都是通用的，而launch_ros却只能在ROS 2中使用。

Node(
    package='turtlesim',
    executable='turtlesim_node',
    name='sim'
),
Node(
    package='learning_tf2_cpp',
    executable='turtle_tf2_broadcaster',
    name='broadcaster1',
    parameters=[
        {'turtlename': 'turtle1'}
    ]
),

        启动两个节点，一个是turtlesim/turtlesim_node，一个是learning_tf2_cpp/turtle_tf2_broadcaster,参数turtlename赋值为turtle1,广播turtle1的状态数据给tf2系统。

添加依赖项

        将下面关于launch和launch_ros的执行依赖项添加到package.xml：

<exec_depend>launch</exec_depend>
<exec_depend>launch_ros</exec_depend>

CMakeLists.txt

        添加下面内容以使launch/文件夹下的launch文件能够被正确安装到指定路径下：

install(DIRECTORY launch
  DESTINATION share/${PROJECT_NAME})

构建

        切换到工作空间根路径下，分别执行以下流程（检查依赖、单独构建、激活环境）：

$rosdep install -i --from-path src --rosdistro iron -y

$colcon build --packages-select learning_tf2_cpp

$source install/setup.bash

或者

$. install/setup.bash

运行

        先运行启动文件，这样小海龟turtlesim_node节点和广播节点turtle_tf2_broadcaster就都跑起来了：

$ros2 launch learning_tf2_cpp turtle_tf2_demo_launch.py

 

        但此时小海龟还是处于静止状态，不会产生位姿数据，我们需要再打开一个终端启动控制节点teleop_key，让小海龟游动起来（键盘控制）：

$ros2 run turtlesim turtle_teleop_key

        等这些节点都弄完后，我们需要利用tf2_echo工具检查一下广播节点发布的数据情况，命令为（world turtle1表示从world坐标系到turtle1坐标系的转换数据）：

$ros2 run tf2_ros tf2_echo world turtle1

        再回到控制终端，随便控制下小海龟，此时切换到tf2_echo终端，可以看到这些转换的发布数据。

（只不过有警告，可能与/world坐标系的初始位置标定有关，待后续解决后将解决方法发布出来，或者有了解的同学欢迎评论区告知一二） 

        如果我们执行ros2 run tf2_ros tf2_echo world turtle2，我们不会看到任何数据，因为turtle2不存在，在后面的博文中我们将添加第二只小海龟turtle2，那时就能看到turtle2的位姿数据广播到tf2系统中去了。

本篇完。