ROS2系列 (11) : C++话题通信节点——订阅者示例

在前一篇中，我们在demo_cpp_topic功能包中实现了“控制海龟画圆”的发布者节点。本文将继续使用该功能包，添加“海龟到达目标点”的订阅者节点，通过修改CMakeLists.txt实现多节点共存，并深入解析运动控制算法的核心逻辑。

一、功能包结构与集成思路

我们将订阅者节点代码添加到已有的demo_cpp_topic功能包中，最终目录结构如下：

ros2_ws/src/demo_cpp_topic/
├── src/
│   ├── circle_pub.cpp       # 已有的画圆发布者节点
│   └── target_controller.cpp  # 新增的目标点控制订阅者节点
├── CMakeLists.txt           # 共享的构建配置文件
├── package.xml              # 功能包元信息
└── LICENSE

核心目标：通过同一个功能包管理两个节点，实现“画圆”与“到达目标点”的功能切换。

二、添加订阅者节点代码

在demo_cpp_topic/src/目录下创建target_controller.cpp，代码如下：

#include "geometry_msgs/msg/twist.hpp"
#include "rclcpp/rclcpp.hpp"
#include "turtlesim/msg/pose.hpp"

class TurtleController : public rclcpp::Node
{
public:
    TurtleController() : Node("turtle_controller")
    {
        velocity_publisher_ = this->create_publisher<geometry_msgs::msg::Twist>(
            "/turtle1/cmd_vel", 10);
        pose_subscription_ = this->create_subscription<turtlesim::msg::Pose>(
            "/turtle1/pose", 10,
            std::bind(&TurtleController::on_pose_received_, this, std::placeholders::_1));
    }

private:
    void on_pose_received_(const turtlesim::msg::Pose::SharedPtr pose) {
    auto message = geometry_msgs::msg::Twist();
    // 1.记录当前位置
    double current_x = pose->x;
    double current_y = pose->y;
    RCLCPP_INFO(this->get_logger(), "当前位置:(x=%f,y=%f)", current_x, current_y);

    // 2.计算距离目标的距离，与当前海龟朝向的角度差
    double distance =
        std::sqrt((target_x_ - current_x) * (target_x_ - current_x) +
                  (target_y_ - current_y) * (target_y_ - current_y));
    double angle =
        std::atan2(target_y_ - current_y, target_x_ - current_x) - pose->theta;

    // 3.控制策略：距离大于0.1继续运动，角度差大于0.2则原地旋转，否则直行
    if (distance > 0.1) {
      if(fabs(angle)>0.2)
      {
        message.angular.z = fabs(angle);
      }else{
        // 通过比例控制器计算输出速度
        message.linear.x = k_ * distance;

      }
    }

    // 4.限制最大值并发布消息
    if (message.linear.x > max_speed_) {
       message.linear.x = max_speed_;
    }
    velocity_publisher_->publish(message);
  }


private:
    rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr pose_subscription_;
    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr velocity_publisher_;
    double target_x_{1.0};  // 目标位置X,设置默认值1.0
    double target_y_{1.0};  // 目标位置Y,设置默认值1.0
    double k_{1.0};         // 比例系数，控制输出=误差*比例系数
    double max_speed_{3.0}; // 最大线速度，设置默认值3.0
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<TurtleController>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}

三、修改CMakeLists.txt（支持多节点编译）

打开demo_cpp_topic/CMakeLists.txt，在原有基础上添加订阅者节点的编译规则：

cmake_minimum_required(VERSION 3.8)
project(demo_cpp_topic)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# 查找依赖包（与之前一致）
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(turtlesim REQUIRED)

# 1. 编译画圆发布者节点（已有的）
add_executable(circle_pub src/circle_pub.cpp)
ament_target_dependencies(circle_pub rclcpp geometry_msgs turtlesim)

# 2. 新增：编译目标点控制订阅者节点
add_executable(target_controller src/target_controller.cpp)
ament_target_dependencies(target_controller rclcpp geometry_msgs turtlesim)

# 安装两个节点的可执行文件
install(TARGETS
  circle_pub           # 画圆节点
  target_controller    # 目标点控制节点
  DESTINATION lib/${PROJECT_NAME}
)

ament_package()

关键修改：通过add_executable分别为两个节点创建可执行文件，再通过install统一安装，实现一个功能包管理多个节点。

四、构建与运行流程

4.1 构建功能包

在工作空间根目录执行构建：

cd ~/ros2_ws
colcon build --packages-select demo_cpp_topic

构建成功后，install/demo_cpp_topic/lib/demo_cpp_topic/目录下会生成两个可执行文件：circle_pub和target_controller。

4.2 运行目标点控制节点（三个终端）

终端1：启动海龟模拟器

source install/setup.bash
ros2 run turtlesim turtlesim_node

终端2：启动目标点控制订阅者节点

source install/setup.bash
ros2 run demo_cpp_topic target_controller

终端3（可选）：查看话题数据

source install/setup.bash
# 查看海龟实时位姿
ros2 topic echo /turtle1/pose
# 或查看速度指令
ros2 topic echo /turtle1/cmd_vel

4.3 效果验证

• 海龟会从初始位置（默认(5.5, 5.5)）向目标点(1.0, 1.0)移动；
• 终端2会实时输出当前位置，直到海龟到达目标点附近（距离小于0.1）后停止运动。

五、运动控制算法逻辑详解

5.1 核心控制流程

节点通过“订阅位姿→计算误差→生成速度指令”的闭环逻辑控制海龟运动，具体步骤如下：

1. 获取当前位姿：通过订阅/turtle1/pose话题，得到海龟的实时位置(current_x, current_y)和朝向current_theta。

2. 计算误差：

   • 距离误差：当前位置到目标点(target_x, target_y)的直线距离：

     distance = √[(target_x - current_x)² + (target_y - current_y)²]
     

   • 角度误差：目标方向与海龟当前朝向的夹角：

     angle = atan2(target_y - current_y, target_x - current_x) - current_theta
     

     • atan2(Δy, Δx)：计算目标点相对当前位置的方向角（范围[-π, π]）；
     • 减去当前朝向current_theta，得到需要旋转的角度差。

3. 生成控制指令：

   • 若distance > 0.1（未到达目标）：
     • 若|angle| > 0.2（角度偏差大）：优先旋转（angular.z = |angle|），直到对准目标；
     • 若|angle| ≤ 0.2（角度偏差小）：直线移动（linear.x = k * distance），速度与距离成正比（比例控制）。
   • 若distance ≤ 0.1（到达目标附近）：不发布速度指令，海龟停止。

4. 速度限制：通过max_speed_限制最大线速度，避免运动过快。

5.2 为什么能到达目标点附近？

• 角度优先策略：先旋转对准目标再移动，确保运动方向正确，避免“走歪路”；
• 比例控制（P控制）：线速度与距离成正比（k=1.0），距离越近速度越慢，有效避免超调（冲过目标点）；
• 误差容忍机制：设置distance > 0.1为运动条件，允许微小误差（实际机器人控制中，机械结构和传感器精度也会导致微小误差，无需追求绝对精确）。

六、总结

本文将“海龟到达目标点”的订阅者节点集成到已有demo_cpp_topic功能包中，通过修改CMakeLists.txt实现多节点共存，并详解了运动控制算法的核心逻辑。关键要点包括：

• 一个功能包可通过CMakeLists.txt的多add_executable配置管理多个节点；
• 订阅者通过create_subscription获取实时位姿，形成反馈闭环；
• 角度优先+比例控制的策略确保海龟稳定到达目标点附近。

后续可扩展目标点参数（如通过命令行参数动态设置），或优化算法（如加入积分项消除静差），进一步提升控制精度。