MAVROS实战指南：用Python与ROS2构建无人机自主飞行系统

第一次看到无人机在代码控制下完成精准悬停时，那种成就感至今难忘。作为连接ROS与PX4飞控的桥梁，MAVROS让开发者能够用熟悉的ROS工具链操控无人机，但文档里零散的话题说明常让人无从下手。本文将用可运行的代码示例，带你跨越从理论到实践的鸿沟。

1. 环境配置与基础检查

在开始编码前，确保你的开发环境满足以下条件：

• PX4固件 ：v1.13或更新版本
• ROS版本 ：
  • ROS Noetic（Ubuntu 20.04）
  • ROS2 Humble（Ubuntu 22.04）
• MAVROS安装 ：

  # 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(f"Connected: {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(
            f"GPS: 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(f"Local Position: X={x:.2f}, Y={y:.2f}, Z={z:.2f}")

if __name__ == '__main__':
    rclpy.init()
    node = SensorFusion()
    rclpy.spin(node)

2.2 系统状态监控表

状态类型	话题路径	关键字段	典型用途
飞控状态	/mavros/state	connected, armed, mode	模式切换判断
电池状态	/mavros/battery	voltage, remaining	低电量预警
RC输入	/mavros/rc/in	channels	手动接管检测
系统状态	/mavros/statustext	text, 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_size=10)
    
    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_sec=1.0) and 
                  self.set_mode_client.wait_for_service(timeout_sec=1.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的航点服务，我们可以构建完整的任务系统：

1. 航点数据结构 ：

from mavros_msgs.msg import Waypoint

def create_waypoint(x, y, z, cmd=16, frame=1):
    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

2. 航点任务上传 ：

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_multiplot）：

  rosrun rqt_multiplot rqt_multiplot
  

  配置建议监控曲线：

  • 本地位置XYZ
  • 速度设定值与实际值
  • 电池电压变化
  • CPU负载与通信延迟

在Gazebo仿真中测试完整流程时，发现姿态控制需要特别注意四元数归一化问题。某次测试因未做归一化导致无人机异常旋转，后来在发布姿态指令前增加检查后问题解决。