Ardupilot+ROS2+gazebo+mavros仿真之读取无人机状态并尝试解锁

0.准备工作

拥有一个安装了ROS2 Humble的ubuntu 22.04虚拟机；处在一个良好的网络环境（能访问Github）

1.搭建ardupilot源码编译环境

在此感谢怒飞垂云大佬的教程
教学文章
教学视频

2.下载编译ardupilot_gazebo

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退出

Gazebo使用ROS2自带的应该就可以。网上有说法不能用Ros自带的gazebo，因为自带gazebo配置总是不能通信，需要卸掉重新安装，这个还有待求证。

3.安装Mavros

mavros安装，使用gitee国内仓库安装，100%成功

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

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

编辑刚复制来的apm.launch，修改默认启动方式，从串口改为改为udp模拟输出

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

4.新建节点

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

新建一个offboard_pkg.py
官网state消息定义如下：

std_msgs/Header header
bool connected
bool armed
bool guided
string mode
uint8 system_status

而且/mavros/state话题的QoS配置是RELIABLE，因此直接创建订阅者即可，默认就是RELIABLE配置。

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',
        ],

编译以下就可以运行了
打开四个终端，分别输入

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

gazebo --verbose worlds/iris_arducopter_runway.world

cd ~/launch
ros2 launch apm.launch

source ~/colcon_ws/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)
……

————————————————
2024.8.23
发现了一个不错的项目ROS2+Mavros+PX4
https://github.com/asmbatati/ros2_mavros
运行了其中的arm.py，解锁成功了，后续有机会会移植其他部分