4. ROS2 发布 PX4 解锁与模式切换指令

任务说明

编写一个 ROS2 节点，向 PX4 的 /fmu/in/vehicle_command 话题发布 px4_msgs/msg/VehicleCommand 消息，实现三种指令：解锁（ARM）、上锁（DISARM）、切换到 Position 模式。通过订阅 /fmu/out/vehicle_status 观察 arming_state 与 nav_state 字段变化来验证响应。

环境与依赖

• ROS2（Humble 或更新），已安装 px4_msgs 包（与 PX4 固件版本匹配）。
• PX4 SITL 或真机已运行，且 uXRCE-DDS 桥接已启动，/fmu/in/vehicle_command 与 /fmu/out/vehicle_status 话题可见。
• 工作空间已 source，px4_msgs 可被 colcon build 找到。

关键常量与消息字段

PX4 的 VehicleCommand 使用 command 字段区分动作，param1 携带模式值。timestamp 必须使用 PX4 时间基准（微秒），因此用 node.get_clock().now() 转换。解锁/上锁命令为 VEHICLE_CMD_COMPONENT_ARM_DISARM (400)，param1=1 解锁、param1=0 上锁；模式切换为 VEHICLE_CMD_DO_SET_MODE (176)，param1=1（自定义模式）、param2=3（Position 模式）。

完整实现

下面节点在启动后按顺序执行：解锁 → 等待 3 秒 → 切换 Position 模式 → 等待 3 秒 → 上锁。同时订阅状态话题打印 arming_state 与 nav_state。

import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
from px4_msgs.msg import VehicleCommand, VehicleStatus

ARM = 400
SET_MODE = 176
POSITION_MODE = 3

class CmdNode(Node):
    def __init__(self):
        super().__init__('px4_cmd_node')
        qos = QoSProfile(reliability=ReliabilityPolicy.BEST_EFFORT,
                         history=HistoryPolicy.KEEP_LAST, depth=1)
        self.pub = self.create_publisher(VehicleCommand, '/fmu/in/vehicle_command', qos)
        self.sub = self.create_subscription(VehicleStatus, '/fmu/out/vehicle_status',
                                            self.on_status, qos)
        self.status = None
        self.timer = self.create_timer(1.0, self.run_once)
        self.step = 0

    def on_status(self, msg):
        self.status = msg
        self.get_logger().info(
            f'arming_state={msg.arming_state} nav_state={msg.nav_state}')

    def send(self, command, p1, p2=0.0):
        m = VehicleCommand()
        m.timestamp = int(self.get_clock().now().nanoseconds / 1000)
        m.command = command
        m.param1 = float(p1)
        m.param2 = float(p2)
        m.target_system = 1
        m.target_component = 1
        m.source_system = 1
        m.source_component = 1
        m.from_external = True
        self.pub.publish(m)
        self.get_logger().info(f'sent command={command} param1={p1} param2={p2}')

    def run_once(self):
        if self.step == 0:
            self.send(ARM, 1)          # 解锁
        elif self.step == 3:
            self.send(SET_MODE, 1, POSITION_MODE)  # Position 模式
        elif self.step == 6:
            self.send(ARM, 0)          # 上锁
        elif self.step > 6:
            self.timer.cancel()
        self.step += 1

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

if __name__ == '__main__':
    main()

运行方式

# 终端 1：启动 PX4 SITL（含 uXRCE-DDS）
# 终端 2：启动 MicroXRCEAgent
MicroXRCEAgent udp4 -p 8888
# 终端 3：运行本节点
python3 px4_cmd_node.py

验证方法与通过条件

• 节点日志应依次出现 sent command=400 param1=1、sent command=176 param1=1 param2=3、sent command=400 param1=0。
• 订阅回调日志中，解锁后 arming_state 应变为 2（ARMED），切换模式后 nav_state 应变为 3（POSCTL），上锁后 arming_state 回到 1（DISARMED）。
• 若状态未变化，检查 QoS 是否为 BEST_EFFORT、from_external=True 是否设置、以及 PX4 是否处于允许解锁的状态（如已通过预检）。