基于Ardupilot与ROS2的Python无人机代码：解锁后自动上锁问题求助

问题根源分析 #

你的代码存在几个触发Ardupilot自动上锁机制的问题：

• 模式与解锁顺序颠倒：Ardupilot要求先切换到GUIDED模式，再执行解锁操作，反之会触发安全保护自动上锁。
• 无效的位置指令：直接将GPS经纬度赋值给局部NED坐标（FRAME_LOCAL_NED），数据完全不符合要求，Ardupilot接收不到有效控制指令，会判定为失联而自动上锁。
• 异步服务调用未确认结果：设置模式时用call_async但未等待操作完成，可能模式切换失败就执行了解锁。
• 重复的模式设置操作：arm方法内已经调用了模式设置，又额外调用set_guided_mode，导致状态混乱。

具体修复步骤 #

1. 调整模式切换与解锁顺序：先确保切换到GUIDED模式成功，再执行解锁操作。
2. 修正坐标转换逻辑：借助Mavros的global_to_local服务将GPS经纬度转换为局部NED坐标系，避免无效指令。
3. 等待异步服务调用结果：调用模式设置服务时，必须等待操作完成并确认成功，再进行下一步。
4. 移除重复的模式设置代码：清理冗余的set_guided_mode调用，避免状态冲突。
5. 确保目标指令持续有效：如果暂时没有外部GPS输入，至少发布一个无人机当前位置附近的有效目标，防止因指令无效触发自动上锁。

修复后的完整代码 #

import rclpy
from rclpy.node import Node
from geographic_msgs.msg import GeoPointStamped
from mavros_msgs.msg import PositionTarget
from mavros_msgs.srv import CommandBool, SetMode, GlobalToLocal

class FollowMe(Node):
    def __init__(self):
        super().__init__('follow_me_node')

        # Publisher for setting target position
        self.target_pub = self.create_publisher(PositionTarget, '/mavros/setpoint_raw/local', 10)
        
        # Service clients
        self.arming_client = self.create_client(CommandBool, '/mavros/cmd/arming')
        self.mode_client = self.create_client(SetMode, '/mavros/set_mode')
        self.global_to_local_client = self.create_client(GlobalToLocal, '/mavros/global_to_local')

        # Subscriber for target GPS position
        self.create_subscription(GeoPointStamped, '/target_position', self.target_callback, 10)
        
        self.target_gps = GeoPointStamped()
        self.timer = self.create_timer(0.1, self.publish_target_position)  # 10 Hz

        # Wait for all services to be available
        self.wait_for_services()

        # 先设置GUIDED模式，再解锁
        self.set_guided_mode()
        self.arm()

    def wait_for_services(self):
        while not self.arming_client.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('Arming service not available, waiting...')
        while not self.mode_client.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('Mode service not available, waiting...')
        while not self.global_to_local_client.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('Global to local service not available, waiting...')

    def arm(self):
        self.get_logger().info("Arming drone")
        arm_req = CommandBool.Request()
        arm_req.value = True
        future = self.arming_client.call_async(arm_req)
        rclpy.spin_until_future_complete(self, future)
        if future.result().success:
            self.get_logger().info('Drone armed successfully!')
        else:
            self.get_logger().error('Failed to arm drone.')

    def set_guided_mode(self):
        self.get_logger().info("Setting mode to GUIDED")
        mode_req = SetMode.Request()
        mode_req.custom_mode = 'GUIDED'
        future = self.mode_client.call_async(mode_req)
        rclpy.spin_until_future_complete(self, future)
        if future.result().mode_sent:
            self.get_logger().info('GUIDED mode set successfully!')
        else:
            self.get_logger().error('Failed to set GUIDED mode.')

    def target_callback(self, msg):
        self.target_gps = msg

    def convert_gps_to_local(self, gps_msg):
        # 使用Mavros的global_to_local服务转换GPS到局部NED坐标
        req = GlobalToLocal.Request()
        req.global_position.latitude = gps_msg.position.latitude
        req.global_position.longitude = gps_msg.position.longitude
        req.global_position.altitude = gps_msg.position.altitude
        
        future = self.global_to_local_client.call_async(req)
        rclpy.spin_until_future_complete(self, future)
        return future.result().local_position

    def publish_target_position(self):
        target = PositionTarget()
        target.coordinate_frame = PositionTarget.FRAME_LOCAL_NED
        # 设置为只控制位置，忽略速度、加速度、偏航
        target.type_mask = PositionTarget.IGNORE_VX | PositionTarget.IGNORE_VY | PositionTarget.IGNORE_VZ \
                           | PositionTarget.IGNORE_AFX | PositionTarget.IGNORE_AFY | PositionTarget.IGNORE_AFZ \
                           | PositionTarget.IGNORE_YAW | PositionTarget.IGNORE_YAW_RATE

        # 如果有有效的GPS目标，转换为局部坐标；否则使用默认位置
        if self.target_gps.position.latitude != 0.0 or self.target_gps.position.longitude != 0.0:
            local_pos = self.convert_gps_to_local(self.target_gps)
            target.position.x = local_pos.x
            target.position.y = local_pos.y
            target.position.z = -15.0  # NED坐标系中z为负表示海拔高度（需根据实际情况调整）
        else:
            # 无外部输入时，发布当前位置附近的目标，防止自动上锁
            target.position.x = 0.0
            target.position.y = 0.0
            target.position.z = -15.0

        self.target_pub.publish(target)

def main(args=None):
    rclpy.init(args=args)
    follow_me = FollowMe()
    try:
        rclpy.spin(follow_me)
    except KeyboardInterrupt:
        pass
    follow_me.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

额外注意事项 #

• 检查Ardupilot参数：调整ARMING_CHECK参数，测试环境下可临时关闭不必要的解锁检查（如GPS数量、电池电压等）。
• 验证Mavros连接：确保mavros节点正常运行，通过ros2 topic list和ros2 service list确认话题、服务通信正常。
• NED坐标系规则：Ardupilot局部NED坐标系中z轴向下为正，所以海拔高度需取负数（如15米海拔对应z=-15.0）。

内容的提问来源于stack exchange，提问作者GreenEngineer