# Obstacle Nav2 Continuation Handoff

## User request

On the RDKx5 robot, add a ROS 2 package under `/home/sunrise/yiliao_ws/src/navigation` that:

1. Converts `obstacle_scanner` topic `/obstacles` into a Nav2-compatible costmap.
2. Uses that costmap with Nav2 Hybrid A* planning and a Nav2 executor/controller, following the existing `gc_navigation2_real` package style.
3. Does not use `slam_toolbox`.

The user later selected **no static map initially** and asked to reserve an interface for future map support. The user also confirmed the physical chassis is Ackermann even though Nav2 outputs `geometry_msgs/Twist` on `/cmd_vel`.

## Remote environment

- Host: `sunrise@192.168.10.210`
- OS/architecture: Ubuntu 22.04 ARM64 on RDKx5
- ROS: ROS 2 Humble
- Workspace: `/home/sunrise/yiliao_ws`
- Target package path: `/home/sunrise/yiliao_ws/src/navigation/obstacle_nav2`
- Source ROS before commands: `source /opt/ros/humble/setup.bash`
- Source overlay when needed: `source /home/sunrise/yiliao_ws/install/setup.bash`

Use SSH carefully from Windows PowerShell. Read `rdkx5-access` and `windows-ssh-escaping` skills before remote operations.

## Current state

No implementation package exists yet. Do not claim that `obstacle_nav2` has been created, built, or tested.

Completed work:

- Read and analyzed the relevant remote packages.
- Wrote the design document at `docs/superpowers/specs/2026-07-20-obstacle-nav2-design.md`.
- Committed only that design document in remote commit `a5ddfeb` with message `docs: design odometry-only obstacle nav2 package`.
- Wrote the implementation plan at `docs/superpowers/plans/2026-07-20-obstacle-nav2.md`.

Implementation, unit tests, builds, launch tests, and robot motion tests are all pending.

## Important source findings

### `/obstacles` interface

Package: `/home/sunrise/yiliao_ws/src/obstacle_scanner`

`msg/Obstacle.msg`:

```text
float64 center_x
float64 center_y
float64 radius
```

`msg/ObstacleArray.msg`:

```text
std_msgs/Header header
Obstacle[] obstacles
```

The publisher is in `src/obstacle_scanner/src/obstacle_scanner_node.cpp`.

- Topic: `/obstacles`
- Type: `obstacle_scanner/msg/ObstacleArray`
- Default message frame: `laser_frame` (parameter `frame_id`)
- Message timestamp is copied from `/scan`.
- Centers are computed in the laser scan XY plane.
- When inspected, `/obstacles` was not active and the installed overlay did not expose the topic. Build `obstacle_scanner` before testing the new package.

### Existing Nav2 reference

Reference package: `/home/sunrise/yiliao_ws/src/navigation/gc_navigation2_real`

Useful files:

- `params/gc_navigation_slam_real.yaml`
- `behavior_tree/nav_to_pose_ackermann.xml`
- `launch/real_nav2_slam.launch.py`
- `launch/real_bringup.launch.py`

Relevant existing settings:

- Planner plugin: `nav2_smac_planner/SmacPlannerHybrid`
- Search model: `REEDS_SHEPP`
- Minimum turning radius: `0.40`
- Controller: `nav2_mppi_controller::MPPIController`
- MPPI motion model: `Ackermann`
- Robot footprint: `[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]`
- Base frame: `base_footprint`
- Odometry: `/odom_combined`
- Existing BT avoids relying on in-place spin for Ackermann recovery.

Do not copy the existing hard-coded BT path `/home/guoch/test_ws/...`; use the new package share path.

### Command conversion decision

Planning and control must use Ackermann kinematics. Nav2 may still output `Twist(v, omega)`.

Existing converter:

`/home/sunrise/yiliao_ws/src/origincar_base/scripts/cmd_vel_to_ackermann_drive.py`

It computes:

```text
steering = atan(wheelbase * omega / velocity)
```

with hard-coded `wheelbase = 0.143` and publishes `/ackermann_cmd`.

There are also direct `/cmd_vel` and `/ackermann_cmd` callbacks in `origincar_base.cpp`. Exactly one conversion path must be active. Do not launch both a direct firmware conversion and the Python converter without checking the actual bottom-level interface. Default the new navigation launch to motion disabled until the command graph is verified.

At minimum turning radius `0.40 m`, the nominal steering angle is `atan(0.143 / 0.40) = 0.343 rad` (about 19.7 degrees).

## Approved design

Package name: `obstacle_nav2`

Default mode:

- No static map.
- No `map_server`.
- No AMCL.
- No `slam_toolbox`.
- Global frame: `odom_combined`.
- Both global and local costmaps are rolling windows.
- Custom plugin `ObstacleArrayLayer` subscribes to `/obstacles`.
- Each obstacle center is transformed from `msg.header.frame_id` into the costmap global frame.
- The obstacle radius is rasterized as lethal cells; Nav2 inflation adds clearance.
- Old obstacle cells are cleared on each accepted update and after an obstacle-message timeout.
- Global planner is Smac Hybrid A* using Reeds-Shepp primitives.
- Controller is MPPI using Ackermann motion model.
- No in-place Spin recovery.
- Motion is disabled by default for the first launch validation.

Reserved map interface:

- Launch argument `use_static_map`, default `false`.
- Launch argument `map_yaml`, default empty.
- Launch/config argument `global_frame`, default `odom_combined`.
- The first implementation must not start map services when `use_static_map=false`.
- Full map/AMCL behavior may be implemented later without changing `ObstacleArrayLayer`.

## Intended package files

```text
src/navigation/obstacle_nav2/
  CMakeLists.txt
  package.xml
  obstacle_nav2_plugins.xml
  include/obstacle_nav2/obstacle_array_layer.hpp
  src/obstacle_array_layer.cpp
  test/test_obstacle_array_layer.cpp
  config/nav2_params.yaml
  behavior_tree/nav_to_pose_ackermann.xml
  launch/obstacle_nav2.launch.py
  README.md
```

## Implementation order

1. Create only the package metadata, header API, and focused gtest.
2. Add tests for circle rasterization, invalid-radius fallback, grid clearing, and stale-data clearing.
3. Build/run the focused test and verify a genuine RED failure caused by missing behavior.
4. Implement the smallest costmap helper and plugin behavior needed for GREEN.
5. Add pluginlib export and confirm the shared library loads.
6. Add odometry-only Nav2 parameters.
7. Add Ackermann BT and launch file with future map arguments.
8. Build `obstacle_scanner` and `obstacle_nav2`.
9. Start with motion disabled; verify topics, TF, lifecycle nodes, plugin loading, costmap marking, and timeout clearing.
10. Check that only one node publishes the effective chassis command before enabling motion.

## Proposed plugin behavior

Derive `ObstacleArrayLayer` from `nav2_costmap_2d::CostmapLayer`.

Suggested parameters under each plugin namespace:

```yaml
enabled: true
topic: /obstacles
obstacle_timeout: 0.5
transform_tolerance: 0.2
default_obstacle_radius: 0.05
minimum_obstacle_radius: 0.02
maximum_obstacle_radius: 0.50
extra_inflation: 0.02
```

Use an internal costmap. On an accepted message:

1. Remember the previous occupied bounds.
2. Reset the internal map to `FREE_SPACE`.
3. Transform obstacle centers to the layer global frame.
4. Clamp/fallback each radius and mark all cell centers inside `radius + extra_inflation` as `LETHAL_OBSTACLE`.
5. Expand update bounds over both previous and current obstacle extents.
6. Merge the layer with the master using overwrite semantics so disappeared obstacles are actually cleared before the downstream InflationLayer runs.

On timeout, clear the layer and include previous occupied bounds in the next update. Throttle TF warnings. Ignore non-finite centers and obstacles outside the rolling costmap.

## Planned Nav2 parameters

- `global_costmap.global_frame: odom_combined`
- `global_costmap.rolling_window: true`
- Suggested global window: 10 m x 10 m at 0.05 m resolution.
- `local_costmap.global_frame: odom_combined`
- Suggested local window: 3 m x 3 m at 0.05 m resolution.
- Plugins in both: `obstacle_array_layer`, then `inflation_layer`.
- No `static_layer` in default mode.
- `track_unknown_space: false` so the empty rolling window is traversable.
- `SmacPlannerHybrid.motion_model_for_search: REEDS_SHEPP`.
- `SmacPlannerHybrid.minimum_turning_radius: 0.40`.
- `MPPIController.motion_model: Ackermann`.
- `AckermannConstraints.min_turning_r: 0.40`.
- Use `odom_topic: /odom_combined` and `robot_base_frame: base_footprint`.

## Build and verification commands

Build only scoped packages first:

```bash
cd /home/sunrise/yiliao_ws
source /opt/ros/humble/setup.bash
colcon build --symlink-install --packages-select obstacle_scanner obstacle_nav2
source install/setup.bash
```

Run tests:

```bash
colcon test --packages-select obstacle_nav2 --event-handlers console_direct+
colcon test-result --verbose
```

Inspect required interfaces before launch:

```bash
ros2 interface show obstacle_scanner/msg/ObstacleArray
ros2 run tf2_ros tf2_echo odom_combined laser_frame
ros2 topic info /cmd_vel -v
ros2 topic info /ackermann_cmd -v
```

The first runtime test must use motion disabled. Verify:

```bash
ros2 node list
ros2 topic list
ros2 topic echo /global_costmap/costmap --once
ros2 topic echo /local_costmap/costmap --once
```

Confirm the default launch does not contain `/slam_toolbox`, `/map_server`, or `/amcl` nodes.

## Worktree warning

The remote Git worktree was already heavily dirty before this task. It contains many pre-existing deletions under old navigation package paths, modifications under `origincar_description` and `planner`, and untracked `src/navigation/` content. These belong to the user.

Rules for continuation:

- Never reset, checkout, clean, or revert the remote worktree.
- Never stage all changes with `git add .` or `git add -A`.
- Restrict edits and staging to `src/navigation/obstacle_nav2` and the three `docs/superpowers/...obstacle-nav2...md` files.
- The new package path did not exist when this handoff was written.
- The dependency `obstacle_scanner` itself contains current source that may not yet be reflected in the installed overlay; do not replace it with the committed version.

## Relevant documents

- Design: `/home/sunrise/yiliao_ws/docs/superpowers/specs/2026-07-20-obstacle-nav2-design.md`
- Plan: `/home/sunrise/yiliao_ws/docs/superpowers/plans/2026-07-20-obstacle-nav2.md`
- Handoff: `/home/sunrise/yiliao_ws/docs/superpowers/handoffs/2026-07-20-obstacle-nav2-handoff.md`

## Resume instruction

Read the design, plan, and this handoff. Re-inspect the current remote Git status because the user may have changed files since this snapshot. Then continue at **Task 1** using TDD. No implementation existed at the snapshot point.