3. 编写第一个ROS2节点

任务目标

编写一个ROS2发布者节点，以固定频率向话题 /robot_status 发布机器人状态消息。消息类型使用标准接口 std_msgs/msg/String，内容包含时间戳与状态文本，用于模拟机器人周期性上报运行状态。

环境准备

• 已安装ROS2（Humble或更新版本），并完成 source /opt/ros/<distro>/setup.bash。
• 创建工作空间（若未创建）：mkdir -p ~/ros2_ws/src。

实现步骤

在 ~/ros2_ws/src 下创建功能包 robot_status_publisher（使用 ament_python 构建类型），然后编写发布者节点代码。

关键完整代码

创建文件 robot_status_publisher/robot_status_publisher/publisher_node.py：

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

class RobotStatusPublisher(Node):
    def __init__(self):
        super().__init__('robot_status_publisher')
        self.publisher_ = self.create_publisher(String, 'robot_status', 10)
        timer_period = 1.0  # 秒
        self.timer = self.create_timer(timer_period, self.timer_callback)

    def timer_callback(self):
        msg = String()
        now = datetime.now().strftime("%Y-%m-%d %H:%M:%S")
        msg.data = f"[{now}] Robot status: OK"
        self.publisher_.publish(msg)
        self.get_logger().info(f'Publishing: "{msg.data}"')

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

if __name__ == '__main__':
    main()

确保 setup.py 中正确配置入口点：

entry_points={
    'console_scripts': [
        'robot_status_publisher = robot_status_publisher.publisher_node:main',
    ],
},

运行与验证

1. 构建并配置环境：

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

2. 运行发布者节点：

ros2 run robot_status_publisher robot_status_publisher

3. 另开终端，查看话题列表与消息内容：

ros2 topic list
ros2 topic echo /robot_status

通过条件：ros2 topic list 中显示 /robot_status；ros2 topic echo 每秒输出一条形如 data: "[2025-01-01 12:00:00] Robot status: OK" 的消息，且发布者终端同步打印日志。

常见问题

• 找不到 std_msgs：确认已安装 ros-<distro>-std-msgs 包。
• colcon build 失败：检查 package.xml 中是否声明了对 rclpy 和 std_msgs 的依赖。
• 无法 echo 到消息：确认发布者节点正在运行，且话题名称拼写一致。