# 语义推理与导航

智能语义推理框架包含两个核心模块：  
语义目标标注：利用 VLM识别物体  
语义导航：机器人借助已保存的物体记忆，通过语义路标完成导航。  

语义标注模块将 Faster R-CNN 与 CLIP 结合，实现零样本目标检测。  

给定一个物体列表（例如：["Refrigerator", "water dispenser", "sofa", "white toilet", "office chair with wheels"]），由 Faster R-CNN 从 RGB 图像帧中生成候选区域。

随后，使用 CLIP 将每个候选区域进行特征编码，并与文本标签做余弦相似度对比，为区域分配最可能的类别标签。

点击 RViz2 中Nav2 Goal，在地图上选择目标位置，调整箭头设置机器人到达后的朝向，确认后机器人将自主导航，在探索过程中，系统会结合 RGB-D 数据 记录物体的中心坐标，并将置信度分数与位置信息存入 JSON 文件。
只有当检测到更高置信度的结果时，才会更新原有记录。

## 1. 环境准备与编译
先点击平台左下角的“终端”，再点击“+”新建终端。在终端1执行下面的命令（使用 conda `ros2` 环境，**不要** `pip install --user` 到系统 Python）：

```bash
conda activate ros2
bash /workspace/bootstrap_ros2_conda_workspace.sh
```

该脚本会：在 conda `ros2` 中安装 PyTorch / OpenAI CLIP 等依赖、用 conda Python 编译工作区，并修正 `ros2 run` 入口脚本的 shebang。首次运行约需数分钟。

预计打印结果：
![图1](assets/picture01.png)

### 注意事项
之后每打开一个新终端，都建议先执行：

```bash
conda activate ros2
export PYTHONNOUSERSITE=1
source /opt/ros/humble/setup.bash
source /workspace/install/setup.bash
```

## 2. 启动仿真

终端1下继续执行启动命令：

```bash
./gzsim_run.sh
```
该脚本会启动仿真和websocket服务，并生成带时间记录的日志文件，预计打印结果：

![图2](assets/picture02.png)

脚本启动成功后，可以在右上角点击添加gazebo界面。等待刷新后，即可查看仿真画面：

![图3](assets/picture03.png)

![图4](assets/picture04.png)

同时可以在左侧点击“数据流”，并点击“refresh”查看当前的ros2话题：

![图5](assets/picture05.png)

## 3. 启动 AMCL节点（负责 “map_server + amcl”）
在终端中点击“+”，新建终端，并执行下面的命令：

![图6](assets/picture06.png)

```bash
conda activate ros2
export PYTHONNOUSERSITE=1
source /opt/ros/humble/setup.bash
source /workspace/install/setup.bash
export NAV2_MAP_PATH=/workspace/src/turtlebot3_simulations/turtlebot3_gazebo/map/office_map.yaml
ros2 launch nav2_rrtstar_planner bringup_localization_with_initial_pose.launch.py \
    use_sim_time:=true \
    map:=$NAV2_MAP_PATH \
    params_file:=/workspace/src/TurtleBot-RRT-Star/nav2_params.yaml
```
预计打印结果：

![图7](assets/picture07.png)

## 4. 启动 Nav2导航栈（负责 “RRT* 规划器 + controller + bt_navigator）
在终端中点击“+”，新建终端，并执行下面的命令：

![图8](assets/picture08.png)

```bash
conda activate ros2
export PYTHONNOUSERSITE=1
source /opt/ros/humble/setup.bash
source /workspace/install/setup.bash
ros2 launch nav2_bringup navigation_launch.py \
    use_sim_time:=True \
    params_file:=/workspace/src/TurtleBot-RRT-Star/nav2_params.yaml
```
预计打印结果：

![图9](assets/picture09.png)

## 5. 打开rviz
在“仿真器”界面下添加“远程桌面”界面：

![图10](assets/picture10.png)

“远程桌面”界面：

![图11](assets/picture11.png)

在终端中点击“+”，新建终端，并执行下面的命令：

![图12](assets/picture12.png)

```bash
ros2 run rviz2 rviz2 -d /opt/ros/humble/share/nav2_bringup/rviz/nav2_default_view.rviz --ros-args -p use_sim_time:=true
```
执行完后可以在“远程桌面”界面看到代价地图和路径规划等信息：

![图13](assets/picture13.png)

## 6. 启动目标检测节点
新开终端，执行环境生效后启动vlm_perception_pkg的语义检测节点，开始目标语义标注并构建语义记忆库，在机器人导航过程中，节点实时检测环境中的目标并生成语义标注结果，保存JSON 格式的语义记忆库到指定路径。

![图14](assets/picture14.png)

```bash
conda activate ros2
export PYTHONNOUSERSITE=1
source /opt/ros/humble/setup.bash
source /workspace/install/setup.bash
# 该命令启动时重置上次记录的语义记忆库
ros2 run vlm_perception_pkg vlm_detection --ros-args -p use_sim_time:=true -p reset_records_on_startup:=true
# ros2 run vlm_perception_pkg vlm_detection
# 不带参数的启动命令：启动时加载历史 JSON（同一个语义记忆库文件），分数高的记录会覆盖低分记录，适合连续测试时使用
```
启动后，在终端中可以看到小车相机拍摄的图像的检测结果：

![图15](assets/picture15.png)

开始检测后，可以在rviz中使用“Nav2 Goal”设置目标点，让小车在地图内跑动，小车会拍摄实时图像，并实时检测目标，并将检测结果保存到JSON文件中。

![图16](assets/picture16.png)

检测完成后可查看目标标注结果（in /workspace/src/vlm_nav_pkg/config/example_object_detection_vlm.json，带时间戳,可参考测试效果较好的test_vlm.json），后续语义导航功能会加载生成的语义记忆库文件，通过 CLIP 将语义标签编码为shared embedding space，当用户输入自然语言查询时，系统匹配最接近的已知目标，提取其坐标并发送至 Nav2，实现基于自然语言的语义导航。

![图17](assets/picture17.png)

### 导航建议
- TurtleBot3 机身较矮，相机视角较低，高目标物体适合较远距离观测。
- 建议在机器人低速或短暂停止时运行检测，以提高 RGB-D 与 TF 的稳定性。
- 建议目标距离相机约 1~3 米，可以多角度对目标进行观察。
- 本项目保存的是“最高分对应的目标坐标”，适合用于导航到目标附近，不保证是物体几何中心。

## 7. 启动语义导航节点
在终端5关闭检测节点，在终端中点击“+”，新建终端，并执行下面的命令：
```bash
# 关闭检测节点后
conda activate ros2
export PYTHONNOUSERSITE=1
source /opt/ros/humble/setup.bash
source /workspace/install/setup.bash
ros2 run vlm_nav_pkg semantic_nav
```

![图18](assets/picture18.png)

节点启动后，在终端输入自然语言语义目标（如 “water dispenser”,“sofa”,“refrigerator”,“white toilet”，“office chair with wheels”），系统将自动完成：语义查询与shared embedding space匹配，提取目标对应的地图坐标（检测节点保存的json文件）并向 Nav2 发送导航目标位姿，然后机器人自主规划路径并导航至语义目标位置，RViz2 中实时可视化规划路径与导航过程。当前导航会默认在目标周围生成多个候选接近点，并优先选择距离当前机器人最近的可行点，避免直接撞向物体中心点。候选点会先经过 `/map` 自由空间筛选，再逐个试算全局路径，只在“有路径”的候选点里选最优目标。
![图19](assets/picture19.png)