掌握无人机自主起飞：深入解析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 = 0.0
takeoff_vel.angular.z = 0.0
self.pose_pub = self.create_publisher(PoseStamped, '/mavros/setpoint_position/local', 10)
self.vel_pub = self.create_publisher(Twist, 'mavros/setpoint_velocity/cmd_vel_unstamped', 10)
self.get_logger().info('Publishing takeoff position')
self.pose_pub.publish(takeoff_pose)
self.get_logger().info('Publishing takeoff velocity')
self.vel_pub.publish(takeoff_vel)
else:
self.get_logger().error('UAV arming failed.')
self.get_logger().error('Please run this node again.')
except Exception as e:
self.get_logger().error('Service call failed %r' % (e,))
def state_callback(self, msg):
self.current_state = msg
def set_guided_mode(self):
set_mode_request = SetMode.Request()
set_mode_request.custom_mode = "GUIDED"
future = self.set_mode_client.call_async(set_mode_request)
rclpy.spin_until_future_complete(self, future)
if future.result().mode_sent:
self.get_logger().info('Guided mode set successfully')
else:
self.get_logger().info('Failed to set Guided mode')
def send_takeoff_cmd(self):
takeoff_request = CommandTOL.Request()
takeoff_request.altitude = 10.0
takeoff_request.latitude = 0.0
takeoff_request.longitude = 0.0
takeoff_request.min_pitch = 0.0
takeoff_request.yaw = 0.0
future = self.takeoff_client.call_async(takeoff_request)
rclpy.spin_until_future_complete(self, future)
if future.result().success:
self.get_logger().info('Takeoff cmd send successfully')
else:
self.get_logger().info('Failed to send Takeoff cmd')
def main(args=None):
rclpy.init(args=args)
node = UAVTakeoffNode()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
demo 2：上升至5米，并定高转圈飞行
首先我们需要推导无人机进行匀速圆周运动时的实时位置、速度大小方向。假设无人机到达5米高度时位于坐标原点O，接下来，它会到达 ( r , 0 ) (r,0) (r,0)点，并从该点出发开始圆周运动。
经过任意时间 t t t以后，无人机的二维坐标发生了更新：
{ x circle = r cos ω t y circle = r sin ω t (1) \left\{ \begin{array}{l} x_{\text{circle}} = r \cos \omega t\\ y_{\text{circle}} = r \sin \omega t\\ \end{array} \right. \quad \text{(1)} {xcircle=rcosωtycircle=rsinωt(1)
初始速度大小为 ω r \omega r ωr，我们将其写成复数形式：
v 0 = 0 + i ω r (2) v_0 = 0 + \text{i} \omega r \quad \text{(2)} v0=0+iωr(2)
t t t时刻的速度矢量相当于对初始速度矢量进行了二维旋转：
v = e i ω t ⋅ v 0 = i ω r cos ω t − ω r sin ω t (3) v = \text{e}^{\text{i} \omega t} \cdot v_0 = \text{i} \omega r \cos \omega t - \omega r \sin \omega t \quad \text{(3)} v=eiωt⋅v0=iωrcosωt−ωrsinωt(3)
{ v x = − ω r sin ω t = − ω ⋅ y c i r c l e v y = ω r cos ω t = ω ⋅ x c i r c l e (4) \left\{ \begin{array}{l} v_x=-\omega r\sin \omega t=-\omega \cdot y_{circle}\\ v_y=\omega r\cos \omega t=\omega \cdot x_{circle}\\ \end{array} \right. \quad \text{(4)} {vx=−ωrsinωt=−ω⋅ycirclevy=ωrcosωt=ω⋅xcircle(4)
根据 ( 1 ) ( 4 ) (1)(4) (1)(4)两式可以写出匀速圆周运动的核心部分
circle_x = radius * math.cos(angular_velocity * current_time.nanoseconds * 1e-9)
circle_y = radius * math.sin(angular_velocity * current_time.nanoseconds * 1e-9)
takeoff_vel.linear.x = -angular_velocity * circle_y
takeoff_vel.linear.y = angular_velocity * circle_x
在第一个程序基础上修改即可得到转圈飞行的程序
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
import math
class UAVTakeoffNode(Node):
def __init__(self):
super().__init__('uav_takeoff_node')
self.declare_parameter('takeoff_height', 5.0)
self.takeoff_height = self.get_parameter('takeoff_height').get_parameter_value().double_value
self.current_state = State()
self.state_sub = self.create_subscription(State, '/mavros/state', self.state_callback, 10)
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')
self.timer = self.create_timer(1.0, self.takeoff)
def state_callback(self, msg):
self.current_state = msg
def takeoff(self):
self.timer.cancel()
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)
self.start_circular_motion() # Start circular motion after takeoff
else:
self.get_logger().error('UAV arming failed.')
self.get_logger().error('Please run this node again.')
except Exception as e:
self.get_logger().error('Service call failed %r' % (e,))
def set_guided_mode(self):
set_mode_request = SetMode.Request()
set_mode_request.custom_mode = "GUIDED"
future = self.set_mode_client.call_async(set_mode_request)
rclpy.spin_until_future_complete(self, future)
if future.result().mode_sent:
self.get_logger().info('Guided mode set successfully')
else:
self.get_logger().info('Failed to set Guided mode')
def send_takeoff_cmd(self):
takeoff_request = CommandTOL.Request()
takeoff_request.altitude = self.takeoff_height
future = self.takeoff_client.call_async(takeoff_request)
rclpy.spin_until_future_complete(self, future)
if future.result().success:
self.get_logger().info('Takeoff cmd send successfully')
else:
self.get_logger().info('Failed to send Takeoff cmd')
def start_circular_motion(self):
self.get_logger().info('Starting circular motion')
radius = 1.0 # 半径为1.0米
linear_velocity = 1.0 # 线速度为1.0米/秒
angular_velocity = linear_velocity / radius # 计算角速度
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 = 0.0
takeoff_vel.angular.z = 0.0
self.pose_pub = self.create_publisher(PoseStamped, '/mavros/setpoint_position/local', 10)
self.vel_pub = self.create_publisher(Twist, '/mavros/setpoint_velocity/cmd_vel_unstamped', 10)
start_time = self.get_clock().now()
while rclpy.ok():
current_time = self.get_clock().now() - start_time
circle_x = radius * math.cos(angular_velocity * current_time.nanoseconds * 1e-9)
circle_y = radius * math.sin(angular_velocity * current_time.nanoseconds * 1e-9)
z = self.takeoff_height
takeoff_pose.header.stamp = self.get_clock().now().to_msg()
takeoff_pose.header.frame_id = 'map'
takeoff_pose.pose.position = Point(x=circle_x, y=circle_y, z=z)
takeoff_pose.pose.orientation = Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)
self.pose_pub.publish(takeoff_pose)
takeoff_vel.linear.x = -angular_velocity * circle_y
takeoff_vel.linear.y = angular_velocity * circle_x
takeoff_vel.linear.z = 0.0
takeoff_vel.angular.x = 0.0
takeoff_vel.angular.y = 0.0
takeoff_vel.angular.z = angular_velocity
self.vel_pub.publish(takeoff_vel)
rclpy.spin_once(self, timeout_sec=0.1) # 等待下一个循环
def main(args=None):
rclpy.init(args=args)
node = UAVTakeoffNode()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
Ardupilot仿真旋转飞行效果演示：
Ardupilot仿真旋转飞行效果演示
目前这个程序还有一些bug，有时会出现请求失败，需要重新运行节点，还有飞机未解锁就进入转圈模式等小问题，欢迎大家批评指正。
特别感谢以下项目和文章，为本文提供部分参考
【github】ros2_mavros_px4
【csdn】解决Ardupilot+gazebo+mavros在仿真状态下无人机能解锁，但是不能起飞的问题
【知乎】Ardupilot基于ROS-Gazebo的仿真模拟