3. ROS2 订阅 PX4 无人机状态

任务说明

编写一个 ROS2 节点，订阅 PX4 通过 uXRCE-DDS 桥接发布的 /fmu/out/vehicle_status 与 /fmu/out/vehicle_local_position 两个话题，实时打印飞行模式与本地位置（NED 坐标）。

环境与依赖

• ROS2（Humble 或 Foxy），已 source /opt/ros/$ROS_DISTRO/setup.bash
• PX4 已通过 uXRCE-DDS 与 ROS2 建立连接，两个话题可被 ros2 topic list 看到
• 消息类型：px4_msgs/msg/VehicleStatus、px4_msgs/msg/VehicleLocalPosition

数据流

完整代码

文件 px4_status_listener.py：

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

class Px4StatusListener(Node):
    def __init__(self):
        super().__init__('px4_status_listener')
        qos = QoSProfile(
            reliability=ReliabilityPolicy.BEST_EFFORT,
            durability=DurabilityPolicy.TRANSIENT_LOCAL,
            history=HistoryPolicy.KEEP_LAST,
            depth=1,
        )
        self.mode = None
        self.pos = None
        self.create_subscription(
            VehicleStatus, '/fmu/out/vehicle_status', self.status_cb, qos)
        self.create_subscription(
            VehicleLocalPosition, '/fmu/out/vehicle_local_position', self.pos_cb, qos)
        self.create_timer(1.0, self.report)

    def status_cb(self, msg):
        self.mode = msg.nav_state

    def pos_cb(self, msg):
        self.pos = (msg.x, msg.y, msg.z)

    def report(self):
        if self.mode is None or self.pos is None:
            self.get_logger().info('等待 PX4 数据...')
            return
        self.get_logger().info(
            f'nav_state={self.mode}  '
            f'x={self.pos[0]:.2f} y={self.pos[1]:.2f} z={self.pos[2]:.2f}')

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

if __name__ == '__main__':
    main()

运行方式

# 终端1：启动 PX4 与 MicroXRCEAgent（若尚未运行）
MicroXRCEAgent udp4 -p 8888

# 终端2：确认话题存在
ros2 topic list | grep fmu/out

# 终端3：运行订阅节点
python3 px4_status_listener.py

验证与通过条件

• 节点启动后每秒打印一行，包含 nav_state 与 x/y/z。
• 若话题尚未发布，先打印 等待 PX4 数据...，收到消息后自动切换为数值输出。
• 在 QGroundControl 或遥控器切换飞行模式时，nav_state 数值应随之变化；移动无人机时 x/y/z 应随之变化，即判定订阅链路正常。