技术：ROS2(humble) + Ardupilot + mavros + gazebo11

• ArduPilot 是一个功能丰富且灵活的开源自动驾驶平台，它的广泛适用性和开源特性，使其在无人系统领域得到了广泛应用。

• Mavros负责ROS2和Ardupilot之间的数据传输，并通过MAVLink协议与Ardupilot进行通信，也就是有了它我们可以在ROS2编写节点控制无人机在gazebo仿真环境下运动。

• 假设已经有了Ubuntu22.04的ROS2 Humble环境，并且已经装好了gazebo11。

✈️配置Ardupilot环境

飞控源代码脚本文件搭建编译环境

• 首先安装git（如果没有），然后克隆Ardupilot仓库（在你需要放置的目录下，我这里就是home目录下），最好使用魔法上网。

sudo apt install git
git clone https://github.com/ArduPilot/ardupilot.git

• 进入到ardupilot目录，然后执行下面的命令，第二句不能在前面加上sudo或者sh，否则会报错。遇到[Y/N]选择时，都选y。

cd ardupilot/Tools/environment_install
./install-prereqs-ubuntu.sh

• 安装完成后，重启电脑，让一些环境变量生效

编译飞控固件

• 下列步骤依次是：

1. 进入飞控源代码的最顶层目录；
2. 切换到一个稳定版（以多旋翼4.2.3为例）；
3. 更新子模块；
4. 指定飞控硬件型号（以Pixhawk2.4.8为例）；
5. 编译飞控固件（多旋翼固件）。

cd ardupilot
git checkout -b MyCopter-4.2.3 Copter-4.2.3
git submodule update --init --recursive
./waf configure --board fmuv3
./waf copter

测试

cd Tools/autotest
sim_vehicle.py -v ArduCopter

运行如上指令，会弹出这样的界面，就说明装好了。

✈️下载编译ardupilot_gazebo

• 回到home目录，运行如下指令

git clone https://github.com/SwiftGust/ardupilot_gazebo
#或者 git clone https://github.com/khancyr/ardupilot_gazebo
sudo apt-get install libgazebo11-dev
cd ardupilot_gazebo
mkdir build
cd build
cmake ..
make -j4
sudo make install

• 下一步编辑环境变量并加入以下内容：

sudo nano ~/.bashrc

source /usr/share/gazebo/setup.sh
export GAZEBO_MODEL_PATH=~/ardupilot_gazebo/models:${GAZEBO_MODEL_PATH}
export GAZEBO_MODEL_PATH=~/ardupilot_gazebo/models_gazebo:${GAZEBO_MODEL_PATH}
export GAZEBO_RESOURCE_PATH=~/ardupilot_gazebo/worlds:${GAZEBO_RESOURCE_PATH}
export GAZEBO_PLUGIN_PATH=~/ardupilot_gazebo/build:${GAZEBO_PLUGIN_PATH}

ctrl+O ，回车保存；ctrl+x退出

✈️安装Mavros

• 运行如下指令安装

sudo apt install ros-humble-mavros ros-humble-mavros-extras
git clone https://gitee.com/MrZhaosx/geographic-lib.git
cd geographic-lib
sudo cp -r geoids gravity magnetic /usr/share/GeographicLib/
sudo ./install_geographiclib_datasets.sh

输出以下内容就是成功：

GeographicLib geoids dataset egm96-5 already exists, skipping
GeographicLib gravity dataset egm96 already exists, skipping
GeographicLib magnetic dataset emm2015 already exists, skipping

• 接着复制一个launch文件用于启动mavros，可以放在home目录。

mkdir ~/launch
cp /opt/ros/humble/share/mavros/launch/apm.launch ~/launch/

• 将launc文件中的内容进行如下更改，从串口改为改为udp模拟输出：

<arg name="fcu_url" default="/dev/ttyACM0:57600" />
#修改为
<arg name="fcu_url" default="udp://127.0.0.1:14551@14555" />

✈️基础节点测试

• 首先在你习惯的位置创建一个工作空间：

ros2 pkg create offboard_pkg --build-type ament_python --dependencies rclpy mavros geometry_msgs --license Apache-2.0

• 先写一个简单的节点看看是否可以获取无人机状态，命名为offboard_node：

import rclpy
import rclpy.logging
from rclpy.node import Node
from mavros_msgs.msg import State
# 全局变量
current_state = State()

# 无人机状态回调函数
def state_callback(msg):
    global current_state
    current_state = msg
    #测试打印
    logger = rclpy.logging.get_logger('state_sub')
    logger.info('State: isconnected={} (bool), armed={} (bool)'.format(
        current_state.connected,
        current_state.armed))
    logger.info('State: isguided={} (bool), mode={} (string)'.format(
        current_state.guided,
        current_state.mode))

def main(args=None):
    rclpy.init(args=args)
    node = Node('offboard_node')
    # 创建订阅者
    state_sub = node.create_subscription(State, '/mavros/state', state_callback, 10)
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

• setup.py记得添加该节点：

'console_scripts': [
            'offboard_node = offboard_pkg.offboard_node:main',
        ],

• 使用4个终端分别启动ardupilot、gazebo、mavros、节点

ardupilot：

sim_vehicle.py -v ArduCopter -f gazebo-iris  --map --console

gazebo，在ardupilot_gazebo底下加载世界world（也不一定得是这个world文件）：

gazebo --verbose worlds/iris_ardupilot.world

通过launch文件夹中的launch文件启动mavros：

cd ~/launch
ros2 launch apm.launch

在你的工作目录中启动节点：

source install/setup.bash
ros2 run offboard_pkg offboard_node

• 如果最后这个终端输出如下信息，即为测试成功：

[INFO] [1724314101.290484203] [state_sub]: State: isconnected=True (bool), armed=False (bool)
[INFO] [1724314101.291869777] [state_sub]: State: isguided=False (bool), mode=STABILIZE (string)
[INFO] [1724314102.332203155] [state_sub]: State: isconnected=True (bool), armed=False (bool)
[INFO] [1724314102.337817151] [state_sub]: State: isguided=False (bool), mode=STABILIZE (string)
……

✈️控制无人机运动

• 我们需要新建一个ROS2节点。这个节点的主要职责是控制无人机安全地起飞到预设的高度，并进行圆周运动。代码的主体结构如下：

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

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()

• 实现效果如下，刚开始还是没有那么稳定的圆周运动，并且启动节点时常会有可能没成功解锁无人机要多次启动节点。但是至此也算是成功实现了无人机最基本的仿真。