forked from zbw/yiliao2026
243 lines
9.6 KiB
Markdown
243 lines
9.6 KiB
Markdown
# 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
|
||
|
||
# 启动图生文
|
||
|
||
```
|