4、ROS2基础编程实战：编写第一个发布者与订阅者节点、自定义消息接口、编译与运行

说实话，很多初学者学ROS2，上来就被各种概念砸晕了。节点、话题、服务、动作……听着就头大。

但我想告诉你一个秘密：你只需要搞懂发布者和订阅者，就能解决自动驾驶中80%的通信问题。传感器数据、控制指令、状态信息，本质上都是“一个节点发，一个节点收”。

今天我们就亲手写一个。别怕，代码量很少，逻辑很直白。

4.1 先搭个架子：创建功能包

我习惯用命令行的方式创建功能包，干净利落。打开终端，进入你的工作空间：

cd ~/ros2_ws/src
ros2 pkg create --build-type ament_python vehicle_control_pkg

这条命令会生成一个Python风格的功能包。里面有个package.xml和setup.py，这两个文件是ROS2认识这个包的“身份证”。

嗯，这里要注意：package.xml里要填好依赖。我们后面要用到rclpy和std_msgs，先写上：

<depend>rclpy</depend>
<depend>std_msgs</depend>

我个人习惯在setup.py里把entry_points也提前配好，这样后面不用再改。不过今天我们先手动写节点文件，再回来配置入口点。

4.2 写第一个发布者节点

在vehicle_control_pkg/vehicle_control_pkg/目录下，新建一个文件叫speed_publisher.py。

为什么叫这个名？我在项目中遇到过，命名越直白，三个月后你自己回来看代码越省心。

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

class SpeedPublisher(Node):
    def __init__(self):
        super().__init__('speed_publisher')
        self.publisher_ = self.create_publisher(Float64, 'vehicle_speed', 10)
        self.timer = self.create_timer(0.1, self.timer_callback)  # 10Hz
        self.speed = 0.0

    def timer_callback(self):
        self.speed += 0.5  # 模拟加速
        msg = Float64()
        msg.data = self.speed
        self.publisher_.publish(msg)
        self.get_logger().info(f'发布速度: {msg.data:.1f} km/h')

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

if __name__ == '__main__':
    main()

这段代码干了三件事：

• 创建一个节点，名字叫speed_publisher
• 在话题vehicle_speed上发布Float64类型的数据
• 每0.1秒回调一次，模拟车速增加

你想想看，如果这是真实车辆，车速不可能一直增加。但作为教学示例，这样写足够让你看清数据流动的过程。

4.3 写订阅者节点

同样目录下，新建speed_subscriber.py：

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

class SpeedSubscriber(Node):
    def __init__(self):
        super().__init__('speed_subscriber')
        self.subscription = self.create_subscription(
            Float64,
            'vehicle_speed',
            self.listener_callback,
            10)
        self.subscription  # 防止被垃圾回收

    def listener_callback(self, msg):
        self.get_logger().info(f'收到速度: {msg.data:.1f} km/h')

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

if __name__ == '__main__':
    main()

这段代码更简单。订阅vehicle_speed话题，收到数据就打印出来。

我曾经犯过一个低级错误：忘记给self.subscription赋值，结果订阅者节点一直收不到消息。后来查了半天才发现是Python的垃圾回收把订阅器给回收了。所以上面代码里特意写了那句注释。

4.4 配置入口点并编译

现在回到setup.py，找到entry_points部分，改成：

entry_points={
    'console_scripts': [
        'speed_publisher = vehicle_control_pkg.speed_publisher:main',
        'speed_subscriber = vehicle_control_pkg.speed_subscriber:main',
    ],
},

然后编译：

cd ~/ros2_ws
colcon build --packages-select vehicle_control_pkg
source install/setup.bash

编译成功后，开两个终端，分别运行：

# 终端1
ros2 run vehicle_control_pkg speed_publisher

# 终端2
ros2 run vehicle_control_pkg speed_subscriber

你会看到订阅者终端不断打印出递增的速度值。恭喜，你的第一个ROS2通信系统跑起来了！

4.5 自定义消息接口

Float64太简单了。真实自动驾驶中，一条消息往往包含多个字段。比如车速消息，除了速度值，还应该有时间戳、置信度、车辆ID等。

这时候就需要自定义消息接口。

在vehicle_control_pkg/目录下新建msg/文件夹，创建VehicleSpeed.msg文件：

float64 speed
float64 confidence
string vehicle_id
builtin_interfaces/Time timestamp

然后修改package.xml，添加依赖：

<depend>builtin_interfaces</depend>
<depend>rosidl_default_generators</depend>
<member_of_group>rosidl_interface_packages</member_of_group>

再修改CMakeLists.txt（注意，Python包也可以用CMake来编译消息接口）：

find_package(rosidl_default_generators REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME}
  "msg/VehicleSpeed.msg"
)

重新编译：

colcon build --packages-select vehicle_control_pkg
source install/setup.bash

现在你就可以在代码里用vehicle_control_pkg.msg.VehicleSpeed了。把发布者改成：

from vehicle_control_pkg.msg import VehicleSpeed

# 在回调中
msg = VehicleSpeed()
msg.speed = self.speed
msg.confidence = 0.95
msg.vehicle_id = 'AV-001'
msg.timestamp = self.get_clock().now().to_msg()

4.6 核心逻辑流程图

下面这张图帮你理清今天学的内容：

4.7 验证你的成果

跑起来之后，可以用命令行检查：

# 查看话题列表
ros2 topic list

# 查看话题内容
ros2 topic echo /vehicle_speed

# 查看话题信息
ros2 topic info /vehicle_speed

你会看到/vehicle_speed话题有两个节点在通信。这就是ROS2最基本的“发布-订阅”模式。

说白了，自动驾驶里成百上千个节点，都是用这个模式在交换数据。你掌握了这个，就等于拿到了打开ROS2大门的钥匙。

可可精品资料库