📚 课程目录第3章 ROS2编程基础

3. ROS2 编程基础：使用 Python 和 C++ 编写发布订阅、服务客户端、动作客户端与服务器

3.1 核心原理：三大通信机制的本质区别

ROS2 的机器人算法最终都要落实到节点间的数据交互上。理解发布订阅（Topic）、服务（Service）和动作（Action）三者的本质区别，是编写正确代码的前提。Topic 是异步、单向、持续的数据流，适合传感器数据、状态信息等高频广播；Service 是同步、双向、一次性的请求-应答模式，适合查询参数、触发开关等短任务；Action 则是异步、双向、可取消的长任务模式，它在 Service 的基础上增加了进度反馈和取消机制，适合导航、机械臂运动规划等耗时操作。

从底层实现看，Topic 基于发布-订阅模型，多个订阅者可以同时接收同一话题数据；Service 基于客户端-服务器模型，一次请求只能由一个服务器处理；Action 则拆分为三个 Topic（goal、feedback、result）和一个 Service（cancel），但上层 API 封装了这些细节。选择哪种通信方式，取决于任务的实时性、持续性和可取消性需求。

3.2 方法流程：Python 与 C++ 的发布订阅实现

编写 ROS2 节点的核心流程可以归纳为五个步骤：导入接口 → 创建节点 → 创建发布者/订阅者 → 编写回调逻辑 → 运行节点。Python 和 C++ 在 API 设计上高度对称，但语法和构建方式不同。Python 使用 rclpy，C++ 使用 rclcpp，两者都遵循相同的生命周期管理。

下面以发布 std_msgs/msg/String 话题为例，展示两种语言的对应实现。Python 版发布者：

import rclpy
from rclpy.node import Node
from std_msgs.msg import String

class Talker(Node):
    def __init__(self):
        super().__init__('talker')
        self.pub = self.create_publisher(String, 'chatter', 10)
        self.timer = self.create_timer(0.5, self.timer_callback)

    def timer_callback(self):
        msg = String()
        msg.data = 'Hello ROS2'
        self.pub.publish(msg)
        self.get_logger().info(f'发布: {msg.data}')

def main(args=None):
    rclpy.init(args=args)
    node = Talker()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

C++ 版发布者：

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

class Talker : public rclcpp::Node {
public:
    Talker() : Node("talker") {
        pub_ = this->create_publisher<std_msgs::msg::String>("chatter", 10);
        timer_ = this->create_wall_timer(
            std::chrono::milliseconds(500),
            std::bind(&Talker::timer_callback, this));
    }
private:
    void timer_callback() {
        auto msg = std_msgs::msg::String();
        msg.data = "Hello ROS2";
        pub_->publish(msg);
        RCLCPP_INFO(this->get_logger(), "发布: %s", msg.data.c_str());
    }
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pub_;
    rclcpp::TimerBase::SharedPtr timer_;
};

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

订阅者的实现逻辑完全对称，只需将 create_publisher 换成 create_subscription，并在回调函数中处理接收到的消息。C++ 的订阅回调需要绑定 std::bind 或使用 lambda 表达式，Python 则直接传入回调函数名。

3.3 案例实战：服务客户端与服务器

以自定义服务 AddTwoInts.srv 为例，服务器接收两个整数并返回它们的和。首先定义接口文件：

int64 a
int64 b
---
int64 sum

Python 服务器端实现：

from example_interfaces.srv import AddTwoInts
import rclpy
from rclpy.node import Node

class Server(Node):
    def __init__(self):
        super().__init__('add_server')
        self.srv = self.create_service(AddTwoInts, 'add_two_ints', self.callback)

    def callback(self, request, response):
        response.sum = request.a + request.b
        self.get_logger().info(f'收到: {request.a} + {request.b} = {response.sum}')
        return response

Python 客户端需要等待服务可用后再发送请求，否则会抛出异常。使用 wait_for_service 方法阻塞等待：

client = node.create_client(AddTwoInts, 'add_two_ints')
while not client.wait_for_service(timeout_sec=1.0):
    node.get_logger().info('等待服务...')
req = AddTwoInts.Request()
req.a = 3; req.b = 5
future = client.call_async(req)
rclpy.spin_until_future_complete(node, future)
result = future.result()
print(f'结果: {result.sum}')

C++ 客户端使用 async_send_request 并注册回调，或者使用 spin_until_future_complete 同步等待。注意 C++ 中需要管理 SharedFuture 的生命周期，避免回调访问已销毁的对象。

3.4 动作客户端与服务器：导航任务实现

动作通信适合需要长时间执行且可中断的任务。以 ROS2 自带的 nav2_msgs/action/NavigateToPose 为例，动作服务器接收目标点坐标，持续反馈当前进度，最终返回是否成功。动作接口包含三部分：goal（目标）、feedback（反馈）、result（结果）。

动作服务器端需要继承 ActionServer 基类，并实现四个回调：handle_goal（接受或拒绝目标）、handle_cancel（处理取消请求）、handle_accepted（启动执行线程）、execute（执行具体任务并发布反馈）。核心代码结构如下：

class NavigateServer(Node):
    def __init__(self):
        super().__init__('navigate_server')
        self.action_server = ActionServer(
            self, NavigateToPose, 'navigate',
            handle_goal_callback=self.goal_callback,
            handle_cancel_callback=self.cancel_callback,
            execute_callback=self.execute_callback)

    def goal_callback(self, goal_request):
        # 检查目标合法性，返回 GoalResponse.ACCEPT 或 REJECT
        return GoalResponse.ACCEPT

    def execute_callback(self, goal_handle):
        # 循环发布 feedback，完成后设置 result
        for i in range(100):
            feedback = NavigateToPose.Feedback()
            feedback.current_pose.pose.position.x = i * 0.1
            goal_handle.publish_feedback(feedback)
            time.sleep(0.1)
        goal_handle.succeed()
        result = NavigateToPose.Result()
        return result

动作客户端通过 send_goal_async 发送目标，并注册 feedback 和 done 回调。客户端可以随时调用 cancel_goal 取消任务，服务器收到取消请求后应停止执行并返回状态。

3.5 误区与复盘：常见错误及规避策略

误区	现象	正确做法
忘记初始化 rclpy/rclcpp	节点创建失败或崩溃	Python 调用 `rclpy.init()`，C++ 调用 `rclcpp::init()`
回调函数执行耗时操作	节点卡死，其他回调不响应	耗时任务放入独立线程或使用动作
服务客户端未等待服务就绪	抛出 Service unavailable 异常	使用 `wait_for_service` 或循环重试
C++ 中回调绑定错误	编译错误或运行时崩溃	使用 `std::bind` 时注意参数占位符 `_1`
话题/服务名拼写不一致	节点间无法通信	使用 `ros2 topic list` 和 `ros2 service list` 验证
动作服务器未调用 `succeed()`	客户端永远等待 result	任务完成后必须显式设置目标状态

一个典型的复盘案例：某开发者编写服务客户端时，在 main 函数中直接调用 call_async 后立即退出，导致请求尚未发送程序就结束了。正确做法是使用 rclpy.spin_until_future_complete 保持节点运行直到收到响应。另一个常见问题是 C++ 中在回调里访问已析构的成员变量，解决方法是使用 shared_ptr 管理节点生命周期，并确保回调绑定的是有效对象。

调试技巧：使用 ros2 topic echo /chatter 验证发布订阅是否正常；使用 ros2 service call /add_two_ints example_interfaces/srv/AddTwoInts "{a: 3, b: 5}" 测试服务；使用 ros2 action send_goal /navigate nav2_msgs/action/NavigateToPose "{pose: {pose: {position: {x: 1.0}}}}" --feedback 测试动作通信。

注意：在 C++ 中，如果使用 create_subscription 的回调需要访问类成员，务必使用 this 指针绑定，并确保节点对象在回调执行期间仍然存活。推荐使用 rclcpp::spin 配合 std::make_shared 创建节点，避免栈上对象提前析构。

通过以上案例可以看到，Python 和 C++ 在 ROS2 编程中的核心逻辑完全一致，差异仅在于语法和构建系统。Python 适合快速原型验证，C++ 适合性能敏感模块。实际项目中常采用混合编程：用 C++ 编写底层驱动和算法模块，用 Python 编写上层逻辑和调试工具。掌握两种语言的对应关系，能让你在阅读和移植代码时事半功倍。