diff --git a/docs/superpowers/handoffs/2026-07-20-obstacle-nav2-handoff.md b/docs/superpowers/handoffs/2026-07-20-obstacle-nav2-handoff.md new file mode 100644 index 0000000..a4d1892 --- /dev/null +++ b/docs/superpowers/handoffs/2026-07-20-obstacle-nav2-handoff.md @@ -0,0 +1,273 @@ +# 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. diff --git a/docs/superpowers/plans/2026-07-20-obstacle-nav2.md b/docs/superpowers/plans/2026-07-20-obstacle-nav2.md new file mode 100644 index 0000000..ce65194 --- /dev/null +++ b/docs/superpowers/plans/2026-07-20-obstacle-nav2.md @@ -0,0 +1,75 @@ +# 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.