3、ROS2基础编程：C++与Python节点编写、发布者与订阅者实现、自定义消息接口、launch文件编写、参数服务器使用

好，咱们直接进入正题。这一章是ROS2的“动手”章节，说白了就是让你真正写代码跑起来。很多初学者喜欢先啃理论，我个人的习惯是——先让轮子转起来，再回头理解为什么这么转。所以这一章，我会带着你从零写一个节点，然后一步步把它变成能用的系统。

3.1 节点编写：C++与Python的“Hello World”

节点是ROS2的最小执行单元。你可以把它理解成一个独立的小程序，专门干一件事。比如一个节点负责读激光雷达，另一个节点负责规划路径。

先看C++版本。创建一个包，然后在src目录下写一个cpp文件：

#include "rclcpp/rclcpp.hpp"

int main(int argc, char **argv) {
    rclcpp::init(argc, argv);
    auto node = std::make_shared<rclcpp::Node>("my_first_cpp_node");
    RCLCPP_INFO(node->get_logger(), "C++节点已启动！");
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}

这段代码干了三件事：初始化ROS2、创建节点、让节点保持运行。嗯，这里要注意，rclcpp::spin是个阻塞函数，它会一直等在那里，直到你按Ctrl+C。

再看Python版本，其实更简洁：

import rclpy
from rclpy.node import Node

def main(args=None):
    rclpy.init(args=args)
    node = Node("my_first_py_node")
    node.get_logger().info("Python节点已启动！")
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

我个人觉得Python版本更适合快速原型验证。我在项目中经常先用Python把逻辑跑通，再用C++重写一遍做性能优化。你想想看，调试阶段Python的print大法多方便啊。

3.2 发布者与订阅者：让节点“说话”

节点之间怎么通信？靠的就是发布者（Publisher）和订阅者（Subscriber）。一个节点发布消息，另一个节点接收消息。这就像电台和收音机的关系。

写一个发布者，每隔1秒发一个“Hello”字符串：

// C++ 发布者示例
auto publisher = node->create_publisher<std_msgs::msg::String>("topic_hello", 10);
rclcpp::WallRate rate(1.0);
while (rclcpp::ok()) {
    auto msg = std_msgs::msg::String();
    msg.data = "Hello, ROS2!";
    publisher->publish(msg);
    rate.sleep();
}

对应的订阅者：

// C++ 订阅者示例
auto subscription = node->create_subscription<std_msgs::msg::String>(
    "topic_hello", 10, [](const std_msgs::msg::String::SharedPtr msg) {
        RCLCPP_INFO(rclcpp::get_logger("subscriber"), "收到: %s", msg->data.c_str());
    });

这里有个坑，我曾经遇到过——队列长度设成10是什么意思？说白了就是缓冲区大小。如果订阅者处理速度跟不上发布者，消息会积压。队列满了之后，最老的消息会被丢弃。所以要根据你的实际场景调整这个值，不是越大越好。

3.3 自定义消息接口：别再用“万能”字符串了

用标准消息类型（比如String、Int32）做练习没问题，但真实项目中，你需要定义自己的消息结构。比如一个“车辆状态”消息，包含速度、转向角、GPS坐标。

在包的msg目录下创建VehicleStatus.msg：

float32 speed
float32 steering_angle
float64 latitude
float64 longitude
string vehicle_id

然后在CMakeLists.txt或setup.py中注册这个消息接口。编译之后，你就可以像使用标准消息一样使用它了：

# Python中使用自定义消息
from my_interfaces.msg import VehicleStatus

msg = VehicleStatus()
msg.speed = 30.5
msg.steering_angle = 0.2

我个人建议，消息字段尽量用基础类型，别嵌套太深。嵌套太深的消息，序列化和反序列化会消耗大量CPU，在嵌入式平台上尤其明显。

3.4 Launch文件编写：一键启动所有节点

手动启动每个节点太累了。Launch文件就是用来批量启动节点的。ROS2的Launch文件用Python写，比ROS1的XML灵活得多。

一个简单的launch文件：

from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([
        Node(
            package='my_package',
            executable='my_cpp_node',
            name='cpp_node',
            output='screen'
        ),
        Node(
            package='my_package',
            executable='my_py_node',
            name='py_node',
            output='screen'
        ),
    ])

你还可以在launch文件中设置参数、条件判断、包含其他launch文件。我在做多传感器融合项目时，一个launch文件里启动了十几个节点，包括相机驱动、激光雷达驱动、IMU驱动、融合算法节点。没有launch文件，光启动这些节点就能让人崩溃。

3.5 参数服务器使用：让节点“可配置”

参数服务器说白了就是一个全局的键值对存储。你可以在启动时设置参数，也可以在运行时动态修改。

声明参数并获取：

// C++ 参数示例
node->declare_parameter("max_speed", 10.0);
double max_speed = node->get_parameter("max_speed").as_double();

在launch文件中设置参数：

Node(
    package='my_package',
    executable='my_node',
    parameters=[{'max_speed': 20.0}]
)

这里有个经验之谈——参数不要滥用。我曾经见过一个项目，把算法内部的所有常数都放到参数服务器里，结果参数文件有几百行，维护起来非常痛苦。我的建议是：只有那些需要外部配置的、可能经常变化的值才用参数。算法内部的固定常数，直接写在代码里就好。

这张图把本章的知识点串起来了。你看，所有东西都围绕着“节点”这个核心。节点怎么创建、怎么通信、怎么定义数据、怎么批量启动、怎么配置参数——把这五个点吃透，ROS2基础编程你就入门了。

最后说一句，代码一定要亲手敲一遍。看十遍不如写一遍，写一遍不如调试一遍。遇到问题别怕，ROS2的错误信息其实挺友好的，仔细看日志，大部分问题都能自己解决。