3、创建第一个机械臂节点：使用Python编写ROS2节点、发布关节状态、订阅关节指令、使用rqt_graph可视化

好，咱们正式开始写代码了。前面两章我们把环境搭好、概念理清，现在轮到真正动手——创建你的第一个机械臂ROS2节点。

说实话，我第一次写ROS2节点的时候，心里也没底。那时候刚从ROS1迁移过来，总觉得别扭。但后来我发现，只要你理解了“节点就是一个小程序，它只管发布和订阅”这个核心，剩下的都是套路。

3.1 节点到底是个什么东西？

你想想看，一个机械臂要动起来，需要哪些信息？

• 它要知道每个关节当前转到什么位置了（关节状态）
• 它要接收外部发来的指令，告诉它“往哪转、转多快”（关节指令）

这两个需求，正好对应ROS2里最基础的两个通信模式：发布（Publisher）和订阅（Subscriber）。

我习惯把节点想象成一个“小管家”。它手里拿着两个工具：一个喇叭（发布者），用来喊出关节状态；一个对讲机（订阅者），用来听指令。嗯，就这么简单。

3.2 编写你的第一个节点

我们直接上手。创建一个Python文件，就叫 arm_controller.py。

我个人习惯把节点文件放在 scripts/ 目录下，这样结构清晰。当然你也可以放别处，但团队协作时统一路径能省不少麻烦。

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import JointState
from std_msgs.msg import Float64MultiArray

class ArmController(Node):
    def __init__(self):
        super().__init__('arm_controller_node')
        
        # 创建发布者：发布关节状态
        self.joint_state_pub = self.create_publisher(
            JointState, 
            '/joint_states', 
            10
        )
        
        # 创建订阅者：订阅关节指令
        self.joint_command_sub = self.create_subscription(
            Float64MultiArray,
            '/joint_commands',
            self.joint_command_callback,
            10
        )
        
        self.get_logger().info('机械臂节点已启动！')
    
    def joint_command_callback(self, msg):
        # 收到指令后，这里先简单打印出来
        self.get_logger().info(f'收到关节指令: {msg.data}')

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

if __name__ == '__main__':
    main()

3.3 发布关节状态——让机械臂“开口说话”

光有节点框架还不够，我们得让节点真正发布数据。机械臂的关节状态，说白了就是每个关节当前的角度。

假设我们有一个6轴机械臂，关节名称分别是 joint1 到 joint6。我们来写一个定时器，每隔0.1秒发布一次关节状态。

import math
from builtin_interfaces.msg import Duration

class ArmController(Node):
    def __init__(self):
        # ... 前面的代码不变 ...
        
        # 创建定时器，每100ms发布一次关节状态
        self.timer = self.create_timer(0.1, self.publish_joint_states)
        
        # 初始化关节位置（模拟数据）
        self.joint_positions = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        self.joint_names = ['joint1', 'joint2', 'joint3', 
                           'joint4', 'joint5', 'joint6']
    
    def publish_joint_states(self):
        msg = JointState()
        msg.header.stamp = self.get_clock().now().to_msg()
        msg.name = self.joint_names
        
        # 模拟关节缓慢转动
        self.joint_positions[0] += 0.01
        if self.joint_positions[0] > math.pi:
            self.joint_positions[0] = -math.pi
        
        msg.position = self.joint_positions
        self.joint_state_pub.publish(msg)

3.4 订阅关节指令——让机械臂“听懂人话”

发布搞定了，接下来是订阅。我们期望外部程序（比如一个运动规划节点）往 /joint_commands 话题发指令，我们的节点收到后做出响应。

这里我设计一个简单的回调函数，把收到的指令打印出来，并更新关节目标位置。

def joint_command_callback(self, msg):
    # 检查数据长度是否匹配
    if len(msg.data) != len(self.joint_names):
        self.get_logger().warn(
            f'指令长度不匹配！期望{len(self.joint_names)}个关节，'
            f'收到{len(msg.data)}个值'
        )
        return
    
    self.get_logger().info(
        f'收到新指令: {[round(v, 3) for v in msg.data]}'
    )
    # 这里可以添加实际控制逻辑，比如发送给电机驱动

你可能会问：“为什么用 Float64MultiArray 而不是自定义消息？”

其实都可以。我个人习惯在原型阶段先用标准消息类型，等系统稳定了再定义专用的接口文件。这样迭代速度快，不用频繁编译接口包。

3.5 使用rqt_graph可视化——看看节点到底连没连上

代码写完了，怎么验证它跑对了？

我最推荐的工具就是 rqt_graph。它能实时显示当前ROS2系统中所有节点和话题的连接关系。说白了，就是一张“谁在跟谁说话”的关系图。

启动方法很简单：

# 终端1：运行你的节点
ros2 run your_package arm_controller

# 终端2：打开rqt_graph
rqt_graph

你会看到类似下面的画面：

看到这张图，你就能确认：

• 节点 /arm_controller_node 确实在运行
• 它发布了 /joint_states 话题
• 如果有其他节点订阅了这个话题，也会显示出来

3.6 完整代码与运行测试

把上面几段代码拼起来，就是一个完整的机械臂节点了。我们来测试一下：

# 编译包
colcon build --packages-select your_package

# 运行节点
ros2 run your_package arm_controller

# 另开终端，查看话题列表
ros2 topic list
# 应该能看到 /joint_states 和 /joint_commands

# 手动发一条指令测试
ros2 topic pub /joint_commands std_msgs/msg/Float64MultiArray \
  "{data: [0.5, 0.2, -0.3, 0.0, 1.0, -0.5]}"

如果一切正常，你会看到节点终端打印出收到的指令数据。同时，rqt_graph 里也会显示完整的通信链路。

好了，第一个机械臂节点就这么写出来了。说实话，代码量不大，但背后的设计思路值得琢磨——发布和订阅的分离，让整个系统变得灵活可扩展。下一章我们会在这个基础上，让机械臂真正动起来。