# Obstacle Nav2 Implementation Plan

> **For agentic workers:** REQUIRED SUB-SKILL: Use superpowers:subagent-driven-development (recommended) or superpowers:executing-plans to implement this plan task-by-task. Steps use checkbox (`- [ ]`) syntax for tracking.

**Goal:** Build an odometry-only Nav2 package with a TF-aware `/obstacles` costmap layer, Hybrid A* planning, and Ackermann MPPI execution.

**Architecture:** A pluginlib `CostmapLayer` maintains a rolling dynamic obstacle grid in `odom_combined`; Nav2's Smac Hybrid planner and MPPI Ackermann controller consume the rolling costmaps. The launch file excludes map, AMCL, and SLAM while exposing future `use_static_map`, `map_yaml`, and `global_frame` arguments.

**Tech Stack:** ROS 2 Humble, C++17, `nav2_costmap_2d`, `pluginlib`, `tf2_ros`, `obstacle_scanner` messages, Nav2 Smac Planner, Nav2 MPPI Controller, Python launch.

---

## Progress at handoff (2026-07-20)

- [x] Inspected `obstacle_scanner` message definitions and publisher implementation.
- [x] Inspected `gc_navigation2_real` Hybrid A*, MPPI, costmap, BT, and launch configuration.
- [x] Confirmed the target architecture with the user: no map initially, rolling odometry-frame costmaps, future map hook reserved.
- [x] Wrote and committed the design document as commit `a5ddfeb`.
- [ ] No `obstacle_nav2` package files have been created yet.
- [ ] No tests have been written or run yet.
- [ ] No build or runtime verification has been performed yet.

Continue at Task 1. Follow test-driven development: add the focused failing test first, run it and record the expected failure, then add production code.

### Task 1: Package and test scaffold

**Files:**
- Create: `/home/sunrise/yiliao_ws/src/navigation/obstacle_nav2/package.xml`
- Create: `/home/sunrise/yiliao_ws/src/navigation/obstacle_nav2/CMakeLists.txt`
- Create: `/home/sunrise/yiliao_ws/src/navigation/obstacle_nav2/include/obstacle_nav2/obstacle_array_layer.hpp`
- Create: `/home/sunrise/yiliao_ws/src/navigation/obstacle_nav2/src/obstacle_array_layer.cpp`
- Create: `/home/sunrise/yiliao_ws/src/navigation/obstacle_nav2/test/test_obstacle_array_layer.cpp`

- [ ] Add package dependencies for `rclcpp`, `nav2_costmap_2d`, `pluginlib`, `tf2`, `tf2_ros`, `tf2_geometry_msgs`, `obstacle_scanner`, `geometry_msgs`, and `nav_msgs`.
- [ ] Add a gtest target and plugin export, without implementing behavior yet.
- [ ] Write tests for a circle's lethal-cell rasterization, previous-grid clearing, and invalid-radius fallback.
- [ ] Run the focused test target and confirm it fails because the helper implementation is absent.

### Task 2: Implement the dynamic obstacle layer

**Files:**
- Modify: `/home/sunrise/yiliao_ws/src/navigation/obstacle_nav2/include/obstacle_nav2/obstacle_array_layer.hpp`
- Modify: `/home/sunrise/yiliao_ws/src/navigation/obstacle_nav2/src/obstacle_array_layer.cpp`

- [ ] Implement `onInitialize`, parameter declarations, `/obstacles` subscription, TF buffer/listener, and lifecycle-safe shutdown.
- [ ] Transform obstacle centers to `global_frame`, rasterize each circle into the internal costmap, and publish bounds covering previous and current obstacle sets.
- [ ] Clear the internal layer on every accepted message and on timeout; merge with the master using overwrite so stale cells are removed.
- [ ] Run the focused tests and confirm they pass.

### Task 3: Configure Nav2 for odometry-only Ackermann navigation

**Files:**
- Create: `/home/sunrise/yiliao_ws/src/navigation/obstacle_nav2/config/nav2_params.yaml`
- Create: `/home/sunrise/yiliao_ws/src/navigation/obstacle_nav2/behavior_tree/nav_to_pose_ackermann.xml`

- [ ] Configure rolling global/local costmaps in `odom_combined`, the custom obstacle layer, and inflation.
- [ ] Configure `SmacPlannerHybrid` with `REEDS_SHEPP`, minimum turning radius `0.40`, and unknown/outside handling suitable for a bounded rolling window.
- [ ] Configure MPPI with `motion_model: Ackermann`, no in-place spin behavior, and the existing robot footprint.
- [ ] Add lifecycle, BT navigator, planner, controller, smoother, behavior, and waypoint follower parameters.

### Task 4: Add launch and future map hook

**Files:**
- Create: `/home/sunrise/yiliao_ws/src/navigation/obstacle_nav2/launch/obstacle_nav2.launch.py`

- [ ] Add launch arguments `global_frame`, `use_static_map`, `map_yaml`, `enable_motion`, and `start_obstacle_scanner`.
- [ ] Start the existing robot base bringup, lidar, obstacle scanner, and Nav2 `navigation_launch.py`; do not start map server, AMCL, or slam toolbox in the default path.
- [ ] Pass `global_frame` and motion enable parameters into Nav2 and document that `use_static_map/map_yaml` are reserved for a future map-enabled launch extension.

### Task 5: Build and runtime verification

- [ ] Build `obstacle_scanner` and `obstacle_nav2` on the RDK with ROS 2 Humble sourced.
- [ ] Use `pluginlib`/Nav2 startup logs to verify the layer loads and costmap topics publish.
- [ ] Publish a synthetic `ObstacleArray` and verify a lethal obstacle appears in the costmap, then disappears after timeout.
- [ ] Confirm no `slam_toolbox`, map server, or AMCL process is launched by the default launch.
