## 補充：如何確認 RViz 的 2D Pose Estimate 與 2D Goal Pose 是否成功
### 1. 確認 2D Pose Estimate 是否成功送出初始定位
在 RViz 中使用 **2D Pose Estimate** 設定 AMCL 初始位置時，可以開另一個 terminal 檢查 `/initialpose` 是否有訊息發布：
```bash
ros2 topic echo /initialpose
接著在 RViz 上方工具列點選 2D Pose Estimate，在地圖上點選機器人目前位置，並拖曳出機器人面向方向。如果 terminal 中有輸出，代表 RViz 已成功發布初始位姿給 AMCL：
RViz 2D Pose Estimate → /initialpose → AMCL
這表示 AMCL 已收到機器人在 map 座標系中的初始位置與方向。
2. 確認 2D Goal Pose 是否成功觸發導航
在送出導航目標時，也可以檢查 Nav2 是否有產生速度命令。
先確認 relay 已經執行：
ros2 run topic_tools relay /cmd_vel_nav /cmd_vel
接著可開另一個 terminal 檢查 Nav2 輸出的速度命令：
ros2 topic echo /cmd_vel_nav
或檢查 relay 後實際送給 Stage robot 的速度命令：
ros2 topic echo /cmd_vel
然後在 RViz 中點選 2D Goal Pose，在地圖上指定導航目標位置與方向。如果 /cmd_vel_nav 有輸出，代表 Nav2 已收到 goal，並開始由 controller 產生速度命令：
RViz 2D Goal Pose → Nav2 → /cmd_vel_nav
如果 /cmd_vel 也有輸出，代表 relay 已成功將 /cmd_vel_nav 轉發到 /cmd_vel：
/cmd_vel_nav → topic_tools relay → /cmd_vel
3. 正常流程判斷
因此，若 /initialpose、/cmd_vel_nav、/cmd_vel 都有輸出，代表以下流程大致正常：
RViz 初始定位 → AMCL
RViz 送出導航目標 → Nav2
Nav2 產生速度命令 → relay → Stage robot
4. 簡單判斷表
| 檢查項目 | 判斷結果 |
| ------------------ | -------------------------------------- |
| /initialpose 有輸出 | 表示 2D Pose Estimate 成功送出初始定位 |
| /cmd_vel_nav 有輸出 | 表示 Nav2 成功收到導航目標，並產生速度命令 |
| /cmd_vel 有輸出 | 表示 relay 成功把 Nav2 的速度命令轉發給 Stage robot |
5. 常見問題判斷
情況 1：/cmd_vel_nav有輸出，但/cmd_vel沒有輸出
通常表示 relay 沒有正確執行。
請重新確認是否已執行：
ros2 run topic_tools relay /cmd_vel_nav /cmd_vel
情況 2：/cmd_vel_nav沒有輸出
通常表示 Nav2 尚未成功開始導航，可能需要重新確認：
- AMCL 初始定位是否已設定
- goal 是否成功送出
- map 是否正常
- scan 是否正常
- costmap 是否正常