4. ROS 2核心编程

ROS 2的中间件抽象层将DDS（数据分发服务）的通信能力封装为统一的rclcpp（ROS客户端库C++版）接口。对于自动驾驶系统而言，感知、规划、控制等模块之间的数据交互频率高、实时性强，理解rclcpp的编程模型、回调调度机制以及多线程执行器的工作原理，是构建稳定、低延迟中间件的基础。本章将围绕四大通信原语（发布者、订阅者、服务端、客户端）展开，深入剖析回调机制的本质，并通过对比实验说明多线程执行器在提升系统吞吐量方面的实际效果。

rclcpp 通信原语与执行器调度模型 发布者 (Publisher) create_publisher<T>() publish(msg) 订阅者 (Subscription) create_subscription<T>() 回调函数 on_message 话题 Topic (DDS数据流) 服务端 (Service) create_service<Srv>() 处理请求并返回响应 客户端 (Client) create_client<Srv>() async_send_request() 服务 Service (请求-响应) 执行器 Executor 单线程 / 多线程 (rclcpp::executors::MultiThreadedExecutor) 回调队列 回调队列

上图展示了rclcpp中四大通信原语与执行器之间的核心关系。发布者与订阅者通过话题进行异步数据流通信，服务端与客户端通过服务进行同步请求-响应通信。所有通信事件最终都汇聚到执行器的回调队列中，由执行器统一调度。理解这张图是掌握本章后续所有编程技巧的前提。

4.1 核心原理：rclcpp的通信抽象与回调驱动模型

rclcpp的设计哲学是"一切皆回调"。无论是话题数据的到达、服务请求的接收，还是定时器的触发，底层DDS事件都会被rclcpp的WaitSet机制捕获，并转化为对应的回调函数放入执行器的就绪队列。执行器（Executor）负责从队列中取出回调并执行。这种事件驱动模型使得ROS 2节点天然支持异步并发，但也引入了回调执行顺序不确定、共享数据竞争等新问题。

在自动驾驶场景中，一个典型的感知节点可能同时订阅相机图像、激光雷达点云、车辆底盘状态三个话题，并周期性地发布融合结果。如果使用默认的单线程执行器，所有回调都在同一个线程中串行执行，一旦某个回调（如点云处理）耗时过长，就会阻塞其他回调的及时响应。这正是多线程执行器需要引入的根本原因。

关键概念辨析：rclcpp中的"回调"与Linux信号处理中的"回调"不同。ROS 2的回调是用户态函数指针，由执行器在事件就绪时主动调用，而非由内核异步触发。因此，回调的执行时机完全由执行器的调度策略决定，开发者必须显式管理回调之间的同步关系。

4.2 操作流程：从零编写一个完整的发布-订阅程序

下面通过一个简化但完整的示例，演示如何使用rclcpp编写发布者和订阅者。该示例模拟自动驾驶中的车速信息发布与接收。

// speed_publisher.cpp
#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/float32.hpp>

class SpeedPublisher : public rclcpp::Node {
public:
    SpeedPublisher() : Node("speed_publisher") {
        publisher_ = this->create_publisher<std_msgs::msg::Float32>("vehicle_speed", 10);
        timer_ = this->create_wall_timer(
            std::chrono::milliseconds(100),
            std::bind(&SpeedPublisher::timer_callback, this));
    }
private:
    void timer_callback() {
        auto msg = std_msgs::msg::Float32();
        msg.data = 30.0 + std::rand() % 20;  // 模拟30-50 km/h车速
        publisher_->publish(msg);
        RCLCPP_INFO(this->get_logger(), "Publishing speed: %.2f", msg.data);
    }
    rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr publisher_;
    rclcpp::TimerBase::SharedPtr timer_;
};

int main(int argc, char** argv) {
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<SpeedPublisher>());
    rclcpp::shutdown();
    return 0;
}
// speed_subscriber.cpp
#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/float32.hpp>

class SpeedSubscriber : public rclcpp::Node {
public:
    SpeedSubscriber() : Node("speed_subscriber") {
        subscription_ = this->create_subscription<std_msgs::msg::Float32>(
            "vehicle_speed", 10,
            std::bind(&SpeedSubscriber::topic_callback, this, std::placeholders::_1));
    }
private:
    void topic_callback(const std_msgs::msg::Float32::SharedPtr msg) {
        RCLCPP_INFO(this->get_logger(), "Received speed: %.2f km/h", msg->data);
        // 此处可加入速度校验、限速判断等业务逻辑
    }
    rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr subscription_;
};

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

编译时需在CMakeLists.txt中声明两个可执行文件，并链接rclcpp和std_msgs依赖。运行前先启动发布者，再启动订阅者，即可在终端看到车速数据的实时传输。

4.3 服务端与客户端：同步请求-响应模式

话题通信适合单向数据流，但自动驾驶中大量存在"请求-响应"场景，例如规划模块向定位模块请求当前位姿、诊断模块向各传感器节点查询健康状态。ROS 2服务提供了这种同步通信能力。

// 服务端：提供位姿查询服务
#include <rclcpp/rclcpp.hpp>
#include <nav_msgs/srv/get_map.hpp>  // 示例服务类型

class MapServer : public rclcpp::Node {
public:
    MapServer() : Node("map_server") {
        service_ = this->create_service<nav_msgs::srv::GetMap>(
            "get_map",
            std::bind(&MapServer::handle_request, this, 
                      std::placeholders::_1, std::placeholders::_2));
    }
private:
    void handle_request(const std::shared_ptr<nav_msgs::srv::GetMap::Request> req,
                        std::shared_ptr<nav_msgs::srv::GetMap::Response> res) {
        RCLCPP_INFO(this->get_logger(), "Received map request, frame_id: %s", req->frame_id.c_str());
        // 实际项目中此处从地图数据库加载数据
        res->map.header.frame_id = "map";
        res->map.header.stamp = this->now();
        res->map.info.resolution = 0.05;
        res->map.info.width = 100;
        res->map.info.height = 100;
        // 填充栅格数据...
    }
    rclcpp::Service<nav_msgs::srv::GetMap>::SharedPtr service_;
};
// 客户端：请求地图数据
#include <rclcpp/rclcpp.hpp>
#include <nav_msgs/srv/get_map.hpp>

class MapClient : public rclcpp::Node {
public:
    MapClient() : Node("map_client") {
        client_ = this->create_client<nav_msgs::srv::GetMap>("get_map");
        // 等待服务上线
        while (!client_->wait_for_service(std::chrono::seconds(2))) {
            RCLCPP_WARN(this->get_logger(), "Waiting for map service...");
        }
        send_request();
    }
private:
    void send_request() {
        auto request = std::make_shared<nav_msgs::srv::GetMap::Request>();
        request->frame_id = "map";
        auto future = client_->async_send_request(request);
        // 使用回调处理响应，避免阻塞
        std::thread([this, future]() mutable {
            auto result = future.get();
            RCLCPP_INFO(this->get_logger(), "Map received: %dx%d, resolution=%.2f",
                        result->map.info.width, result->map.info.height,
                        result->map.info.resolution);
        }).detach();
    }
    rclcpp::Client<nav_msgs::srv::GetMap>::SharedPtr client_;
};

服务通信的要点在于：客户端必须使用async_send_request而非阻塞式调用，否则会卡死执行器线程。上述代码中通过创建独立线程等待future结果，是一种简单但有效的非阻塞模式。更优雅的做法是使用rclcpp::spin_until_future_complete或回调函数。

4.4 回调机制深度剖析与多线程执行器实战

默认的rclcpp::spin()使用单线程执行器，所有回调串行执行。当节点订阅多个话题且处理耗时不同时，长回调会阻塞短回调，导致数据延迟累积。多线程执行器通过线程池并行处理回调，显著提升吞吐量。

特性SingleThreadedExecutorMultiThreadedExecutorStaticSingleThreadedExecutor
线程数1可配置（默认硬件并发数）1（但优化了内存分配）
回调并发无支持无
适用场景简单节点、低负载多话题高吞吐、异构回调高频小消息、低延迟要求
数据竞争风险低高（需加锁）低
典型自动驾驶用途诊断节点、参数服务器感知融合、规划决策底盘控制、传感器驱动

使用多线程执行器只需修改main函数：

int main(int argc, char** argv) {
    rclcpp::init(argc, argv);
    auto node = std::make_shared<SpeedSubscriber>();
    rclcpp::executors::MultiThreadedExecutor executor(
        std::make_shared<rclcpp::executors::MultiThreadedExecutor::Options>()
    );
    executor.add_node(node);
    executor.spin();
    rclcpp::shutdown();
    return 0;
}

但多线程引入了一个关键问题：回调可能并发访问共享成员变量。例如，订阅者回调中更新了current_speed_成员，而另一个定时器回调读取该值用于决策，此时必须使用std::mutex保护共享数据。

风险提示：多线程执行器并非万能。如果多个回调竞争同一把锁，线程会阻塞等待，实际吞吐量可能不升反降。此外，回调中严禁调用阻塞操作（如sleep、等待I/O），否则会占用线程池资源。建议将耗时操作放入独立线程，回调仅负责数据搬运。

4.5 案例拆解：多传感器融合节点的性能调优

某自动驾驶测试车上的感知融合节点需要同时订阅6路摄像头图像（30fps）、1路激光雷达点云（10Hz）和1路毫米波雷达数据（20Hz），并将融合结果以50Hz频率发布。初始版本使用单线程执行器，实测发现点云回调耗时80ms，导致图像回调延迟最高达120ms，融合输出频率跌至20Hz。

分析过程：通过rclcpp::get_logger().set_level()开启调试日志，统计各回调耗时分布。发现点云处理（聚类算法）占80ms，图像预处理（降采样+色彩转换）每路约15ms，毫米波解析仅2ms。瓶颈在于点云回调阻塞了所有图像回调。

优化方案：

  1. 将执行器改为MultiThreadedExecutor，线程数设为4（考虑CPU核心数及内存带宽）。
  2. 将点云回调中的聚类算法拆分为两个阶段：第一阶段仅做体素滤波（耗时20ms），第二阶段将聚类任务放入独立线程池异步执行，回调立即返回。
  3. 为图像回调设置独立的回调组（CallbackGroup），使用rclcpp::CallbackGroupType::MutuallyExclusive确保同一路图像的回调不并发，但不同路图像可并行。
  4. 共享数据（融合结果）使用std::atomic<float>或双缓冲策略，避免锁竞争。

结果对比：优化后，图像回调延迟降至25ms以内，融合输出稳定在48-50Hz，CPU占用率从85%降至62%。代价是代码复杂度显著增加，需要仔细设计线程安全的数据结构。

经验总结：多线程执行器的线程数并非越多越好。对于I/O密集型回调（如网络通信），线程数可设为CPU核心数的2倍；对于CPU密集型计算，线程数应接近核心数。建议通过perf或top监控实际CPU使用率，动态调整线程数。

4.6 常见误区与复盘

误区一：认为create_subscription的回调函数在单独的线程中执行。实际上，回调执行线程完全由执行器决定，与创建位置无关。

误区二：在回调中调用rclcpp::spin()或executor.spin()导致死锁。回调函数中不应再启动新的spin循环。

误区三：忽略服务质量（QoS）设置。自动驾驶中，传感器数据通常使用SensorDataQoS（BestEffort + 大深度队列），而控制指令使用Reliable策略。错误的QoS配置会导致数据丢失或通信阻塞。

误区四：多线程执行器下未保护共享数据，导致数据竞争和未定义行为。建议使用tsan（ThreadSanitizer）工具检测数据竞争。

复盘要点：在编写任何rclcpp节点前，先画出回调依赖图，明确哪些回调可能并发执行，哪些必须串行。对于自动驾驶这类安全关键系统，建议优先使用单线程执行器保证确定性，仅在性能瓶颈明确时引入多线程，并配合严格的锁策略和测试验证。

← 上一章 📖 返回目录 下一章 →