🎯?五大核心模塊詳解

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執行 → 傳感器反饋 → 狀態更新 → 循環繼續