21.1 MAVROS2 概述与安装21.2 MAVROS2 话题与服务映射核心话题映射表核心服务映射表21.3 通过 MAVROS2 获取飞控状态#!/usr/bin/env python3drone_state_monitor.py - 无人机状态监控节点import rclpyfrom rclpy.node import Nodefrom sensor_msgs.msg import Imu, NavSatFix, BatteryStatefrom geometry_msgs.msg import PoseStamped, TwistStampedfrom mavros_msgs.msg import State, VFR_HUDimport mathclass DroneStateMonitor(Node):def __init__(self):super().__init__(drone_state_monitor)self.state State()self.pose PoseStamped()self.velocity TwistStamped()self.gps NavSatFix()self.battery BatteryState()self.hud VFR_HUD()self.create_subscription(State, /mavros/state,lambda m: setattr(self, state, m), 10)self.create_subscription(PoseStamped, /mavros/local_position/pose,lambda m: setattr(self, pose, m), 10)self.create_subscription(TwistStamped, /mavros/local_position/velocity_local,lambda m: setattr(self, velocity, m), 10)self.create_subscription(NavSatFix, /mavros/global_position/global,lambda m: setattr(self, gps, m), 10)self.create_subscription(BatteryState, /mavros/battery,lambda m: setattr(self, battery, m), 10)self.create_subscription(VFR_HUD, /mavros/vfr_hud,lambda m: setattr(self, hud, m), 10)self.create_timer(1.0, self.print_summary)self.get_logger().info(飞控状态监控节点已启动)def quat_to_euler(self, x, y, z, w):roll math.degrees(math.atan2(2*(w*xy*z), 1-2*(x*xy*y)))pitch math.degrees(math.asin(max(-1, min(1, 2*(w*y-z*x)))))yaw math.degrees(math.atan2(2*(w*zx*y), 1-2*(y*yz*z)))return roll, pitch, yawdef print_summary(self):q self.pose.pose.orientationr, p, y self.quat_to_euler(q.x, q.y, q.z, q.w)pos self.pose.pose.positionvel self.velocity.twist.linearself.get_logger().info(f\n--- ZP_H743 状态 ---f\n 连接:{self.state.connected} 模式:{self.state.mode} 解锁:{self.state.armed}f\n 位置: ({pos.x:.2f}, {pos.y:.2f}, {pos.z:.2f}) mf\n 姿态: ({r:.1f}, {p:.1f}, {y:.1f}) degf\n 速度: ({vel.x:.2f}, {vel.y:.2f}, {vel.z:.2f}) m/sf\n GPS: {self.gps.latitude:.7f}, {self.gps.longitude:.7f}, {self.gps.altitude:.1f}mf\n 电池: {self.battery.voltage:.1f}V {self.battery.percentage*100:.0f}%)def main(argsNone):rclpy.init(argsargs)node DroneStateMonitor()try:rclpy.spin(node)except KeyboardInterrupt:passfinally:node.destroy_node()rclpy.shutdown()if __name__ __main__:main()21.4 通过 MAVROS2 发送控制指令#!/usr/bin/env python3drone_commander.py - 无人机指令控制器import rclpyfrom rclpy.node import Nodefrom mavros_msgs.srv import CommandBool, CommandTOL, SetMode, WaypointPushfrom mavros_msgs.msg import State, Waypointfrom geometry_msgs.msg import PoseStampedimport timeclass DroneCommander(Node):def __init__(self):super().__init__(drone_commander)self.current_state State()self.create_subscription(State, /mavros/state,lambda m: setattr(self, current_state, m), 10)self.setpoint_pub self.create_publisher(PoseStamped, /mavros/setpoint_position/local, 10)self.arm_client self.create_client(CommandBool, /mavros/cmd/arming)self.takeoff_client self.create_client(CommandTOL, /mavros/cmd/takeoff)self.land_client self.create_client(CommandTOL, /mavros/cmd/land)self.mode_client self.create_client(SetMode, /mavros/set_mode)self.mission_client self.create_client(WaypointPush, /mavros/mission/push)self.get_logger().info(指令控制器已启动)def wait_for_connection(self, timeout10.0):start time.time()while not self.current_state.connected:if time.time() - start timeout:return Falserclpy.spin_once(self, timeout_sec0.5)return Truedef set_mode(self, mode):req SetMode.Request()req.custom_mode modefuture self.mode_client.call_async(req)rclpy.spin_until_future_complete(self, future)ok future.result() and future.result().mode_sentself.get_logger().info(f模式切换 {成功 if ok else 失败}: {mode})return okdef arm(self):req CommandBool.Request()req.value Truefuture self.arm_client.call_async(req)rclpy.spin_until_future_complete(self, future)ok future.result() and future.result().successself.get_logger().info(f{解锁 if ok else 解锁失败})return okdef takeoff(self, altitude):req CommandTOL.Request()req.altitude altitudefuture self.takeoff_client.call_async(req)rclpy.spin_until_future_complete(self, future)ok future.result() and future.result().successself.get_logger().info(f起飞 {成功 if ok else 失败} - {altitude}m)return okdef land(self):req CommandTOL.Request()future self.land_client.call_async(req)rclpy.spin_until_future_complete(self, future)ok future.result() and future.result().successself.get_logger().info(f降落指令 {已发送 if ok else 失败})return okdef upload_mission(self, waypoints):上传航点任务, waypoints: [{lat:..,lon:..,alt:..,command:16}, ...]req WaypointPush.Request()for i, wp in enumerate(waypoints):w Waypoint()w.frame wp.get(frame, 3) # GLOBAL_REL_ALTw.command wp.get(command, 16)w.is_current (i 0)w.autocontinue Truew.param1 wp.get(param1, 0.0)w.param4 wp.get(param4, 0.0)w.x_lat wp[lat]w.y_long wp[lon]w.z_alt wp[alt]req.waypoints.append(w)future self.mission_client.call_async(req)rclpy.spin_until_future_complete(self, future)result future.result()if result and result.success:self.get_logger().info(f任务上传成功: {result.wp_transferred} 个航点)return Trueself.get_logger().error(任务上传失败)return False21.5 PX4 原生 DDS 方案——MicroXRCE-DDS Agent 桥接PX4 v1.14 提供基于 DDS 的原生通信方案通过 MicroXRCE-DDS Agent 将飞控内部 uORB 消息直接映射到 ROS2 DDS 域绕过 MAVLink 层。机载计算机: ROS2 节点 --DDS-- MicroXRCE-DDS Agent|UDP/Serial|飞控 STM32H743: PX4 模块 --uORB-- XRCE Client# 安装 MicroXRCE-DDS Agentgit clone https://github.com/eProsima/Micro-XRCE-DDS-Agent.gitcd Micro-XRCE-DDS-Agent mkdir build cd buildcmake .. make sudo make install sudo ldconfig# 启动 AgentUDP 模式MicroXRCEAgent udp4 -p 8888# 串口模式MicroXRCEAgent serial --serial /dev/ttyUSB0 --baud 921600### 21.6 MicroXRCE-DDS 配置与 uORB 话题直连uORB 话题与 ROS2 映射# 安装 PX4 消息包cd ~/ros2_ws/srcgit clone https://github.com/PX4/px4_msgs.gitgit clone https://github.com/PX4/px4_ros_com.gitcd ~/ros2_ws colcon build --packages-select px4_msgs px4_ros_com#!/usr/bin/env python3xrce_drone_monitor.py - 通过 XRCE-DDS 获取飞控数据import rclpyfrom rclpy.node import Nodefrom px4_msgs.msg import VehicleAttitude, VehicleLocalPosition, VehicleStatusclass XRCEDroneMonitor(Node):def __init__(self):super().__init__(xrce_drone_monitor)self.attitude Noneself.position Noneself.create_subscription(VehicleAttitude, /fmu/out/vehicle_attitude,lambda m: setattr(self, attitude, m), 10)self.create_subscription(VehicleLocalPosition, /fmu/out/vehicle_local_position,lambda m: setattr(self, position, m), 10)self.create_timer(1.0, self.print_status)self.get_logger().info(XRCE-DDS 监控节点已启动)def print_status(self):if self.attitude and self.position:q self.attitude.qself.get_logger().info(f\n--- XRCE-DDS 状态 ---f\n 四元数: [{q[0]:.4f}, {q[1]:.4f}, {q[2]:.4f}, {q[3]:.4f}]f\n 位置: ({self.position.x:.2f}, {self.position.y:.2f}, {self.position.z:.2f})f\n 高度有效: {self.position.z_valid})def main(argsNone):rclpy.init(argsargs)rclpy.spin(XRCEDroneMonitor())rclpy.shutdown()if __name__ __main__:main()21.7 MAVROS2 vs MicroXRCE-DDS 方案对比选型建议生产稳定环境选 MAVROS2追求最低延迟且使用 PX4 v1.14 选 XRCE-DDS需要两者优势可混合方案MAVROS2 远程 XRCE 本地高频数据。
21.1 MAVROS2 概述与安装21.2 MAVROS2 话题与服务映射核心话题映射表核心服务映射表21.3 通过 MAVROS2 获取飞控状态#!/usr/bin/env python3drone_state_monitor.py - 无人机状态监控节点import rclpyfrom rclpy.node import Nodefrom sensor_msgs.msg import Imu, NavSatFix, BatteryStatefrom geometry_msgs.msg import PoseStamped, TwistStampedfrom mavros_msgs.msg import State, VFR_HUDimport mathclass DroneStateMonitor(Node):def __init__(self):super().__init__(drone_state_monitor)self.state State()self.pose PoseStamped()self.velocity TwistStamped()self.gps NavSatFix()self.battery BatteryState()self.hud VFR_HUD()self.create_subscription(State, /mavros/state,lambda m: setattr(self, state, m), 10)self.create_subscription(PoseStamped, /mavros/local_position/pose,lambda m: setattr(self, pose, m), 10)self.create_subscription(TwistStamped, /mavros/local_position/velocity_local,lambda m: setattr(self, velocity, m), 10)self.create_subscription(NavSatFix, /mavros/global_position/global,lambda m: setattr(self, gps, m), 10)self.create_subscription(BatteryState, /mavros/battery,lambda m: setattr(self, battery, m), 10)self.create_subscription(VFR_HUD, /mavros/vfr_hud,lambda m: setattr(self, hud, m), 10)self.create_timer(1.0, self.print_summary)self.get_logger().info(飞控状态监控节点已启动)def quat_to_euler(self, x, y, z, w):roll math.degrees(math.atan2(2*(w*xy*z), 1-2*(x*xy*y)))pitch math.degrees(math.asin(max(-1, min(1, 2*(w*y-z*x)))))yaw math.degrees(math.atan2(2*(w*zx*y), 1-2*(y*yz*z)))return roll, pitch, yawdef print_summary(self):q self.pose.pose.orientationr, p, y self.quat_to_euler(q.x, q.y, q.z, q.w)pos self.pose.pose.positionvel self.velocity.twist.linearself.get_logger().info(f\n--- ZP_H743 状态 ---f\n 连接:{self.state.connected} 模式:{self.state.mode} 解锁:{self.state.armed}f\n 位置: ({pos.x:.2f}, {pos.y:.2f}, {pos.z:.2f}) mf\n 姿态: ({r:.1f}, {p:.1f}, {y:.1f}) degf\n 速度: ({vel.x:.2f}, {vel.y:.2f}, {vel.z:.2f}) m/sf\n GPS: {self.gps.latitude:.7f}, {self.gps.longitude:.7f}, {self.gps.altitude:.1f}mf\n 电池: {self.battery.voltage:.1f}V {self.battery.percentage*100:.0f}%)def main(argsNone):rclpy.init(argsargs)node DroneStateMonitor()try:rclpy.spin(node)except KeyboardInterrupt:passfinally:node.destroy_node()rclpy.shutdown()if __name__ __main__:main()21.4 通过 MAVROS2 发送控制指令#!/usr/bin/env python3drone_commander.py - 无人机指令控制器import rclpyfrom rclpy.node import Nodefrom mavros_msgs.srv import CommandBool, CommandTOL, SetMode, WaypointPushfrom mavros_msgs.msg import State, Waypointfrom geometry_msgs.msg import PoseStampedimport timeclass DroneCommander(Node):def __init__(self):super().__init__(drone_commander)self.current_state State()self.create_subscription(State, /mavros/state,lambda m: setattr(self, current_state, m), 10)self.setpoint_pub self.create_publisher(PoseStamped, /mavros/setpoint_position/local, 10)self.arm_client self.create_client(CommandBool, /mavros/cmd/arming)self.takeoff_client self.create_client(CommandTOL, /mavros/cmd/takeoff)self.land_client self.create_client(CommandTOL, /mavros/cmd/land)self.mode_client self.create_client(SetMode, /mavros/set_mode)self.mission_client self.create_client(WaypointPush, /mavros/mission/push)self.get_logger().info(指令控制器已启动)def wait_for_connection(self, timeout10.0):start time.time()while not self.current_state.connected:if time.time() - start timeout:return Falserclpy.spin_once(self, timeout_sec0.5)return Truedef set_mode(self, mode):req SetMode.Request()req.custom_mode modefuture self.mode_client.call_async(req)rclpy.spin_until_future_complete(self, future)ok future.result() and future.result().mode_sentself.get_logger().info(f模式切换 {成功 if ok else 失败}: {mode})return okdef arm(self):req CommandBool.Request()req.value Truefuture self.arm_client.call_async(req)rclpy.spin_until_future_complete(self, future)ok future.result() and future.result().successself.get_logger().info(f{解锁 if ok else 解锁失败})return okdef takeoff(self, altitude):req CommandTOL.Request()req.altitude altitudefuture self.takeoff_client.call_async(req)rclpy.spin_until_future_complete(self, future)ok future.result() and future.result().successself.get_logger().info(f起飞 {成功 if ok else 失败} - {altitude}m)return okdef land(self):req CommandTOL.Request()future self.land_client.call_async(req)rclpy.spin_until_future_complete(self, future)ok future.result() and future.result().successself.get_logger().info(f降落指令 {已发送 if ok else 失败})return okdef upload_mission(self, waypoints):上传航点任务, waypoints: [{lat:..,lon:..,alt:..,command:16}, ...]req WaypointPush.Request()for i, wp in enumerate(waypoints):w Waypoint()w.frame wp.get(frame, 3) # GLOBAL_REL_ALTw.command wp.get(command, 16)w.is_current (i 0)w.autocontinue Truew.param1 wp.get(param1, 0.0)w.param4 wp.get(param4, 0.0)w.x_lat wp[lat]w.y_long wp[lon]w.z_alt wp[alt]req.waypoints.append(w)future self.mission_client.call_async(req)rclpy.spin_until_future_complete(self, future)result future.result()if result and result.success:self.get_logger().info(f任务上传成功: {result.wp_transferred} 个航点)return Trueself.get_logger().error(任务上传失败)return False21.5 PX4 原生 DDS 方案——MicroXRCE-DDS Agent 桥接PX4 v1.14 提供基于 DDS 的原生通信方案通过 MicroXRCE-DDS Agent 将飞控内部 uORB 消息直接映射到 ROS2 DDS 域绕过 MAVLink 层。机载计算机: ROS2 节点 --DDS-- MicroXRCE-DDS Agent|UDP/Serial|飞控 STM32H743: PX4 模块 --uORB-- XRCE Client# 安装 MicroXRCE-DDS Agentgit clone https://github.com/eProsima/Micro-XRCE-DDS-Agent.gitcd Micro-XRCE-DDS-Agent mkdir build cd buildcmake .. make sudo make install sudo ldconfig# 启动 AgentUDP 模式MicroXRCEAgent udp4 -p 8888# 串口模式MicroXRCEAgent serial --serial /dev/ttyUSB0 --baud 921600### 21.6 MicroXRCE-DDS 配置与 uORB 话题直连uORB 话题与 ROS2 映射# 安装 PX4 消息包cd ~/ros2_ws/srcgit clone https://github.com/PX4/px4_msgs.gitgit clone https://github.com/PX4/px4_ros_com.gitcd ~/ros2_ws colcon build --packages-select px4_msgs px4_ros_com#!/usr/bin/env python3xrce_drone_monitor.py - 通过 XRCE-DDS 获取飞控数据import rclpyfrom rclpy.node import Nodefrom px4_msgs.msg import VehicleAttitude, VehicleLocalPosition, VehicleStatusclass XRCEDroneMonitor(Node):def __init__(self):super().__init__(xrce_drone_monitor)self.attitude Noneself.position Noneself.create_subscription(VehicleAttitude, /fmu/out/vehicle_attitude,lambda m: setattr(self, attitude, m), 10)self.create_subscription(VehicleLocalPosition, /fmu/out/vehicle_local_position,lambda m: setattr(self, position, m), 10)self.create_timer(1.0, self.print_status)self.get_logger().info(XRCE-DDS 监控节点已启动)def print_status(self):if self.attitude and self.position:q self.attitude.qself.get_logger().info(f\n--- XRCE-DDS 状态 ---f\n 四元数: [{q[0]:.4f}, {q[1]:.4f}, {q[2]:.4f}, {q[3]:.4f}]f\n 位置: ({self.position.x:.2f}, {self.position.y:.2f}, {self.position.z:.2f})f\n 高度有效: {self.position.z_valid})def main(argsNone):rclpy.init(argsargs)rclpy.spin(XRCEDroneMonitor())rclpy.shutdown()if __name__ __main__:main()21.7 MAVROS2 vs MicroXRCE-DDS 方案对比选型建议生产稳定环境选 MAVROS2追求最低延迟且使用 PX4 v1.14 选 XRCE-DDS需要两者优势可混合方案MAVROS2 远程 XRCE 本地高频数据。
平方复杂度无法被常规优化消除｜TaoToken 统一 Key 通道实测)
