别再对着文档发愁了！手把手教你用MAVROS控制PX4无人机（附Python/ROS2代码）

MAVROS实战指南用Python与ROS2构建无人机自主飞行系统第一次看到无人机在代码控制下完成精准悬停时那种成就感至今难忘。作为连接ROS与PX4飞控的桥梁MAVROS让开发者能够用熟悉的ROS工具链操控无人机但文档里零散的话题说明常让人无从下手。本文将用可运行的代码示例带你跨越从理论到实践的鸿沟。1. 环境配置与基础检查在开始编码前确保你的开发环境满足以下条件PX4固件v1.13或更新版本ROS版本ROS NoeticUbuntu 20.04ROS2 HumbleUbuntu 22.04MAVROS安装# ROS1 sudo apt install ros-noetic-mavros ros-noetic-mavros-extras # ROS2 sudo apt install ros-humble-mavros ros-humble-mavros-extras连接验证确保飞控与地面站通信正常import rospy from mavros_msgs.msg import State def state_cb(msg): print(fConnected: {msg.connected}, Armed: {msg.armed}, Mode: {msg.mode}) rospy.init_node(check_connection) state_sub rospy.Subscriber(/mavros/state, State, state_cb) rospy.spin()注意如果显示未连接检查USB/UART连接或MAVROS启动参数中的设备路径2. 关键数据订阅与解析无人机状态监控是自主控制的基础这些核心话题需要特别关注2.1 姿态与位置数据融合# ROS2示例 import rclpy from rclpy.node import Node from sensor_msgs.msg import NavSatFix, Imu from geometry_msgs.msg import PoseStamped class SensorFusion(Node): def __init__(self): super().__init__(sensor_fusion) # 创建订阅 self.gps_sub self.create_subscription( NavSatFix, /mavros/global_position/global, self.gps_cb, 10) self.imu_sub self.create_subscription( Imu, /mavros/imu/data, self.imu_cb, 10) self.local_pos_sub self.create_subscription( PoseStamped, /mavros/local_position/pose, self.local_pos_cb, 10) def gps_cb(self, msg): self.get_logger().info( fGPS: Lat{msg.latitude:.6f}, Lon{msg.longitude:.6f}, Alt{msg.altitude:.1f}) def imu_cb(self, msg): # 四元数转欧拉角略 pass def local_pos_cb(self, msg): x, y, z msg.pose.position.x, msg.pose.position.y, msg.pose.position.z self.get_logger().info(fLocal Position: X{x:.2f}, Y{y:.2f}, Z{z:.2f}) if __name__ __main__: rclpy.init() node SensorFusion() rclpy.spin(node)2.2 系统状态监控表状态类型话题路径关键字段典型用途飞控状态/mavros/stateconnected, armed, mode模式切换判断电池状态/mavros/batteryvoltage, remaining低电量预警RC输入/mavros/rc/inchannels手动接管检测系统状态/mavros/statustexttext, severity故障诊断3. 控制指令发布实战3.1 安全进入Offboard模式# ROS1完整示例 #!/usr/bin/env python import rospy from geometry_msgs.msg import PoseStamped from mavros_msgs.msg import State from mavros_msgs.srv import CommandBool, SetMode current_state State() def state_cb(state): global current_state current_state state if __name__ __main__: rospy.init_node(offboard_ctrl) state_sub rospy.Subscriber(/mavros/state, State, state_cb) local_pos_pub rospy.Publisher(/mavros/setpoint_position/local, PoseStamped, queue_size10) rospy.wait_for_service(/mavros/cmd/arming) arming_client rospy.ServiceProxy(/mavros/cmd/arming, CommandBool) rospy.wait_for_service(/mavros/set_mode) set_mode_client rospy.ServiceProxy(/mavros/set_mode, SetMode) rate rospy.Rate(20) # 必须先发送设定值否则PX4会拒绝Offboard模式 pose PoseStamped() pose.pose.position.x 0 pose.pose.position.y 0 pose.pose.position.z 2 for i in range(100): local_pos_pub.publish(pose) rate.sleep() last_request rospy.Time.now() while not rospy.is_shutdown(): now rospy.Time.now() if current_state.mode ! OFFBOARD and (now - last_request rospy.Duration(5.)): if set_mode_client(0, OFFBOARD).mode_sent: rospy.loginfo(Offboard enabled) last_request now else: if not current_state.armed and (now - last_request rospy.Duration(5.)): if arming_client(True).success: rospy.loginfo(Vehicle armed) last_request now local_pos_pub.publish(pose) rate.sleep()关键安全措施在真实环境中测试时务必配置RC遥控器作为Fallback并设置电子围栏3.2 多控制模式实现对比位置控制def send_position_target(x, y, z): pose PoseStamped() pose.header.stamp rospy.Time.now() pose.pose.position.x x pose.pose.position.y y pose.pose.position.z z local_pos_pub.publish(pose)速度控制from geometry_msgs.msg import TwistStamped def send_velocity_target(vx, vy, vz): twist TwistStamped() twist.header.stamp rospy.Time.now() twist.twist.linear.x vx twist.twist.linear.y vy twist.twist.linear.z vz vel_pub.publish(twist)姿态控制import math def send_attitude_target(roll, pitch, yaw, thrust): q quaternion_from_euler(roll, pitch, yaw) pose PoseStamped() pose.pose.orientation.x q[0] pose.pose.orientation.y q[1] pose.pose.orientation.z q[2] pose.pose.orientation.w q[3] # 需要配合混控器话题发布推力 att_pub.publish(pose)4. 典型应用场景实现4.1 自动起飞与定点悬停# ROS2实现类 class AutoTakeoff(Node): def __init__(self): super().__init__(auto_takeoff) self.target_alt 5.0 # 目标高度 self.current_pose None self.state None self.pose_sub self.create_subscription( PoseStamped, /mavros/local_position/pose, self.pose_cb, 10) self.state_sub self.create_subscription( State, /mavros/state, self.state_cb, 10) self.pos_pub self.create_publisher( PoseStamped, /mavros/setpoint_position/local, 10) self.arming_client self.create_client( CommandBool, /mavros/cmd/arming) self.set_mode_client self.create_client( SetMode, /mavros/set_mode) async def arm_and_takeoff(self): # 等待服务可用 while not (self.arming_client.wait_for_service(timeout_sec1.0) and self.set_mode_client.wait_for_service(timeout_sec1.0)): self.get_logger().info(等待服务...) # 发送初始位置 pose PoseStamped() pose.pose.position.z self.target_alt for _ in range(100): self.pos_pub.publish(pose) await asyncio.sleep(0.05) # 切换模式 set_mode_req SetMode.Request() set_mode_req.custom_mode OFFBOARD await self.set_mode_client.call_async(set_mode_req) # 解锁 arm_req CommandBool.Request() arm_req.value True await self.arming_client.call_async(arm_req) # 控制循环 while rclpy.ok(): if abs(self.current_pose.pose.position.z - self.target_alt) 0.1: break self.pos_pub.publish(pose) await asyncio.sleep(0.05) self.get_logger().info(到达目标高度)4.2 航点任务规划结合MAVROS的航点服务我们可以构建完整的任务系统航点数据结构from mavros_msgs.msg import Waypoint def create_waypoint(x, y, z, cmd16, frame1): wp Waypoint() wp.frame frame # MAV_FRAME_LOCAL_NED wp.command cmd # MAV_CMD_NAV_WAYPOINT wp.x_lat x wp.y_long y wp.z_alt z wp.autocontinue True return wp航点任务上传from mavros_msgs.srv import WaypointPush async def upload_mission(self, waypoints): wp_push self.create_client(WaypointPush, /mavros/mission/push) req WaypointPush.Request() req.waypoints waypoints future wp_push.call_async(req) try: response await future if response.success: self.get_logger().info(f上传成功共{response.wp_transfered}个航点) else: self.get_logger().error(上传失败) except Exception as e: self.get_logger().error(f服务调用失败: {str(e)})5. 调试技巧与异常处理实际部署中常见问题解决方案GPS信号丢失def handle_gps_loss(): # 切换至高度保持模式 set_mode_client(0, ALTCTL) # 缓慢下降 send_velocity_target(0, 0, -0.5)通信中断应急处理def check_heartbeat(): last_heartbeat rospy.Time.now() while not rospy.is_shutdown(): if (rospy.Time.now() - last_heartbeat).to_sec() 5.0: rospy.logerr(心跳丢失触发应急措施) # 执行预设应急方案 break rospy.sleep(1)状态监控看板使用rqt_multiplotrosrun rqt_multiplot rqt_multiplot配置建议监控曲线本地位置XYZ速度设定值与实际值电池电压变化CPU负载与通信延迟在Gazebo仿真中测试完整流程时发现姿态控制需要特别注意四元数归一化问题。某次测试因未做归一化导致无人机异常旋转后来在发布姿态指令前增加检查后问题解决。