掌握无人机自主起飞：深入解析ROS2节点实现(Ardupilot+ROS2+Gazebo+Mavros仿真)

一：ROS2与MAVROS

ROS2是一个用于机器人软件开发的开源框架，它提供了一套丰富的工具和库，使得开发者能够快速构建复杂的机器人应用程序。ROS2可以通过Mavros插件包与无人机的飞行控制系统进行通信，实现了对无人机的精确控制。
目前网上基于ROS2的Mavros教程极少，且几乎都是针对PX4固件的，这无疑增大了Arudupilot、ROS2和Mavros的学习困难。
PX4官网的ROS1 demo是使用mavros_msgs的，可是ROS2例程却使用了px4_msgs：
PX4官网ROS2
ROS2下的px4 offboard示例代码变了
Ardupilot官网的ROS2没有提供mavros的示例程序，新引入了AP_DDS：
Goal Interface - Waypoints ROS2
AP_DDS/Github
甚至有人说APM固件不支持Gazebo仿真，无法自动起飞：
机载计算机控制APM固件飞控的缺点
摸索过程中困难重重，终于顺利起飞，虽然还是有一些小bug，下面介绍一下实现思路吧。

二：代码概览

我们需要新建一个ROS2节点，名为uav_takeoff_node。这个节点的主要职责是控制无人机安全地起飞到预设的高度。代码的主体结构如下：

初始化：设置节点名称，声明参数，并创建与无人机状态、起飞、模式设置等相关的服务客户端。
起飞逻辑：通过服务请求，依次进行无人机的解锁（arming）、模式设置、以及起飞命令的发送。
状态回调：实时更新无人机的状态信息。
模式设置：将无人机设置为“GUIDED”模式，以便进行后续的控制指令发送。
起飞命令：发送起飞命令，并等待起飞成功。

三：代码详解

接下来，我们将逐行分析这段代码，从参数声明到服务调用，再到状态更新和命令发送，全面理解每一部分的功能和实现方式。
在ROS的进化版——ROS 2中，client和future成为了两个至关重要的概念，它们在节点间的服务调用中扮演着核心角色。

Client：发起服务请求的使者

在ROS 2的网络世界里，client是一个特殊的节点，它向其他节点（服务端）发送服务请求。这些请求携带了需要服务端处理的数据。一旦服务端完成了数据处理，它会将结果封装在一个响应中返回给客户端。在ROS 2中，服务请求的异步发送是通过使用create_client创建的服务客户端实现的。例如，arm_client、set_mode_client和takeoff_client都是服务客户端的实例，它们可以进行异步调用。

Future：异步操作的承诺

future对象是异步操作中的一个关键机制，它代表了将来某个时刻会得到的结果。当客户端异步发送服务请求时，会立即收到一个future对象。这个对象允许客户端在未来的某个时刻查询结果是否已经准备就绪。

异步请求与回调

在takeoff函数中，通过future.add_done_callback(self.arm_response_callback)将回调函数与异步请求的结果关联起来。当异步操作完成时，会自动调用这个回调函数来处理结果。这种方式使得节点可以在等待响应的同时继续执行其他任务，并在响应到达时及时作出反应。

等待服务响应

在arm_response_callback和其他相关函数中，使用rclpy.spin_until_future_complete(self, future)方法来等待异步调用的结果。这个方法会阻塞当前线程，直到关联的future对象有结果返回，确保了服务请求的响应能够被及时处理。

服务调用的异步特性

在set_guided_mode函数中，通过调用self.set_mode_client.call_async(set_mode_request)异步发送服务请求，并使用rclpy.spin_until_future_complete等待服务响应。这种异步特性大大提高了节点的效率和响应能力。

起飞命令的发送

在send_takeoff_cmd函数中，通过self.takeoff_client.call_async(takeoff_request)异步发送起飞命令，并同样使用rclpy.spin_until_future_complete来等待结果。这确保了起飞命令的发送和处理不会因为等待响应而阻塞其他任务的执行。

四：程序实现

demo 1：上升至10米，再下降至2米处悬停

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import PoseStamped, Twist
from mavros_msgs.srv import SetMode, CommandBool, CommandTOL
from mavros_msgs.msg import State
from std_msgs.msg import Header
from geometry_msgs.msg import Point, Quaternion
import time

class UAVTakeoffNode(Node):
    def __init__(self):
        super().__init__('uav_takeoff_node')
        self.declare_parameter('takeoff_height', 2.0)
        self.takeoff_height = self.get_parameter('takeoff_height').get_parameter_value().double_value
        # 创建订阅者
        self.state_sub = self.create_subscription(State, '/mavros/state', self.state_callback, 10)
        self.current_state = State()
        self.arm_client = self.create_client(CommandBool, '/mavros/cmd/arming')
        self.set_mode_client = self.create_client(SetMode, '/mavros/set_mode')
        self.takeoff_client = self.create_client(CommandTOL, '/mavros/cmd/takeoff')
        while not self.arm_client.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('/mavros/cmd/arming service not available, waiting again...')
        
        self.timer = self.create_timer(1, self.takeoff)

    def takeoff(self):
        self.timer.cancel()  # Ensure this method is called only once
        
        self.get_logger().info('Requesting to arm the UAV')
        arm_request = CommandBool.Request()
        arm_request.value = True
        
        future = self.arm_client.call_async(arm_request)
        future.add_done_callback(self.arm_response_callback)

    def arm_response_callback(self, future):
        try:
            response = future.result()
            if self.current_state.armed:
                self.get_logger().info('UAV arming successful')
                self.set_guided_mode()
                self.send_takeoff_cmd()
                time.sleep(10)
                takeoff_pose = PoseStamped()
                takeoff_pose.header = Header()
                takeoff_pose.header.stamp = self.get_clock().now().to_msg()
                takeoff_pose.header.frame_id = "map"
                
                takeoff_pose.pose.position = Point(x=0.0, y=0.0, z=self.takeoff_height)
                takeoff_pose.pose.orientation = Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)
                takeoff_vel = Twist()
                takeoff_vel.linear.x = 0.0
                takeoff_vel.linear.y = 0.0
                takeoff_vel.linear.z = 0.0
                takeoff_vel.angular.x = 0.0
                takeoff_vel.angular.y =<