这篇文章介绍一个用于 PX4 Offboard 控制的 ROS 2 功能包：px4_ros2_ctrl。

项目地址：https://github.com/Atticlmr/px4_ros2_ctrl

它的目标不是写一个把所有事情都包进去的“大节点”，而是把 PX4 Offboard 控制拆成几个边界清楚的部分：控制器只负责产生控制输出，状态机负责安全流程，适配层负责把统一控制输出翻译成 PX4 的 /fmu/in/* 消息。

为什么需要这个包

ROS 2 控 PX4 时，最容易把下面这些逻辑混在一起：

  • 轨迹或控制律计算
  • Offboard 心跳发布
  • PX4 模式切换
  • 解锁、降落等 vehicle command
  • 遥控器接管检测
  • 控制器超时和估计器超时处理

这些逻辑写在一个 demo 节点里当然能飞，但后面换 MPC、SO3、RL policy 或者 motion capture 定位时，节点会越来越难维护。px4_ros2_ctrl 的设计思路是：控制器只发布“我想让飞机怎么动”，不要直接碰 PX4 模式、解锁和 failsafe；这些更接近系统安全边界的事情交给统一的 FSM 节点处理。

整体数据流如下：

1
2
3
4
5
6
7
8
9
10
11
controller node
  -> /controller/<name>/output
  -> fsm_node
  -> Px4OutputAdapter
  -> /fmu/in/offboard_control_mode
  -> /fmu/in/trajectory_setpoint
  -> /fmu/in/vehicle_rates_setpoint
  -> /fmu/in/vehicle_thrust_setpoint
  -> /fmu/in/vehicle_torque_setpoint
  -> /fmu/in/vehicle_command
  -> PX4

这个结构的好处是：新增控制器时，不需要每个控制器都重新实现一遍 Offboard 预热、手动接管锁定、PX4 状态检测和降级处理。

仓库结构

核心文件大致如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
px4_ros2_ctrl
├── include/px4_ros2_ctrl
│   ├── controller_output.h
│   ├── fsm_node.h
│   ├── frame_transforms.h
│   └── px4_output_adapter.h
├── launch
│   ├── fsm_position_control.launch.py
│   ├── fsm_body_rate_nmpc.launch.py
│   ├── fsm_rl_thrust.launch.py
│   └── mocap_udp_bridge.launch.py
├── scripts
│   ├── body_rate_nmpc_controller.py
│   ├── generate_body_rate_nmpc_solver.py
│   ├── test_body_rate_nmpc_solver.py
│   └── fit_thrust_calibration.py
├── src
│   ├── controller
│   │   ├── position_controller.cpp
│   │   └── rl_thrust_controller.cpp
│   ├── fsm/fsm_node.cpp
│   ├── lib/px4_output_adapter.cpp
│   ├── localization/mocap_udp_bridge.cpp
│   └── calibration/thrust_calibration_node.cpp
├── generated/body_rate_nmpc
├── models
└── third_party
    ├── acados
    ├── casadi
    └── onnxruntime

主要节点：

节点 作用
fsm_node PX4 Offboard 状态机和安全监督节点
position_controller 简单位置控制 demo，发布 /controller/position/output
body_rate_nmpc_controller.py 基于 acados/CasADi 的 BODY_RATE + THRUST NMPC demo
rl_thrust_controller 基于 ONNX Runtime 的纯推力策略推理节点
mocap_udp_bridge 从 UDP 接收 mocap 位姿并发布 PX4 visual odometry
thrust_calibration_node 记录电压和推力命令，用于推力标定

核心设计：控制输出先统一，再适配 PX4

仓库里用 ControllerOutput 表示控制器输出层级：

1
2
3
4
5
6
7
8
9
enum class ControlLevel : uint8_t {
  POSITION = 0,
  VELOCITY = 1,
  ACCELERATION = 2,
  ATTITUDE = 3,
  BODY_RATE = 4,
  THRUST = 5,
  ACTUATOR = 6,
};

目前已经打通的路径主要有三类：

控制器输出 控制器 topic PX4 输入 topic
位置控制 /controller/position/output /fmu/in/trajectory_setpoint
机体系角速度 + 推力 /controller/body_rate/output /fmu/in/vehicle_rates_setpoint
纯推力 /controller/thrust/output /fmu/in/vehicle_thrust_setpoint 和 /fmu/in/vehicle_torque_setpoint

Px4OutputAdapter 是唯一真正发布 PX4 输入 topic 的层。比如位置控制会被映射成：

1
2
OffboardControlMode.position = true
TrajectorySetpoint.position = controller position target

BODY_RATE 控制会被映射成：

1
2
3
OffboardControlMode.body_rate = true
VehicleRatesSetpoint.roll / pitch / yaw
VehicleRatesSetpoint.thrust_body

纯推力控制会被映射成：

1
2
3
OffboardControlMode.thrust_and_torque = true
VehicleThrustSetpoint.xyz
VehicleTorqueSetpoint.xyz = [0, 0, 0]

也就是说，新控制器最好不要直接发布 /fmu/in/*，而是发布到 /controller/<name>/output，再由 FSM 判断当前是否允许把这条控制输出送给 PX4。

FSM 状态机

fsm_node 是这个包最重要的部分。它订阅 PX4 状态、估计器状态、手动控制输入和控制器输出，并通过 service 控制 Offboard 的进入和退出。

状态如下：

1
2
3
4
5
6
7
WAIT_FOR_PX4
STANDBY
OFFBOARD_PREPARE
OFFBOARD_REQUESTED
OFFBOARD_ACTIVE
MANUAL_OVERRIDE
FAILSAFE

正常启动流程：

1
2
3
4
5
6
7
8
9
10
启动 launch
  -> fsm_node 等待 PX4 和 estimator 数据
  -> WAIT_FOR_PX4 -> STANDBY
  -> 用户调用 /fsm_node/start_offboard
  -> STANDBY -> OFFBOARD_PREPARE
  -> 先持续发布 Offboard heartbeat 和 setpoint
  -> 发送 PX4 Offboard mode command
  -> OFFBOARD_PREPARE -> OFFBOARD_REQUESTED
  -> PX4 确认进入 Offboard
  -> OFFBOARD_REQUESTED -> OFFBOARD_ACTIVE

这里有一个很重要的细节：启动 launch 不等于立刻进入 Offboard。控制器可以已经在发布目标点，但 fsm_node 在 STANDBY 时不会把它转发到 /fmu/in/*。必须显式调用：

1
ros2 service call /fsm_node/start_offboard std_srvs/srv/Trigger

停止 Offboard：

1
ros2 service call /fsm_node/stop_offboard std_srvs/srv/Trigger

重置手动接管或 failsafe 锁定：

1
ros2 service call /fsm_node/reset_override std_srvs/srv/Trigger

安全逻辑

FSM 会检查三类健康状态：

  • PX4 control mode 数据是否超时
  • local position 是否有效并且没有超时
  • 控制器输出是否足够新

默认参数在 fsm_position_control.launch.py 中是：

1
2
3
4
5
6
7
active_controller: position
allow_auto_arm: false
land_on_failsafe: false
controller_timeout_s: 0.25
px4_timeout_s: 3.0
estimator_timeout_s: 0.5
offboard_prepare_s: 1.1

allow_auto_arm 默认是 false，所以 FSM 默认不会自动解锁。SITL 调试时可以改成 true，但真实飞机上建议保守一点，先通过 QGroundControl 或遥控器手动 arm。

如果 PX4 离开 Offboard、进入手动模式，或者 manual control sticks moving，FSM 会进入 MANUAL_OVERRIDE，并且不会自动重新进 Offboard。这个锁定需要用户显式 reset。

编译

工作空间路径假设为：

1
/home/li/Desktop/ws_ros2

编译：

1
2
3
4
cd /home/li/Desktop/ws_ros2
source /opt/ros/humble/setup.bash
colcon build --packages-select px4_ros2_ctrl
source install/setup.bash

依赖上需要 ROS 2、px4_msgs、PX4 uXRCE-DDS bridge，以及包内使用的 Eigen、ONNX Runtime、acados/CasADi 等组件。仓库里已经带了 third_party/onnxruntime、third_party/acados 和 third_party/casadi 相关内容。

PX4 SITL 启动流程

推荐先启动 Micro XRCE-DDS Agent：

1
MicroXRCEAgent udp4 -p 8888

再启动 PX4 SITL：

1
2
cd /home/li/PX4-Autopilot
make px4_sitl gz_x500

然后检查 ROS 2 是否能看到 PX4 话题：

1
2
3
cd /home/li/Desktop/ws_ros2
source install/setup.bash
ros2 topic list | grep /fmu/out

至少应该能看到类似：

1
2
3
4
/fmu/out/vehicle_control_mode
/fmu/out/vehicle_local_position
/fmu/out/vehicle_odometry
/fmu/out/manual_control_setpoint

运行位置控制 demo

启动 FSM 和位置控制器：

1
2
3
cd /home/li/Desktop/ws_ros2
source install/setup.bash
ros2 launch px4_ros2_ctrl fsm_position_control.launch.py

这个 launch 会启动：

1
2
/fsm_node
/position_controller

position_controller 订阅 /fmu/out/vehicle_local_position，然后以 10 Hz 发布 /controller/position/output。默认目标点是 PX4 NED 坐标系下的正方形轨迹：

1
2
3
4
(0, 0, -2)
(5, 0, -2)
(5, 5, -2)
(0, 5, -2)

这里的 z = -2 表示 NED 坐标系下向上 2 米。启动 launch 后，等 PX4 和 local position 健康，再调用：

1
ros2 service call /fsm_node/start_offboard std_srvs/srv/Trigger

如果 allow_auto_arm 还是默认的 false，需要手动解锁。

BODY_RATE + THRUST NMPC demo

仓库里还提供了一个 body_rate_nmpc_controller.py。它用 acados 生成的 C solver 求解 NMPC，输出 roll/pitch/yaw body rate 和 normalized thrust，然后由 FSM 转发给 PX4。

运行链路：

1
2
3
4
5
6
/fmu/out/vehicle_odometry
  -> body_rate_nmpc_controller.py
  -> acados generated C solver
  -> /controller/body_rate/output
  -> fsm_node
  -> /fmu/in/vehicle_rates_setpoint

生成 solver：

1
2
3
4
5
cd /home/li/Desktop/ws_ros2/src/px4_ros2_ctrl
PYTHONPATH=$PWD/third_party/acados/interfaces/acados_template:$PWD/third_party/casadi/build/lib \
LD_LIBRARY_PATH=$PWD/third_party/acados/lib:$PWD/third_party/casadi/build/lib \
ACADOS_SOURCE_DIR=$PWD/third_party/acados \
./scripts/generate_body_rate_nmpc_solver.py

测试 solver：

1
./scripts/test_body_rate_nmpc_solver.py

启动 NMPC demo：

1
2
3
cd /home/li/Desktop/ws_ros2
source install/setup.bash
ros2 launch px4_ros2_ctrl fsm_body_rate_nmpc.launch.py

再请求 Offboard：

1
ros2 service call /fsm_node/start_offboard std_srvs/srv/Trigger

这个 demo 的 NMPC 状态量是 NED 位置、NED 速度和 body-FRD 到 NED 的 Hamilton quaternion：

1
2
3
4
5
x = [
  p_n, p_e, p_d,
  v_n, v_e, v_d,
  q_w, q_x, q_y, q_z
]

控制输入是：

1
2
3
4
5
6
u = [
  roll_rate,
  pitch_rate,
  yaw_rate,
  thrust
]

PX4 多旋翼里，向上的 body-FRD 推力通常写成 thrust_body[2] 的负值，所以输出会映射到：

1
VehicleRatesSetpoint.thrust_body = [0, 0, -thrust]

RL thrust controller

rl_thrust_controller 是一个 ONNX Runtime C++ 推理节点。它订阅：

1
/fmu/out/vehicle_odometry

发布：

1
/controller/thrust/output

默认输入 tensor 是 [1, 10]：

1
2
3
4
5
[
  p_n, p_e, p_d,
  v_n, v_e, v_d,
  q_w, q_x, q_y, q_z
]

如果 policy 输出是 [1]，节点会把它当作 scalar normalized thrust，并按参数 clamp 到 thrust_min 和 thrust_max。如果输出是 [3]，则当作 body-FRD thrust vector。

启动方式：

1
2
3
cd /home/li/Desktop/ws_ros2
source install/setup.bash
ros2 launch px4_ros2_ctrl fsm_rl_thrust.launch.py model_path:=/absolute/path/to/policy.onnx

需要注意：纯推力控制不是完整的多旋翼控制器。这个路径更适合接口测试、垂向推力实验，或者和其他稳定层配合使用。真实飞机上使用前必须先明确姿态或力矩稳定策略。

Mocap UDP bridge

仓库里还有 mocap_udp_bridge，用于从 UDP 接收 motion capture 位姿，并发布到 PX4 visual odometry 输入。相关 launch 是：

1
ros2 launch px4_ros2_ctrl mocap_udp_bridge.launch.py

这部分适合把外部定位系统接进 PX4 estimator，例如室内动捕、外部 VIO 或自定义定位链路。使用时要特别注意坐标系方向、时间戳和 PX4 estimator 参数，否则看起来“有数据”，但 EKF 可能并没有正确融合。

推力标定

thrust_calibration_node 用来记录电池电压和当前推力命令。启动：

1
ros2 run px4_ros2_ctrl thrust_calibration_node

开始记录：

1
ros2 service call /thrust_calibration_node/start std_srvs/srv/Trigger

停止并保存：

1
ros2 service call /thrust_calibration_node/stop std_srvs/srv/Trigger

清空缓存：

1
ros2 service call /thrust_calibration_node/reset std_srvs/srv/Trigger

指定输出文件和参数：

1
2
3
4
5
ros2 run px4_ros2_ctrl thrust_calibration_node --ros-args \
  -p output_file:=/tmp/thrust_calibration.csv \
  -p time_interval:=1.0 \
  -p min_battery_voltage:=13.2 \
  -p mass_kg:=1.0

拟合标定参数：

1
ros2 run px4_ros2_ctrl fit_thrust_calibration.py /tmp/thrust_calibration.csv

添加自己的控制器

如果要添加一个新控制器，建议遵循这个模式：

  1. 订阅需要的 PX4 或估计器状态，例如 odometry、local position、IMU。
  2. 计算控制输出。
  3. 发布到 /controller/<name>/output。
  4. 在 fsm_node 中添加对应 controller 名称、topic callback 和 ControlLevel。
  5. 在 Px4OutputAdapter 中确认这个 ControlLevel 如何映射到 PX4 topic。
  6. 新增 launch 文件，设置 active_controller。

这样新控制器不用关心 Offboard 预热、PX4 command、手动接管和超时 failsafe，可以把注意力放在控制律本身。

小结

  • 控制器层：只计算输出。
  • FSM 层：负责进入 Offboard、退出 Offboard、超时和手动接管。
  • PX4 adapter 层：负责真正发布 /fmu/in/*。