25/1/21 算法笔记＜ROS2＞ 话题通信接口

在 ROS 2 中，通信接口是节点之间进行数据交换的核心机制。ROS 2 提供了多种通信接口，包括话题（Topic）、服务（Service）、动作（Action）和参数（Parameter）。每种接口适用于不同的场景，具体选择取决于你的应用需求。

这次我们先讲服务

1. 话题（Topic）

• 话题是 ROS 2 中最常用的通信机制，用于节点之间的异步数据交换。

• 一个节点可以发布（Publish）消息到话题，另一个节点可以订阅（Subscribe）该话题以接收消息。

• 特点：

• 一对多或多对多：多个节点可以发布到同一个话题，多个节点也可以订阅同一个话题。

• 异步通信：发布者和订阅者之间不需要直接交互，消息通过话题传递。

• 适用于流式数据：如传感器数据、控制指令等。

示例：

• 发布者：

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"

class PublisherNode : public rclcpp::Node
{
public:
    PublisherNode() : Node("publisher_node")
    {
        publisher_ = this->create_publisher<std_msgs::msg::String>("topic_name", 10);
        timer_ = this->create_wall_timer(1s, std::bind(&PublisherNode::timer_callback, this));
    }

private:
    void timer_callback()
    {
        auto msg = std_msgs::msg::String();
        msg.data = "Hello, ROS 2!";
        publisher_->publish(msg);
    }

    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
    rclcpp::TimerBase::SharedPtr timer_;
};

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

订阅者：

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"

class SubscriberNode : public rclcpp::Node
{
public:
    SubscriberNode() : Node("subscriber_node")
    {
        subscriber_ = this->create_subscription<std_msgs::msg::String>(
            "topic_name", 10, std::bind(&SubscriberNode::topic_callback, this, std::placeholders::_1));
    }

private:
    void topic_callback(const std_msgs::msg::String::SharedPtr msg)
    {
        RCLCPP_INFO(this->get_logger(), "Received: %s", msg->data.c_str());
    }

    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscriber_;
};

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

接下来我们详细的用代码讲解步骤

1. 创建 ROS 2 工作空间

在开始之前，我们需要创建一个 ROS 2 工作空间来存放我们的代码。

步骤 1：创建工作空间

打开终端，创建一个工作空间目录：

mkdir -p ~/topic_ws/src
cd ~/topic_ws/src

创建一个 ROS 2 包：

ros2 pkg create --build-type ament_cmake topic_demo

进入包目录：

cd topic_demo

2. 编写发布者代码

发布者节点会定期发布消息到话题。

步骤 2：创建发布者代码

在 src 目录下创建 publisher_node.cpp 文件：

touch src/publisher_node.cpp

编辑 publisher_node.cpp 文件，编写以下代码：

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"

using namespace std::chrono_literals;

class PublisherNode : public rclcpp::Node
{
public:
    PublisherNode() : Node("publisher_node")
    {
        // 创建一个发布者，发布到 "topic_name" 话题，消息类型为 std_msgs::msg::String，队列长度为 10
        publisher_ = this->create_publisher<std_msgs::msg::String>("topic_name", 10);

        // 创建一个定时器，每隔 1 秒触发一次回调函数
        timer_ = this->create_wall_timer(1s, std::bind(&PublisherNode::timer_callback, this));
    }

private:
    void timer_callback()
    {
        // 创建一个消息对象
        auto msg = std_msgs::msg::String();
        msg.data = "Hello, ROS 2!"; // 设置消息内容

        // 发布消息
        publisher_->publish(msg);

        // 打印日志
        RCLCPP_INFO(this->get_logger(), "Publishing: %s", msg.data.c_str());
    }

    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
    rclcpp::TimerBase::SharedPtr timer_;
};

int main(int argc, char *argv[])
{
    // 初始化 ROS 2
    rclcpp::init(argc, argv);

    // 创建发布者节点并进入事件循环
    rclcpp::spin(std::make_shared<PublisherNode>());

    // 关闭 ROS 2
    rclcpp::shutdown();
    return 0;
}

3. 编写订阅者代码

订阅者节点会订阅话题并接收消息。

步骤 3：创建订阅者代

在 src 目录下创建 subscriber_node.cpp 文件。

touch src/subscriber_node.cpp

编辑 subscriber_node.cpp 文件，编写以下代码：

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"

class SubscriberNode : public rclcpp::Node
{
public:
    SubscriberNode() : Node("subscriber_node")
    {
        // 创建一个订阅者，订阅 "topic_name" 话题，消息类型为 std_msgs::msg::String，队列长度为 10
        subscriber_ = this->create_subscription<std_msgs::msg::String>(
            "topic_name", 10, std::bind(&SubscriberNode::topic_callback, this, std::placeholders::_1));
    }

private:
    void topic_callback(const std_msgs::msg::String::SharedPtr msg)
    {
        // 打印接收到的消息
        RCLCPP_INFO(this->get_logger(), "Received: %s", msg->data.c_str());
    }

    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscriber_;
};

int main(int argc, char *argv[])
{
    // 初始化 ROS 2
    rclcpp::init(argc, argv);

    // 创建订阅者节点并进入事件循环
    rclcpp::spin(std::make_shared<SubscriberNode>());

    // 关闭 ROS 2
    rclcpp::shutdown();
    return 0;
}

4. 配置 CMakeLists.txt

为了让 ROS 2 能够编译我们的代码，需要在 CMakeLists.txt 文件中添加编译规则。

步骤 4：修改 CMakeLists.txt

1. 打开 CMakeLists.txt 文件，添加以下内容：

   # 添加可执行文件
   add_executable(publisher_node src/publisher_node.cpp)
   ament_target_dependencies(publisher_node rclcpp std_msgs)
   
   add_executable(subscriber_node src/subscriber_node.cpp)
   ament_target_dependencies(subscriber_node rclcpp std_msgs)
   
   # 安装可执行文件
   install(TARGETS
     publisher_node
     subscriber_node
     DESTINATION lib/${PROJECT_NAME})

5. 编译工作空间

步骤 5：编译工作空间

返回工作空间根目录：

cd ~/topic_ws

使用 colcon build 编译工作空间：

colcon build

编译完成后，加载工作空间环境：

source install/setup.bash

6. 运行节点

步骤 6：运行发布者和订阅者

打开第一个终端，运行发布者节点：

ros2 run topic_demo publisher_node

打开第二个终端，运行订阅者节点：

ros2 run topic_demo subscriber_node

在发布者终端的输出中，你会看到类似以下内容：

[INFO] [publisher_node]: Publishing: Hello, ROS 2!

在订阅者终端的输出中，你会看到类似以下内容：

[INFO] [subscriber_node]: Received: Hello, ROS 2!

在机器人中，ROS 2 的话题（Topic）机制可以用于多种场景，实现机器人各个模块之间的数据交换和协同工作。以下是话题在机器人中的一些典型应用场景和功能：

1. 传感器数据发布

作用：

• 机器人上的传感器（如激光雷达、摄像头、IMU 等）可以通过话题发布数据，供其他模块使用。

示例：

• 激光雷达：发布 sensor_msgs/msg/LaserScan 消息，包含扫描数据。

• 摄像头：发布 sensor_msgs/msg/Image 消息，包含图像数据。

• IMU：发布 sensor_msgs/msg/Imu 消息，包含加速度、角速度和姿态数据。

代码示例：

// 发布激光雷达数据
auto scan_msg = sensor_msgs::msg::LaserScan();
scan_msg.header.stamp = this->now();
scan_msg.ranges = {1.0, 2.0, 3.0}; // 示例数据
laser_publisher_->publish(scan_msg);

2. 控制指令发布

作用：

• 控制模块可以通过话题发布控制指令，驱动机器人的执行器（如电机、舵机等）。

示例：

• 移动机器人：发布 geometry_msgs/msg/Twist 消息，包含线速度和角速度。

• 机械臂：发布 std_msgs/msg/Float64MultiArray 消息，包含关节角度或力矩。

代码示例：

// 发布移动指令
auto twist_msg = geometry_msgs::msg::Twist();
twist_msg.linear.x = 0.5;  // 前进速度
twist_msg.angular.z = 0.2; // 转向速度
cmd_vel_publisher_->publish(twist_msg);

3. 状态信息发布

作用：

• 机器人可以发布自身的状态信息（如电池电量、位置、速度等），供监控或决策模块使用。

示例：

• 电池状态：发布 sensor_msgs/msg/BatteryState 消息。

• 位置信息：发布 nav_msgs/msg/Odometry 消息，包含位置和速度。

代码示例：

// 发布电池状态
auto battery_msg = sensor_msgs::msg::BatteryState();
battery_msg.percentage = 0.75; // 电量百分比
battery_publisher_->publish(battery_msg);

4. 多节点协同

作用：

• 多个节点可以通过话题共享数据，实现协同工作。例如，导航模块发布目标点，路径规划模块订阅并生成路径，控制模块执行路径。

示例：

• 导航系统：

  • 导航节点发布目标点（geometry_msgs/msg/PoseStamped）。

  • 路径规划节点订阅目标点并生成路径（nav_msgs/msg/Path）。

  • 控制节点订阅路径并驱动机器人。

代码示例：

// 发布目标点
auto goal_msg = geometry_msgs::msg::PoseStamped();
goal_msg.header.stamp = this->now();
goal_msg.pose.position.x = 5.0; // 目标点坐标
goal_msg.pose.position.y = 3.0;
goal_publisher_->publish(goal_msg);

5. 数据记录与回放

作用：

• 使用 ros2 bag 工具记录话题数据，用于后续分析或回放。

示例：

• 记录激光雷达数据：

ros2 bag record /scan

回放数据：

ros2 bag play <bag_file>

6. 远程监控与调试

作用：

• 通过话题将机器人的数据发送到远程计算机，实现远程监控和调试。

示例：

• 使用 rqt 工具查看话题数据：

rqt

7. 多机器人通信

作用：

• 多个机器人可以通过话题共享数据，实现协同任务。

示例：

• 机器人 A 发布自身位置，机器人 B 订阅并调整自身行为。

8. 自定义消息

作用：

• 如果标准消息类型不满足需求，可以定义自定义消息类型。

示例：

1. 在 msg 目录下创建自定义消息文件 CustomMessage.msg：

float32 data1
int32 data2
string data3

在代码中使用自定义消息：

#include "topic_demo/msg/custom_message.hpp"

auto custom_msg = topic_demo::msg::CustomMessage();
custom_msg.data1 = 1.23;
custom_msg.data2 = 42;
custom_msg.data3 = "Hello";
custom_publisher_->publish(custom_msg);

通过话题机制，机器人各个模块可以高效地共享数据，实现复杂的功能。