4、ROS2核心编程：使用Python和C++编写发布订阅、服务客户端、动作服务器与客户端，掌握rclpy和rclcpp

ROS2的编程模型围绕三大通信原语展开：发布-订阅（Topic）、服务（Service）和动作（Action）。Topic用于连续数据流（如激光雷达点云、车辆速度），Service用于同步请求-响应（如路径规划请求），Action用于长时间运行且可取消的任务（如泊车、变道）。本章将深入剖析rclpy（Python客户端库）与rclcpp（C++客户端库）的实现差异，通过完整代码案例帮助开发者建立跨语言开发能力。理解这三类通信机制的本质区别，是构建可靠自动驾驶系统的前提——错误地使用Service传输高频传感器数据会导致节点阻塞，而用Topic处理需要确认的任务则可能丢失关键指令。

上图展示了ROS2中三种通信原语与rclpy/rclcpp的映射关系。在自动驾驶系统中，Topic用于处理摄像头图像流（30Hz）、Service用于路径规划请求（低频）、Action用于车辆控制指令（需要反馈和取消）。开发者需要根据数据频率、实时性、可靠性要求选择正确的通信方式。

4.1 核心原理：通信模型与QoS机制

ROS2的通信基于DDS（Data Distribution Service）中间件，相比ROS1的TCPROS，DDS提供了更丰富的QoS（Quality of Service）策略。QoS决定了消息传递的可靠性、历史数据保留策略和时效性。在自动驾驶场景中，不同数据对QoS的需求差异巨大：激光雷达点云需要"尽力传输"（BEST_EFFORT）以保证低延迟，而车辆控制指令需要"可靠传输"（RELIABLE）确保不丢失。

rclpy和rclcpp在API设计上保持高度一致，但底层实现不同。rclpy基于C扩展库rcl，通过Python绑定实现；rclcpp直接使用C++模板和智能指针。性能上，rclcpp在千字节级消息处理上比rclpy快约30%，但在开发效率上rclpy更优。实际项目中常采用混合编程：性能关键节点用C++，算法原型和工具脚本用Python。

4.2 发布-订阅编程实战

发布-订阅是ROS2中最常用的通信模式。在自动驾驶中，感知节点发布障碍物列表，规划节点订阅该列表生成轨迹。下面展示一个完整的Python发布订阅示例，包含QoS配置和生命周期管理。

# Python发布者 (rclpy)
import rclpy
from rclpy.node import Node
from std_msgs.msg import Float32MultiArray
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy

class ObstaclePublisher(Node):
    def __init__(self):
        super().__init__('obstacle_publisher')
        qos = QoSProfile(
            depth=10,
            reliability=ReliabilityPolicy.BEST_EFFORT,
            history=HistoryPolicy.KEEP_LAST
        )
        self.publisher = self.create_publisher(Float32MultiArray, 'obstacles', qos)
        self.timer = self.create_timer(0.1, self.publish_callback)  # 10Hz
        
    def publish_callback(self):
        msg = Float32MultiArray()
        msg.data = [1.5, 2.3, 0.8]  # 障碍物坐标x,y,z
        self.publisher.publish(msg)
        self.get_logger().info(f'发布障碍物: {msg.data}')

def main():
    rclpy.init()
    node = ObstaclePublisher()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()

C++订阅者实现如下，注意使用智能指针和回调组（CallbackGroup）来避免回调阻塞。

// C++订阅者 (rclcpp)
#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/float32_multi_array.hpp>
#include <rclcpp/qos.hpp>

class ObstacleSubscriber : public rclcpp::Node {
public:
    ObstacleSubscriber() : Node("obstacle_subscriber") {
        auto qos = rclcpp::QoS(10).best_effort();
        subscription_ = this->create_subscription<Float32MultiArray>(
            "obstacles", qos,
            [this](const Float32MultiArray::SharedPtr msg) {
                RCLCPP_INFO(this->get_logger(), "收到障碍物: [%.2f, %.2f, %.2f]",
                    msg->data[0], msg->data[1], msg->data[2]);
            });
    }
private:
    rclcpp::Subscription<Float32MultiArray>::SharedPtr subscription_;
};

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

4.3 服务通信与客户端实现

服务通信适用于请求-响应模式，如路径规划请求、地图加载指令。在自动驾驶中，服务常用于低频控制指令（如启动/停止）和参数查询。服务分为服务器（Server）和客户端（Client）两端，客户端发送请求后阻塞等待响应，直到超时或收到响应。

# Python服务端 (rclpy)
from example_interfaces.srv import AddTwoInts

class PathPlannerService(Node):
    def __init__(self):
        super().__init__('path_planner_service')
        self.srv = self.create_service(AddTwoInts, 'plan_path', self.plan_callback)
        
    def plan_callback(self, request, response):
        self.get_logger().info(f'收到规划请求: {request.a} + {request.b}')
        response.sum = request.a + request.b  # 简化示例
        return response

// C++客户端 (rclcpp)
#include <rclcpp/rclcpp.hpp>
#include <example_interfaces/srv/add_two_ints.hpp>

class PathPlannerClient : public rclcpp::Node {
public:
    PathPlannerClient() : Node("path_planner_client") {
        client_ = this->create_client<AddTwoInts>("plan_path");
    }
    
    void send_request(int a, int b) {
        auto request = std::make_shared<AddTwoInts::Request>();
        request->a = a;
        request->b = b;
        
        while (!client_->wait_for_service(std::chrono::seconds(1))) {
            RCLCPP_WARN(this->get_logger(), "等待服务上线...");
        }
        
        auto future = client_->async_send_request(request);
        if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), future) == 
            rclcpp::FutureReturnCode::SUCCESS) {
            RCLCPP_INFO(this->get_logger(), "规划结果: %ld", future.get()->sum);
        } else {
            RCLCPP_ERROR(this->get_logger(), "服务调用失败");
        }
    }
private:
    rclcpp::Client<AddTwoInts>::SharedPtr client_;
};

4.4 动作服务器与客户端开发

动作（Action）是ROS2中最复杂的通信模式，适用于需要长时间执行、可取消、带反馈的任务。在自动驾驶中，动作典型应用包括：自动泊车（需要反馈进度）、车道变更（可取消）、紧急制动（需要确认完成）。动作由三部分组成：Goal（目标）、Feedback（反馈）、Result（结果）。

# Python动作服务器 (rclpy)
from action_tutorials_interfaces.action import Fibonacci

class ParkingActionServer(Node):
    def __init__(self):
        super().__init__('parking_action_server')
        self.action_server = self.create_action_server(
            Fibonacci, 'parking', self.execute_callback)
    
    async def execute_callback(self, goal_handle):
        self.get_logger().info(f'开始泊车，目标位置: {goal_handle.request.order}')
        feedback_msg = Fibonacci.Feedback()
        feedback_msg.sequence = [0, 1]
        
        for i in range(1, goal_handle.request.order):
            if goal_handle.is_cancel_requested:
                goal_handle.canceled()
                return Fibonacci.Result()
            feedback_msg.sequence.append(feedback_msg.sequence[-1] + feedback_msg.sequence[-2])
            goal_handle.publish_feedback(feedback_msg)
            await asyncio.sleep(0.5)  # 模拟泊车过程
        
        goal_handle.succeed()
        result = Fibonacci.Result()
        result.sequence = feedback_msg.sequence
        return result

// C++动作客户端 (rclcpp)
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include <action_tutorials_interfaces/action/fibonacci.hpp>

class ParkingActionClient : public rclcpp::Node {
public:
    using Fibonacci = action_tutorials_interfaces::action::Fibonacci;
    using GoalHandle = rclcpp_action::ClientGoalHandle<Fibonacci>;
    
    ParkingActionClient() : Node("parking_action_client") {
        client_ = rclcpp_action::create_client<Fibonacci>(this, "parking");
    }
    
    void send_goal(int order) {
        auto goal_msg = Fibonacci::Goal();
        goal_msg.order = order;
        
        auto send_goal_options = rclcpp_action::Client<Fibonacci>::SendGoalOptions();
        send_goal_options.goal_response_callback = 
            [this](GoalHandle::SharedPtr goal_handle) {
                if (goal_handle) {
                    RCLCPP_INFO(this->get_logger(), "目标被接受");
                } else {
                    RCLCPP_ERROR(this->get_logger(), "目标被拒绝");
                }
            };
        send_goal_options.feedback_callback = 
            [this](GoalHandle::SharedPtr, const std::shared_ptr<const Fibonacci::Feedback> feedback) {
                RCLCPP_INFO(this->get_logger(), "反馈: %ld", feedback->sequence.back());
            };
        send_goal_options.result_callback = 
            [this](const GoalHandle::WrappedResult& result) {
                if (result.code == rclcpp_action::ResultCode::SUCCEEDED) {
                    RCLCPP_INFO(this->get_logger(), "泊车成功");
                }
            };
        client_->async_send_goal(goal_msg, send_goal_options);
    }
private:
    rclcpp_action::Client<Fibonacci>::SharedPtr client_;
};

4.5 案例拆解：自动驾驶中的混合编程实践

在某自动驾驶项目开发中，团队需要实现一个"动态避障"功能。系统包含三个节点：感知节点（Python，处理摄像头数据）、规划节点（C++，生成避障轨迹）、控制节点（C++，执行轨迹）。初始方案全部使用Python，但在实际测试中发现规划节点处理延迟高达200ms，无法满足100ms的实时性要求。

性能分析：通过ros2 topic hz /obstacles和ros2 topic delay /trajectory命令检测，发现瓶颈在Python的GIL锁和消息序列化开销。规划节点需要处理大量矩阵运算，Python的NumPy虽然高效，但消息转换和回调调度消耗了过多时间。

优化方案：将规划节点重写为C++，利用Eigen库进行矩阵运算，并使用rclcpp::executors::MultiThreadedExecutor并行处理订阅回调。感知节点保持Python，因为其算法迭代频繁，Python开发效率更高。控制节点使用C++以确保实时性。

实施步骤：1）定义自定义消息ObstacleArray.msg和Trajectory.msg；2）用C++重写规划节点，使用create_subscription订阅障碍物，create_publisher发布轨迹；3）配置QoS，感知节点使用BEST_EFFORT，规划节点使用RELIABLE；4）使用ros2 launch启动所有节点，设置参数executor: multi_threaded。

结果对比：优化后规划节点延迟从200ms降至45ms，系统整体响应时间满足100ms要求。CPU占用率从85%降至60%，内存占用减少30%。

复盘经验：混合编程的关键在于明确性能边界。Python适合算法原型、数据可视化和工具脚本；C++适合实时控制、高频数据处理和性能关键路径。在项目初期就应建立性能测试基准，避免后期大规模重构。