# fishros_chapter9_code

**Repository Path**: haha-hua/fishros_chapter9_code

## Basic Information

- **Project Name**: fishros_chapter9_code
- **Description**: 这个仓库是修复fishros的第九章中slam建图中,打开rviz2里面地图不更新的问题.
- **Primary Language**: Unknown
- **License**: Not specified
- **Default Branch**: master
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 0
- **Forks**: 0
- **Created**: 2026-01-11
- **Last Updated**: 2026-01-13

## Categories & Tags

**Categories**: Uncategorized

**Tags**: None

## README

# FishBot 建图避坑指南：解决地图卡死、红黑分离与时间同步问题

本文档记录了在使用 FishBot（基于 ESP32 + ROS 2 Humble）进行 SLAM 建图时遇到的典型问题及其终极解决方案。如果你也遇到了 **Rviz2 地图不更新**、**雷达红点与地图黑线分离**、**Extrapolation into the future 报错**，请按此指南排查。

---

## 1. 核心问题现象

1.  **地图卡死**：启动 SLAM 后，地图只显示第一帧，或者更新几下后就彻底不动了。`/map_updates` 话题无数据输出。
2.  **时间穿越报错**：Rviz2 或 SLAM 终端疯狂刷屏：
    ```text
    Lookup would require extrapolation into the future. Requested time ... but the latest data is at time ...
    ```
3.  **红黑分离**：Rviz2 中雷达扫描点（红点）与地图障碍物（黑线）严重错位，甚至垂直分离，导致 SLAM 匹配失败。
4.  **QoS 警告**：雷达节点提示 `requesting incompatible QoS`。

### 修复前后效果对比

| **修复前（红黑分离/地图卡死）** | **修复后（完美贴合/实时更新）** |
| :---: | :---: |
| ![Buggy SLAM](images/buggy_slam.png) | ![Fixed SLAM](images/fixed_slam.jpg) |
| *雷达红点与地图黑线严重错位，建图无法进行* | *雷达数据与地图完美匹配，建图流畅* |

---

## 2. 根本原因分析

*   **WiFi 延迟与时间同步误差**：ESP32 通过 micro-ROS Agent 连接电脑，存在网络延迟。导致雷达数据的时间戳（由 ESP32 产生）往往比电脑端的 TF 变换（由 `odom2tf` 产生）要“快”一点点（约 10-200ms）。SLAM 算法认为雷达数据是“未来的”，无法查找到对应的 TF，因此直接丢弃。
*   **里程计漂移**：低成本电机或轮子打滑会导致里程计角度（Yaw）不准。当实际转过 90 度而里程计只报告 80 度时，SLAM 无法将新数据拼接到旧地图上。
*   **QoS 策略冲突**：ROS 2 默认通信策略严格，如果发布者是 `Best Effort` 而订阅者是 `Reliable`，连接会断开。

---

## 3. 终极解决方案（按顺序执行）

### 第一步：魔法时间补偿（解决 Extrapolation 报错）

这是最关键的一步。我们需要在发布 TF 时人为增加一点时间偏移，让 TF “覆盖”住延迟到达的雷达数据。

**修改文件**：`src/fishbot_bringup/src/odom2tf.cpp`

```cpp
// 在 odom_callback_ 函数中
void odom_callback_(const nav_msgs::msg::Odometry::SharedPtr msg) {
    geometry_msgs::msg::TransformStamped transform;
    
    // 【修改前】直接使用当前时间
    // transform.header.stamp = this->get_clock()->now(); 

    // 【修改后】增加 0.2秒 的魔法偏移，补偿 WiFi 延迟
    transform.header.stamp = this->get_clock()->now() + rclcpp::Duration::from_seconds(0.2);
    
    transform.header.frame_id = "odom";
    // ... 其他代码不变
}
```
*修改后记得重新编译：`colcon build --packages-select fishbot_bringup`*

### 补充步骤：雷达驱动时间戳修正（彻底解决不同步）

除了在 `odom2tf` 中补偿时间，还有一个更底层的修改是直接让雷达驱动使用电脑的当前时间，而不是雷达硬件自带的时间戳。这能防止因硬件时钟漂移导致的“时间穿越”。

**修改文件**：`src/ydlidar_ros2/src/ydlidar_node.cpp`

找到发布 LaserScan 消息的地方（通常在 `while` 循环里），修改时间戳赋值：

```cpp
// 【修改前】使用雷达硬件时间（可能导致不同步）
// scan_msg->header.stamp.sec = RCL_NS_TO_S(scan.stamp);
// scan_msg->header.stamp.nanosec = scan.stamp - RCL_S_TO_NS(scan_msg->header.stamp.sec);

// 【修改后】强制使用电脑当前 ROS 时间
scan_msg->header.stamp = node->get_clock()->now();
```
*修改后同样需要重新编译：`colcon build --packages-select ydlidar_ros2`*

### 第二步：配置 SLAM 宽容模式（解决红黑分离与卡死）

修改 `slam_toolbox` 的配置文件，降低对里程计精度的要求，并强制使用 `Best Effort` 接收数据。

**修改文件**：`src/fishbot_bringup/config/slam_toolbox.yaml`

```yaml
slam_toolbox:
  ros__parameters:
    # 1. 解决 QoS 不匹配，确保能收到雷达数据
    scan_qos_reliability: "best_effort"
    
    # 2. 提高地图更新频率，避免 Rviz2 看起来像卡死
    map_update_interval: 0.5
    
    # 3. 增加 TF 容忍度
    transform_timeout: 0.5
    tf_buffer_duration: 30.0
    
    # 4. 【关键】拯救烂里程计的“宽容参数”
    # 降低匹配最低得分要求（默认 0.5 -> 0.1），哪怕拼得不准也先拼上
    link_match_minimum_response_fine: 0.1  
    
    # 扩大搜索范围，允许里程计有较大误差
    search_window_size: 40
    correlation_search_space_dimension: 0.5
    
    # 只要动一点点就尝试更新
    minimum_travel_distance: 0.1
    minimum_travel_heading: 0.1
```

### 第三步：正确的启动顺序（保证时间基准）

micro-ROS 的时间同步依赖于 Agent。为了获得最佳同步效果：

1.  **先启动电脑端 Agent**：
    ```bash
    ros2 launch fishbot_bringup bringup.launch.py
    ```
2.  **后重启小车**：
    按下 ESP32 开发板上的 **Reset (RST)** 键。
    *观察终端出现 `session established`，说明连接成功且时间已同步。*
3.  **最后启动 SLAM**：
    ```bash
    ros2 launch slam_toolbox online_async_launch.py \
        slam_params_file:=src/fishbot_bringup/config/slam_toolbox.yaml \
        use_sim_time:=False
    ```

---

## 4. 验证方法

1.  **看 Rviz2**：不再报红色 Error，地图随着小车移动实时扩张。
2.  **看 TF**：`odom` -> `base_link` -> `laser_link` 树状结构完整。
3.  **看话题**：`ros2 topic hz /scan` 稳定（约 5-10Hz），`ros2 topic bw /map` 有数据量。

## 5. 附录：Rviz2 显示 Bug
如果 Rviz2 报错 `GLSL link result : active samplers...`，这是 Intel 核显驱动问题，**忽略即可**，不影响建图功能。如果地图不显示，尝试在 Rviz2 中将 Map 插件的 `Reliability Policy` 也改为 `Best Effort`。