单机自动任务

实验环境：Jetson Orin NX，Ubuntu 20.04 LTS。

本章所有程序均在机载电脑上运行，可在本机终端操作，也可通过 SSH 登录机载电脑后操作。

飞行全程须保持遥控器连接。执行自动任务时，将 CH7 置于下方挡位，使用电脑控制模式；需要人工接管时，将 CH7 切换至中间挡位，使用遥控器控制模式。

本章先使用默认的直接飞点模式，运行一次“解锁 → 起飞 → 两点飞行 → 降落 → 锁定”示例，再介绍雷达 SLAM 定位下的 EGO 目标点飞行。两种方式均在机载电脑上执行，使用同一份任务文件。

1. 确认配置

先按所用定位套件的教程完成通信、定位和电脑控制模式检查。以下以 UWB 套件 通过 Mlink 网络连接单架无人机为例。

提前打开功能包中的 fcu_core_v2/launch/fcu_core.launch，将已有的自动任务开关改为：

<arg name="enable_auto_mission" default="true"/>

机载电脑的 auto_ap.sh 开机启动该 launch 时，将按上述配置启用自动任务，无需在启动命令中追加参数。再检查 fcu_bridge_001 节点：

fcu_auto_mission 节点中的 use_planner=false 保持默认，不需要配置规划器话题。使用其他定位套件或串口连接时，先按对应套件教程调整通信与定位配置。

主 launch 还会启动其他通信节点；自动任务本身只控制 fcu_bridge_001。

2. 核对默认任务

任务文件为 fcu_core_v2/src/fcu_auto_mission/example_mission.py：

def mission(drone):
    drone.arm()
    drone.takeoff()
    drone.goto(x=3.0, y=0.0, z=1.0, yaw=0.0)
    drone.hover(seconds=5.0)
    drone.goto(x=3.0, y=1.0, z=1.0, yaw=0.0)
    drone.hover(seconds=5.0)
    drone.land()
    drone.disarm()

起飞后，依次前往（3.0, 0.0, 1.0）和（3.0, 1.0, 1.0），每点保持 5 秒，然后在第二个目标点降落并锁定。

XYZ 使用 /odom_global_001 的坐标约定，单位为米，均为绝对坐标，不叠加起飞位置；yaw 单位为弧度，正方向向左。接口自动转换 Y 和 yaw 的符号，无需手动取反。场地、原点和航段均适合上述路线时可保留示例，否则只修改 goto() 中的目标坐标。任务文件修改后须重启节点才会生效。

起飞高度由 FanciSwarm App 设置，z=1.0 不会修改 App 的起飞高度。goto() 仅判断 XY 到点，不检查高度和朝向；hover() 保持目标并计时，也不验证实际悬停状态。

解锁、起飞、降落和锁定采用“发送一次命令后等待”的方式，不检测动作是否完成。默认解锁后等待 2 秒、起飞后等待 5 秒、降落后等待 15 秒、锁定后等待 2 秒。必须保证降落等待时间足够长：示例在等待结束后发送锁定命令，不检查是否接地。 需要调整时，提前修改 launch 中 land_wait 的默认值；disarm_wait 是锁定后的等待时间，不能代替降落等待。

3. 配置开机自启动

首次部署或更新源码后，在机载电脑中进入实际 ROS 工作空间编译，以下以 ~/ros_ws 为例：

cd ~/ros_ws
catkin_make
source devel/setup.bash

找到机载电脑已有的 auto_ap.sh 开机自启动文件，确认其已加载实际工作空间环境，并启动本教程修改的 launch。对应启动命令为：

roslaunch fcu_core fcu_core.launch

沿用脚本原有的启动管理方式，不要重复添加同一启动项；如原命令显式设置了 enable_auto_mission:=false，须移除该覆盖值。若脚本使用安装空间，应修改实际加载的配置，并在源码更新后重新安装。

完成配置后重启机载电脑，由 auto_ap.sh 自动启动节点。平时使用无需再手动执行 roslaunch。自动任务节点加载默认任务后等待启动服务，节点开机自启动不等于任务自动起飞。等待 bridge 启动至少 5 秒，并确认定位和通信正常，再进行下一节检查。

如需将降落等待改为 20 秒，提前将 launch 中对应配置改为 <arg name="land_wait" default="20.0"/>。

4. 检查并执行

每个新终端均先执行 source ~/ros_ws/devel/setup.bash，工作空间路径按实际情况修改。若自启动脚本使用安装空间，则加载对应的 install/setup.bash，确保终端与自启动程序使用同一工作空间环境。

在终端 B 中查看里程计，确认位置、朝向与实际一致且持续更新，检查完成后按 Ctrl+C 退出该监听：

rostopic echo /odom_global_001

随后监听任务状态，保持该终端运行：

rostopic echo /fcu_auto_mission/status

确认定位正常、遥控器已连接、CH7 位于下方挡位且航线可用后，在终端 C 中调用任务启动服务。也可通过 SSH 登录机载电脑后执行同一命令：

rosservice call /fcu_auto_mission/start "{}"

若需开机后自动执行任务，可在 auto_ap.sh 的末尾安排延时调用上述服务，并在调用前检查飞行条件、定位和话题连接是否就绪。固定延时不代表这些条件已满足；调用成功后，示例将执行解锁和起飞。

服务返回 success: true 表示接受启动请求。终端 B 会显示 RUNNING、各动作名称，最终显示 SUCCEEDED 或 FAILED。SUCCEEDED 只表示程序执行完毕，仍须确认实际着陆并锁定，再关闭无人机、断开电池。

本示例使用的接口

/fcu_auto_mission/command 向 bridge 发送解锁、起飞等命令，/fcu_auto_mission/mission_001 发送直接飞点目标；程序已自动连接，无需手动发布。启用自动任务后，旧键盘控制话题不再控制该 bridge。

/fcu_auto_mission/control_001、/move_base_simple/goal 和 /planning/pos_cmd 用于第 6 节的 EGO 规划模式，前面的直接飞点示例无需配置或监听。

5. 取消与简单排查

需要取消时，在终端 C 中执行：

rosservice call /fcu_auto_mission/cancel "{}"

取消不自动降落或锁定。正在飞点且定位有效时改为保持当前位置；正在起飞或降落时，不撤回已发送的命令。定位失效时停止发布目标。取消后须确认实际状态，按已熟悉的接管方式处理；关闭终端不能代替降落。

同一时间只能运行一个任务。再次调用 start 会从任务开头执行，不是断点继续；重新测试前，先确认无人机已着陆、锁定并完成飞行前检查。

6. 雷达 SLAM 定位与 EGO 目标点飞行

本节以 Jetson Orin NX 通过串口连接飞控、Faster-LIO 输出 /Odometry、EGO 规划目标点为例。沿用第 2 节的 mission(drone)，仅调整连接与规划配置。

修改必要配置

在 fcu_core_v2/launch/fcu_core.launch 中，修改 fcu_bridge_001 节点的以下配置，其余自动任务话题映射保持原样：

里程计映射写法如下：

<remap from="~odometry_001" to="/Odometry"/>

再将 fcu_auto_mission 节点中已有的参数改为：

<param name="use_planner" value="true" type="bool"/>

set_goal 和 use_planner 必须同时设为 true，修改后重启。 恢复直接飞点时，将两者均改回 false，并按所用定位与通信方式恢复相应配置。

启动并运行原示例

1. 提前完成本节配置，并确认 auto_ap.sh 中已有雷达 SLAM、EGO 和 fcu_core.launch 的启动项；enable_auto_mission 按第 1 节设为 true。主 fcu_core.launch 本身不负责启动 SLAM 和 EGO。
2. 将无人机放在地面上启动，由 auto_ap.sh 自动运行上述程序。初始化完成前不要移动无人机；确认定位、地图及 EGO 规划正常后再继续。
3. 确认 EGO 接收 /move_base_simple/goal，输出 /planning/pos_cmd，且 SLAM、EGO 和 /odom_global_001 的原点、轴向与坐标偏移一致。先按 雷达 SLAM 教程 的系统测试检查定位和规划结果。
4. 核对第 2 节两个目标点在当前地图中可达，起飞位置、飞行路径和降落位置均合适；需要时修改示例中的 goto() 坐标并重启节点。起飞高度和降落等待的检查仍按第 2 节执行。
5. 等待 bridge 初始化至少 5 秒，确认 /odom_global_001 持续有效，再调用启动服务，无需重复启动 launch。

确认上述条件满足后，在机载终端或通过 SSH 登录机载电脑调用任务启动服务：

rosservice call /fcu_auto_mission/start "{}"

如需在 auto_ap.sh 中自动调用该服务，应先确认飞行前条件、定位、话题连接及规划器均已就绪；调用服务后将执行解锁和起飞。首次测试建议完成检查后手动调用。

状态查看与取消仍使用第 4、5 节的接口，无需更换任务文件或另写一套动作代码。飞行中观察 EGO 规划与真实轨迹，规划效果取决于地图和规划器配置。

必要话题检查

任务目标由 /fcu_auto_mission/mission_001 经 bridge 发给飞控，再回传到 /move_base_simple/goal；EGO 输出由自动任务节点转发到 /fcu_auto_mission/control_001。这些内部连接无需手动发布，飞控须支持规划目标回传。当前回传分支只填写位置，不保证向 EGO 传递目标 yaw；规划输出也仅转发位置和 yaw，不包含速度、加速度前馈。

如提示规划输出超时，先检查两个模式参数，再观察目标与输出：

rostopic echo /move_base_simple/goal

在另一个终端中执行：

rostopic hz /planning/pos_cmd

运行到 goto() 后没有目标消息时，检查 bridge、串口及飞控回传；有目标但没有规划输出时，检查 EGO 的目标话题配置、地图和规划状态。默认等待首个输出 10 秒，后续输出间隔不能超过 2 秒。定位、规划器和自动任务应能通过同一 ROS master 通信。

返回教程目录 · 上一章：动捕使用 fcu_core_v2 · 下一章：UWB 套件集群飞行