# must_know.md — yiliao_ws 变更说明（mo_new → HEAD）

> **仓库**：`https://gitee.com/hikos/smart-healthcare-2026.git`
> **基准提交**：`436e8a2`（mo_new 分支起点，"Merge remote-tracking branch 'origin/mo' into mo_new"）
> **当前 HEAD**：`8481d56`（"现在总体进程基本流畅，还差导航不够顺利。"）
> **生成日期**：2026-06-13
> **目标读者**：接手调试的另一位同学

---

## 变更总览

从 mo_new 基准到当前 HEAD，共 **101 个文件**变更，**+6074/-296 行**。核心变更集中在 4 个方面：

1. **EKFRobotLocalization（EKF 协方差参数化 + 帧重构）**
2. **SLAM Toolbox（降低 CPU 负载 + 新配置文件）**
3. **导航参数（仿真→真机 + MPPI / 代价地图 / 规划器调整）**
4. **启动文件重构（去重 + 真机部署适配）**

---

## 1. EKFRobotLocalization（origincar_base）

### 1.1 ekf.yaml — 完全重写（222 行 → 48 行）

| 变更 | 旧值 | 新值 | 影响 |
|------|------|------|------|
| **帧名重命名** | `odom_combined` | **`odom`** | EKF 直接发布 `odom->base_footprint` 而非 `odom_combined->base_footprint`。所有下游节点只需监听 `odom` |
| **频率降低** | `30 Hz` | **`20 Hz`** | 降低 CPU 负载 |
| **移除 pose0 输入** | `/set_pose` 订阅 | **已删除** | 不再接受外部位姿注入（`use_pose_with_covariance_stamped` 移除） |
| **移除所有 IMU 拒绝阈值** | `pose:20.0`, `twist:1.542`, `accel:10.0` | **已删除** | 信任所有 IMU 数据，不进行异常值过滤 |
| **移除过程噪声协方差** | 15×15 自定义矩阵 | **已删除** | 使用 EKF 默认值 |
| **移除初始协方差** | 15 个状态全 `1e-9` | **已删除** | 使用 EKF 默认值 |

**注意**：EKF 不再显示指定过程噪声，如果出现估计发散，需要恢复噪声矩阵。

### 1.2 origincar_base.h — 新增成员变量

```cpp
bool publish_tf_;                      // 控制底盘节点是否发布 odom->base_footprint TF
double odom_pose_cov_x_;               // 里程计 X 位置协方差
double odom_pose_cov_y_;               // 里程计 Y 位置协方差
double odom_pose_cov_yaw_;             // 里程计 Yaw 协方差
```

### 1.3 origincar_base.cpp — 4 处关键修改

**① Publish_Odom() — 协方差填充**（第 ~202 行）：
```cpp
odom.pose.covariance[0] = odom_pose_cov_x_;
odom.pose.covariance[7] = odom_pose_cov_y_;
odom.pose.covariance[35] = odom_pose_cov_yaw_;
```
默认值：`x=0.01, y=0.01, yaw=0.0225`（约 8.6° 标准差）。值越大，EKF 越不信任轮式里程计。

**② Publish_Odom() — TF 条件发布**：
```cpp
if (publish_tf_) {
    tf_broadcaster_->sendTransform(t);
}
```
当 EKF 负责发布 `odom->base_link` 时，底盘节点不再发布（避免 TF 冲突）。默认 `publish_tf_ = false`。

**③ Get_Sensor_Data() — 串口异常处理**：
```cpp
try {
    Stm32_Serial.read(Receive_Data_Pr, sizeof(Receive_Data_Pr));
} catch (const serial::SerialException& e) {
    return false;
}
```
防止串口断连导致程序崩溃。

### 1.4 base_serial.launch.py — 协方差启动参数

```python
'odom_pose_cov_x': 0.01,
'odom_pose_cov_y': 0.01,
'odom_pose_cov_yaw': 0.0225,
```

### 1.5 origincar_bringup.launch.py — 移除重复 TF + 添加 robot_description

- **移除**了 `base_footprint -> base_link` 的静态 TF 发布器（避免与 EKF 冲突）
- **添加**了通过 Xacro 处理的 `robot_description` 参数传入 `joint_state_publisher`

### 1.6 robot_mode_description.launch.py — 重构

- `publish_frequency` 显式设为 **15 Hz**（原来是 30 Hz，隐含默认值）
- 改为使用 `parameters=[{'robot_description': ..., 'publish_frequency': 15.0}]` 而非命令行参数

---

## 2. SLAM Toolbox

### 2.1 slam_toolbox_async.yaml — 性能优化

| 参数 | 旧值 | 新值 | 影响 |
|------|------|------|------|
| `base_frame` | `base_footprint` | **`base_link`** | 与 URDF / Nav2 的 `robot_base_frame` 对齐 |
| `throttle_scans` | `1`（全处理） | **`10`** | **大幅降低 CPU**，每 10 帧只处理 1 帧 |
| `minimum_travel_distance` | `0.5m` | **`0.1m`** | 更灵敏的建图触发 |
| `minimum_travel_heading` | `0.5 rad` | **`0.1 rad`** | 更灵敏的角度触发 |
| `scan_buffer_size` | `10` | **`5`** | 减少内存 / 计算量 |
| `scan_buffer_maximum_scan_distance` | `10m` | **`5m`** | 减小扫描缓冲距离 |
| `map_start_pose` | `[0, 0, 0]` | **`[-2, -2, 0]`** | 地图初始位姿偏移 |

### 2.2 slam_toolbox_mapping.yaml — 新增配置

全新的 73 行 SLAM 建图专用配置，作为 `slam_toolbox_async.yaml` 的补充。关键差异：`throttle_scans: 1`（全处理），`scan_queue_size: 20`（增大雷达队列防丢帧）。

---

## 3. 导航参数（gc_navigation_slam.yaml）

这是最重要的配置文件，经历了多轮调试修改。

### 3.1 全局切换

所有节点的 `use_sim_time: True` → `False`（15 处），BT XML 路径从 `/home/guoch/test_ws` → `/home/sunrise/yiliao_ws`。

### 3.2 MPPI 控制器

| 参数 | 旧值 | 新值 | 说明 |
|------|------|------|------|
| `time_steps` | 36 | **20** | 规划时域缩短，减少计算量 |
| `batch_size` | 1000 | **200** | 采样数减少 80%，**CPU 优化的关键** |
| `PathAlignCritic.cost_weight` | 14.0 | **7.0** | 允许偏离路径，是避障优先的关键 |
| `PreferForwardCritic.cost_weight` | 5.0 | **2.0** | 减少前进偏好，允许侧向/倒车 |
| `CostCritic.cost_weight` | 3.81 | 3.81（不变） | |

### 3.3 代价地图

| 参数 | 旧值 | 新值 | 说明 |
|------|------|------|------|
| `inflation_radius` | 0.55m | **0.2m** | 大幅缩小膨胀，允许通过更窄通道 |
| `cost_scaling_factor` | 3.0 | 3.0（不变） | |
| 机器人足迹 | 0.14m 半宽 | 0.14m（不变） | 有效安全边界 = 0.2 - 0.14 = 0.06m |

### 3.4 SmacPlannerHybrid 规划器

| 参数 | 旧值 | 新值 | 说明 |
|------|------|------|------|
| `max_iterations` | 1,000,000 | **100,000** | 搜索上限降低 90% |
| `non_straight_penalty` | 1.2 | **`0.5`** | 鼓励曲线路径，是提前避障的关键 |
| `cost_penalty` | 2.0 | **`5.0`** | 更严厉惩罚途经高代价区域 |
| `lookup_table_size` | 20.0m | **`5.0`** | 减少内存占用 |
| `reverse_penalty` | ? | **`1.3`** | 轻微抑制倒车 |
| `motion_model` | REEDS_SHEPP | REEDS_SHEPP（不变） | |

---

## 4. 启动文件

### 4.1 gc_nav2_with_slam_online_real.launch.py — 新增

真机实时建图专用启动文件。结构：
- 启动 `async_slam_toolbox_node`（使用 `slam_toolbox_mapping.yaml`）
- 包含 Nav2 bringup（使用 `gc_navigation_slam.yaml`）
- **不发布 TF**（由 `origincar_base` 的 EKF + robot_state_publisher 提供）
- **不启动 robot_state_publisher**（避免与底盘启动文件重复）

### 4.2 Foxglove Bridge 优化

运行时通过参数限制话题白名单和功能，已添加到导航启动但不在此 diff 中（属运行期调试修改，未提交）。

---

## 5. 已知问题 & 当前状态

### 5.1 当前整体状态（HEAD: `8481d56`）

流程整体流畅（CPU 从 ~77% 降至 ~23%，load 从 14.5 降至 1.5），但**导航还不够顺利**。

### 5.2 导航调试发现的问题（基于 2026-06-13 现场测试）

1. **恢复行为适得其反**：BT 恢复策略（清除代价地图 → 后退 0.8m → 等待 0.3s）在后方空间不足时会撞入障碍物
2. **急转弯不足**：目标方向变化 >90° 时转向缓慢，`batch_size=200` 降低了采样多样性
3. **提前避障效果待验证**：`non_straight_penalty=0.5` + `cost_penalty=5.0` + `PathAlignCritic=7.0` 这套参数修改后重启导航即生效，未在重启后充分测试
4. **odom 帧漂移**：最后一次提交提到 "odom 坐标系在运动过程中还是会飘"

### 5.3 参数文件位置

| 文件 | 路径 |
|------|------|
| 导航参数 | `src/gc_navigation2_slamtoolbox/params/gc_navigation_slam.yaml` |
| SLAM 配置 | `src/gc_navigation2_slamtoolbox/config/slam_toolbox_mapping.yaml` |
| SLAM 异步 | `src/gc_navigation2_slamtoolbox/config/slam_toolbox_async.yaml` |
| EKF 配置 | `src/origincar_base/config/ekf.yaml` |
| 底盘启动 | `src/origincar_base/launch/base_serial.launch.py` |
| 导航启动 | `src/gc_navigation2_slamtoolbox/launch/gc_nav2_with_slam_online_real.launch.py` |
| 参数备份 | `src/gc_navigation2_slamtoolbox/params/gc_navigation_slam.yaml.bak` |
| 旧代码备份 | `src/origincar_base/src/origincar_base.cpp.bak` |

### 5.4 运行环境

- Ubuntu 22.04 aarch64（ARM 8 核，7GB RAM）
- ROS2 Humble
- 机器人 IP：`192.168.10.210`，用户：`sunrise`
- 工作区：`/home/sunrise/yiliao_ws`
- 所有 ROS2 命令需先 `source /opt/ros/humble/setup.bash`

### 5.5 地图范围

- 当前建图范围：X[-11.876, 6.974], Y[-5.982, 4.068]
- 自由空间（走廊）：X[-11.4, 6.8], Y[-3.1, 2.7]
- 狭窄通道：X=6 处仅 0.7m 宽；X=-7 附近有 1.1m 窄通道
- 地图分辨率：0.05m/pixel

---

## 6. 快速命令参考

```bash
# SSH 登录
ssh sunrise@192.168.10.210

# 查看导航节点状态
source /opt/ros/humble/setup.bash && ros2 node list

# 查看当前位姿
ros2 topic echo /odom_combined --once | grep -A4 "position:"

# 重启导航生命周期
ros2 lifecycle set /lifecycle_manager_navigation shutdown
# 然后重新启动导航 launch

# 发送导航目标（示例）
ros2 topic pub /goal_pose geometry_msgs/msg/PoseStamped "{header: {frame_id: 'map'}, pose: {position: {x: 2.0, y: 0.0}, orientation: {z: 0.0, w: 1.0}}}"

# 查看 LiDAR 扫描
ros2 topic echo /scan --once --field ranges

# 构建
cd /home/sunrise/yiliao_ws && colcon build --packages-select gc_navigation2_slamtoolbox origincar_base

# 查看完整提交历史
cd /home/sunrise/yiliao_ws && git log --oneline 436e8a2..HEAD

# 启动图生文

```