yiliao2026/docs/superpowers/plans/2026-07-20-obstacle-nav2.md

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)

• Inspected obstacle_scanner message definitions and publisher implementation.
• Inspected gc_navigation2_real Hybrid A*, MPPI, costmap, BT, and launch configuration.
• Confirmed the target architecture with the user: no map initially, rolling odometry-frame costmaps, future map hook reserved.
• 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.