激光雷达 SLAM 套件使用 fcu_core_v2

实验环境：Windows 11 宿主机，VMware 17.5 Pro 虚拟机，Ubuntu 20.04 LTS。

从 GitHub 获取源码（已完成此步骤时可跳过）

创建工作空间：

mkdir -p ~/ros_ws/src

拉取 fcu_core_v2 和 quadrotor_msgs：项目 README

修改 launch 文件参数

DRONE_IP：无人机 Mlink 的 IP 地址。
channel：1。
offboard：false。
use_uwb：false。
set_goal：true。
simple_target：true。

SLAM 套件中，set_goal==true 表示发送 EGO 避障目标点，set_goal==false 表示发送普通非避障目标点。

    <node pkg="fcu_core" type="fcu_bridge_001" name="fcu_bridge_001" output="screen" >
        <remap from="~odometry_001" to="/Odometry"/>
        <remap from="~imu_global_001" to="imu_global_001"/>
        <remap from="~odom_global_001" to="odom_global_001"/>
        <remap from="~path_global_001" to="path_global_001"/>
        <remap from="~path_target_001" to="path_target_001"/>
        <remap from="~goal_001" to="/move_base_simple/goal"/>
        <remap from="~motion_001" to="motion_001"/>
        <remap from="~command" to="/fcu_command/command"/>
        <remap from="~mission_001" to="/fcu_mission/mission_001"/>
        <param name="DRONE_IP" value="192.168.0.201" type="string"/> <!-- 修改为对应无人机 Mlink 的 IP 地址 -->
        <param name="DRONE_PORT" value="333"/>
        <param name="USB_PORT" value="/dev/ttyACM0" type="string"/>
        <param name="BANDRATE" value="460800"/>
        <param name="channel" value="1"/>
        <param name="offboard" value="false" type="bool"/>
        <param name="use_uwb" value="false" type="bool"/> <!-- use_uwb改为false -->
        <param name="set_goal" value="true" type="bool"/> <!-- set_goal改为true -->
        <param name="simple_target" value="true" type="bool"/>
        <param name="odom_init_x" value="0.0"/> <!-- FLU 坐标系 -->
        <param name="odom_init_y" value="0.0"/>
        <param name="odom_init_z" value="0.0"/>
    </node>

系统测试

雷达 SLAM 套件中的定点飞行使用定位系统的全局坐标。RViz 插件中的航点为绝对坐标，须与实际地图和里程计的原点、坐标轴方向一致。飞行前应结合显示位置与实际移动方向进行核对，不能将程序内部转换后的“前、右、上”目标坐标直接作为 RViz 坐标填写。

测试 EGO 是否正常启动

将无人机放置在地面上启动。 EGO 成功启动后的界面如下图所示：通过 VNC 远程连接机载电脑或直接连接显示器时的画面： fcu_core_v2 的 RViz 界面：

轨迹测试

确认 SLAM 初始化完成且定位正常后，提起无人机，移动一段距离后放回地面，并检查显示轨迹是否与实际运动一致。初始化完成前不得触碰或移动无人机。正常轨迹如下图所示：

规划测试

使用 2D Nav Goal 工具发布目标点，检查是否生成规划轨迹。正常规划结果如下图所示：

连接无人机：在虚拟机中进入 ros_ws 目录，打开终端并执行以下命令：

catkin_make
source ./devel/setup.bash

随后运行 ROS 程序：

roslaunch fcu_core fcu_core.launch

终端显示以下信息时，表示已收到心跳包，连接成功。

飞行测试

飞行前准备

确保起点和终点周围 1 m 范围内无障碍物，并确认 SLAM 定位正常；定位异常时，不允许解锁。

飞行测试注意以下事项：

无人机必须在地面上通电启动，不得在桌面启动后再移至地面。
从无人机通电至 SLAM 启动完成期间，不得触碰无人机。启动完成后方可移动无人机，且不得用手掌大面积遮挡激光雷达的扫描区域。
可通过以下方式检查 SLAM 是否完成启动：
1. 使用连接至机载电脑的显示器查看地图。无人机通电后，界面出现稳定的 EGO 地图，表示启动完成。
2. 通过 VNC 远程桌面查看地图。界面出现稳定的 EGO 地图，表示启动完成。
3. 使用手机 App 连接无人机。当气压高度由 0 变为 -0.1 时，表示 SLAM 启动完成。随后应核对全局坐标的 x、y 值是否接近 0，以确认 SLAM 工作正常。
4. 在远程电脑的 ROS 环境中订阅 odom_global_001 话题。当 z 值由 0 变为 -0.1 时，表示 SLAM 启动完成。随后应核对 x、y 值是否接近 0，以确认 SLAM 工作正常。

完成上述检查并确认定位正常后，继续进行航点设置和解锁检查。若仅连接远程电脑上的 ROS 程序，未连接遥控器，则默认使用电脑控制模式。若同时连接机载电脑和遥控器，需注意：CH7 位于中间挡位时，无人机由遥控器控制；CH7 位于下方挡位时，无人机由电脑控制。

遥控器设置

熟悉 RViz 插件操作

飞行测试前，请先阅读 rviz_plugin 的使用，熟悉航点设置、轨迹执行及基础控制按钮。

确认无人机连接

确认系统测试中的 ROS 程序仍在运行，且无人机通信正常。若程序已退出，请在虚拟机中进入 ros_ws 目录，打开终端并执行以下命令；若程序已在运行，无需重复启动。

catkin_make
source ./devel/setup.bash

随后运行 ROS 程序：

roslaunch fcu_core fcu_core.launch

设置航点

本节采用单次执行方式，请将 循环执行 设为 关；该设置同时作用于所有活动无人机。

在 RViz 插件中选择 无人机 1，在其航点列表中设置四个航点，构成方形轨迹，如下图所示。图中坐标仅为示例，请根据实际坐标原点、坐标轴方向及场地范围设置航点。

无人机解锁

（1）已启用 SLAM 定高的机型 可通过 global_gnss_001 话题中的状态数据判断 SLAM 是否正常启动。在终端中执行 rostopic echo /fcu_bridge_001/global_gnss_001，查看话题内容并判断 SLAM 启动状态。未启动或启动异常：正常启动：