🎯 五大核心模块详解
1. 📥 输入处理模块
keyboard_control_node
├── 功能：捕获键盘输入并转换为ROS消息
├── 文件：keyboard_control.cpp
├── 输入：键盘按键 (W/A/S/D/R/F/Q/E/L/ESC)
├── 输出：geometry_msgs::Twist 消息
└── 目标话题：/px4ctrl/keyboard_cmd
2. 🧠 状态机控制模块
PX4CtrlFSM (有限状态机)
├── 五种控制状态：
│ ├── MANUAL_CTRL - 手动控制 (遥控器优先)
│ ├── AUTO_HOVER - 自动悬停 (支持键盘控制) ⭐
│ ├── CMD_CTRL - 指令跟踪 (轨迹跟踪)
│ ├── AUTO_TAKEOFF - 自动起飞
│ └── AUTO_LAND - 自动降落
├── 安全机制：
│ ├── 状态转换检查
│ ├── 高度限制保护
│ ├── 通信超时保护
│ └── 遥控器优先级
└── 核心函数：keyboardCmdCallback()
3. 🎮 键盘命令处理模块
keyboardCmdCallback() 函数
├── 输入验证：
│ ├── 状态检查 (仅AUTO_HOVER有效)
│ ├── 消息有效性验证
│ └── 降落特殊指令处理
├── 坐标转换：
│ ├── 机体坐标系 → 世界坐标系
│ ├── 考虑当前偏航角
│ └── 水平/垂直运动分离处理
├── 运动增量计算：
│ ├── 水平移动：±0.3m步长
│ ├── 垂直移动：±0.15m步长
│ └── 偏航旋转：±0.2弧度步长
└── 目标更新：hover_pose位置更新
4. 🔗 通信协调模块
px4ctrl_node (主节点)
├── 订阅话题：
│ ├── /px4ctrl/keyboard_cmd (键盘指令)
│ ├── /mavros/local_position/odom (位置反馈)
│ ├── /mavros/imu (IMU数据)
│ └── /mavros/rc/in (遥控器数据)
├── 发布话题：
│ ├── /mavros/setpoint_raw/attitude (姿态指令)
│ ├── /traj_start_trigger (轨迹触发)
│ └── /debugPx4ctrl (调试信息)
└── 服务客户端：
├── /mavros/set_mode (模式设置)
└── /mavros/cmd/arming (解锁服务)
5. ⚙️ 底层执行模块
MAVROS + PX4
├── MAVROS功能：
│ ├── ROS-PX4通信桥接
│ ├── MAVLink协议转换
│ └── 多种数据类型支持
├── PX4功能：
│ ├── 底层飞行控制
│ ├── 传感器数据融合
│ ├── 电机控制输出
│ └── 安全监控
└── 硬件接口：
├── PWM电机控制
├── I2C/SPI传感器
└── 串口通信
用户按键 → 终端捕获 → 消息封装 → ROS发布 → 状态机接收 →
安全检查 → 坐标转换 → 目标更新 → 控制算法 → 姿态指令 →
MAVROS转发 → PX4执行 → 传感器反馈 → 状态更新 → 循环继续