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

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:

float64 center_x
float64 center_y
float64 radius

msg/ObstacleArray.msg:

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:

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

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:

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:

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:

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

Inspect required interfaces before launch:

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:

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.