# 基于ROS2的无人小车自主探索与建图

本卡片让机器人在未知办公室环境中自主探索，并生成 2D 栅格地图。当机器人还不知道环境长什么样，无人小车通过本模块知道如何先把环境“走一遍、画出来”。本卡片依赖ROS2 Humble、Nav2、TurtleBot3 Gazebo、SLAM Toolbox 与 explore_lite 完成自主探索与地图保存。


## 1. 环境要求
- Ubuntu 22.04
- ROS2 Humble
- Gazebo Sim Harmonic
---

### 编译

```bash
cd /workspace
colcon build --symlink-install
source /opt/ros/humble/setup.bash
source install/setup.bash
```

### 确认 package 能被 ROS2 找到

```bash
ros2 pkg list | grep explore
```
预计打印结果：explore_lite

### 注意事项

之后每打开一个新终端，都建议先执行：

```bash
source /opt/ros/humble/setup.bash
source install/setup.bash
export TURTLEBOT3_MODEL=burger
```

为了方便，也可以写入 `~/.bashrc`：

```bash
echo 'source /opt/ros/humble/setup.bash' >> ~/.bashrc
echo 'source /workspace/install/setup.bash' >> ~/.bashrc
echo 'export TURTLEBOT3_MODEL=burger' >> ~/.bashrc
source ~/.bashrc
```

## 2. 启动仿真， Nav2导航栈， SLAM Toolbox异步建图， rviz2

```bash
./gzsim_run.sh

ros2 launch slam_toolbox online_async_launch.py use_sim_time:=True

ros2 launch nav2_bringup navigation_launch.py use_sim_time:=True headless:=False

ros2 run rviz2 rviz2 -d /opt/ros/humble/share/nav2_bringup/rviz/nav2_default_view.rviz --ros-args -p use_sim_time:=true

```

## 3. 启动explore_lite 自主探索节点（如果机器人在探索时在某一点长时间卡住，可以在rviz中使用Nav2 Goal进行单点导航探索。）
```bash
ros2 launch explore_lite explore.launch.py
```

![图1](assets/picture01.png)

## 4. 保存建图结果

机器人完成环境全区域探索后，新开终端执行地图保存命令，地图将保存至$NAV2_MAP_PATH指定路径

```bash
export NAV2_MAP_PATH=/workspace/src/turtlebot3_simulations/turtlebot3_gazebo/map
ros2 run nav2_map_server map_saver_cli -f ${NAV2_MAP_PATH}/office_map
```

## 5. 模型说明
该package 基于explore_lite进行 ROS2 适配与二次开发，是未知环境自主建图的核心，实现地图前沿检测、探索目标。m-explore-ros2 package 的src/目录下包含explore.cpp、costmap_client.cpp、frontier_search.cpp，分别实现探索节点主要逻辑、代价地图数据subscribe与同步， frontier_search的核心算法。

* costmap_client.cpp 管理代价地图的数据，核心class 是 explore::Costmap2DClient， 实现了代价地图订阅、格式转换、位姿计算的具体逻辑，为上层提供了线程安全的代价地图数据和机器人实时位姿。这层的作用：给探索模块提供“实时可查询的栅格地图 + 当前机器人位姿”。首先启动时订阅 map 和 map_updates，把 OccupancyGrid 转成 nav2_costmap_2d::Costmap2D（updateFullMap / updatePartialMap）。其次，getRobotPose() 通过 TF 把 robot_base_frame 转到地图坐标系，给后续前沿搜索当起点。

* frontier_search.cpp 实现了前沿检测与优先级排序算法，核心的class 是frontier_exploration::FrontierSearch， 实现了 BFS 前沿检测、前沿属性计算、成本排序的核心逻辑，接收 Costmap2DClient 提供的代价地图，输出按优先级排序的前沿列表。具体来说，searchFrom(position) 从机器人附近 free cell 开始做 BFS，然后把“未知栅格且4邻域有free”的格子判为 frontier cell（isNewFrontierCell），把“未知栅格且4邻域有free”的格子判为 frontier cell（isNewFrontierCell），用 8 邻域把相连 frontier cell 聚成一个 frontier（buildNewFrontier），并计算size（前沿大小）、centroid（几何中心）、min_distance（离机器人最近点距离）。最后按代价进行排序，cost = potential_scale * distance - gain_scale * size
即“近的更好、大的更好（代价更低）”。

* explore.cpp：探索节点主逻辑，核心的class是explore::Explore，节点继承自 rclcpp::Node，是 ROS2 探索节点的入口类，在主循环中实现了导航调用、状态控制的流程。具体来说，Explore 节点创建 Nav2 action client（navigate_to_pose），定时器周期调用 makePlan()。makePlan() 核心逻辑是：拿机器人位姿->调 search_.searchFrom() 找 frontiers->过滤 blacklist->选代价最低目标并发给 Nav2->若长时间没进展（progress_timeout），把目标加入 blacklist 并重规划。reachedGoal() 根据 action 结果更新。stop/resume 通过 explore/resume topic 控制暂停恢复。可选 return_to_init=true 时，探索结束回初始点。

* launch/explore.launch.py，启动的入口，声明并加载所有配置参数（ planner_frequency、potential_scale、min_frontier_size 等），直接传递给 Explore 节点的构造函数