3、ROS2基础编程：C++与Python节点编写、话题发布与订阅、自定义消息接口

好，咱们进入正题。这一章是ROS2开发的核心基本功——写节点、发话题、收话题，还有自己定义消息结构。说白了，就是让机器人各个部件能“说话”和“听懂”。

我刚开始学ROS1那会儿，最头疼的就是搞不清节点和话题的关系。后来做项目多了才明白，这其实就是个“发布-订阅”模式，跟公众号差不多——你关注了某个话题，有消息推送你就能收到。

3.1 节点：ROS2的最小执行单元

节点就是一个独立的可执行程序。每个节点负责一件事，比如控制机械臂关节、读取力传感器数据、处理视觉信息。多个节点通过话题通信，组成一个完整的系统。

核心概念：ROS2中每个节点都有一个唯一的名称，通过rclcpp（C++）或rclpy（Python）库实现。

3.1.1 C++节点编写示例

我个人习惯用C++写性能敏感的部分，比如关节控制。下面是一个最简单的发布者节点：

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

int main(int argc, char *argv[])
{
  rclcpp::init(argc, argv);
  auto node = std::make_shared<rclcpp::Node>("simple_publisher");
  auto publisher = node->create_publisher<std_msgs::msg::String>("topic", 10);
  rclcpp::WallRate rate(1); // 1 Hz

  while (rclcpp::ok())
  {
    auto message = std_msgs::msg::String();
    message.data = "Hello from ROS2 C++ node!";
    RCLCPP_INFO(node->get_logger(), "Publishing: '%s'", message.data.c_str());
    publisher->publish(message);
    rate.sleep();
  }

  rclcpp::shutdown();
  return 0;
}

小技巧：创建发布者时第二个参数是队列大小。我一般设10，如果消息处理不过来，旧的会被丢弃，避免内存暴涨。

3.1.2 Python节点编写示例

Python写起来快，适合原型验证和调试。同样的功能，Python版本更简洁：

# simple_publisher.py
import rclpy
from rclpy.node import Node
from std_msgs.msg import String

class SimplePublisher(Node):
    def __init__(self):
        super().__init__('simple_publisher')
        self.publisher_ = self.create_publisher(String, 'topic', 10)
        timer_period = 1.0  # seconds
        self.timer = self.create_timer(timer_period, self.timer_callback)
        self.i = 0

    def timer_callback(self):
        msg = String()
        msg.data = f'Hello from ROS2 Python node: {self.i}'
        self.get_logger().info(f'Publishing: "{msg.data}"')
        self.publisher_.publish(msg)
        self.i += 1

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

if __name__ == '__main__':
    main()

嗯，这里要注意：Python版用了定时器回调，C++版用了循环加sleep。两种方式都行，但我建议在复杂系统里用定时器，更符合事件驱动模型。

3.2 话题发布与订阅：让节点“对话”

话题是ROS2通信的核心机制。一个节点发布消息到话题，另一个节点订阅这个话题就能收到消息。这是异步的、一对多的通信方式。

我曾经在一个抓取项目中，机械臂控制节点订阅视觉检测结果话题，每次检测到目标物体就触发抓取动作。如果话题通信延迟超过50ms，机械臂就会抓空。所以话题的实时性很重要。

3.2.1 订阅者节点示例（C++）

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

void topic_callback(const std_msgs::msg::String::SharedPtr msg)
{
  RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "I heard: '%s'", msg->data.c_str());
}

int main(int argc, char *argv[])
{
  rclcpp::init(argc, argv);
  auto node = std::make_shared<rclcpp::Node>("simple_subscriber");
  auto subscription = node->create_subscription<std_msgs::msg::String>(
    "topic", 10, topic_callback);
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}

3.2.2 订阅者节点示例（Python）

# simple_subscriber.py
import rclpy
from rclpy.node import Node
from std_msgs.msg import String

class SimpleSubscriber(Node):
    def __init__(self):
        super().__init__('simple_subscriber')
        self.subscription = self.create_subscription(
            String,
            'topic',
            self.listener_callback,
            10)
        self.subscription  # prevent unused variable warning

    def listener_callback(self, msg):
        self.get_logger().info(f'I heard: "{msg.data}"')

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

避坑指南：我曾经在调试时发现订阅者收不到消息，折腾了半天才发现是话题名称拼写不一致。ROS2的话题名是大小写敏感的，而且不能有空格。建议用下划线分隔单词，比如/arm_joint_state。

3.3 自定义消息接口：定义你自己的数据结构

标准消息类型（如String、Int32）只能传简单数据。实际项目中，你需要传更复杂的结构——比如机械臂的关节角度、抓取姿态、力传感器读数。这时候就得自定义消息接口。

你想想看，一个抓取指令至少包含：目标位置（x,y,z）、姿态（四元数）、抓取力度、超时时间。把这些打包成一个自定义消息，比传一堆零散参数优雅得多。

3.3.1 创建自定义消息文件

在功能包的msg目录下创建GraspCommand.msg：

# GraspCommand.msg
# 机械臂抓取指令消息
geometry_msgs/Point target_position
geometry_msgs/Quaternion target_orientation
float32 grip_force
float32 timeout_seconds
bool use_adaptive_grip

3.3.2 配置CMakeLists.txt和package.xml

在CMakeLists.txt中添加：

find_package(rosidl_default_generators REQUIRED)
find_package(geometry_msgs REQUIRED)

rosidl_generate_interfaces(${PROJECT_NAME}
  "msg/GraspCommand.msg"
  DEPENDENCIES geometry_msgs
)

在package.xml中添加：

<build_depend>rosidl_default_generators</build_depend>
<exec_depend>rosidl_default_runtime</exec_depend>
<member_of_group>rosidl_interface_packages</member_of_group>

3.3.3 使用自定义消息

编译后，就可以在代码中使用了：

// C++ 中使用自定义消息
#include "your_package/msg/grasp_command.hpp"

auto msg = your_package::msg::GraspCommand();
msg.target_position.x = 0.5;
msg.target_position.y = 0.2;
msg.target_position.z = 0.3;
msg.grip_force = 10.0;
msg.timeout_seconds = 5.0;
msg.use_adaptive_grip = true;

# Python 中使用自定义消息
from your_package.msg import GraspCommand

msg = GraspCommand()
msg.target_position.x = 0.5
msg.target_position.y = 0.2
msg.target_position.z = 0.3
msg.grip_force = 10.0
msg.timeout_seconds = 5.0
msg.use_adaptive_grip = True

经验之谈：自定义消息的字段类型尽量用标准ROS类型（如geometry_msgs），这样不同功能包之间可以复用。我见过有人把所有字段都定义成float32数组，结果代码可读性极差，维护起来想哭。

3.4 本章知识体系总览

下面这张图展示了节点、话题、自定义消息之间的关系：

ROS2 节点通信与自定义消息架构 发布者节点 C++ / Python 发布话题数据 订阅者节点 C++ / Python 接收话题数据 话题 (Topic) 异步通信 / 一对多 消息类型：标准 / 自定义 自定义消息接口 (.msg 文件) GraspCommand.msg geometry_msgs/Point float32 grip_force bool use_adaptive_grip 编译生成 话题通信 发布/订阅 异步传输 订阅者 接收并处理 自定义消息

3.5 实战要点总结

知识点 关键点 常见坑
C++节点编写 使用rclcpp库，注意初始化与关闭 忘记调用rclcpp::shutdown()导致内存泄漏
Python节点编写 使用rclpy库，推荐用类封装 回调函数中执行耗时操作会阻塞事件循环
话题发布 指定话题名、消息类型、队列大小 话题名拼写错误或大小写不一致
话题订阅 回调函数处理接收到的消息 回调函数中不要做阻塞操作
自定义消息 定义.msg文件，配置CMakeLists和package.xml 忘记添加依赖或编译后未source环境

重要提醒：自定义消息修改后，一定要重新编译整个功能包，并且source setup.bash。我曾经改了个消息字段类型，忘了重新编译，结果运行时一直报类型不匹配，排查了半小时才发现问题。

好了，这一章的内容就这些。节点编写、话题通信、自定义消息，这三样东西是ROS2开发的基石。后面的章节里，我们会用这些基础组件搭建完整的机械臂抓取系统。到时候你会发现，现在打下的基础越扎实，后面写代码就越顺手。

上一章 返回目录 下一章