docs: add obstacle nav2 implementation handoff
This commit is contained in:
273
docs/superpowers/handoffs/2026-07-20-obstacle-nav2-handoff.md
Normal file
273
docs/superpowers/handoffs/2026-07-20-obstacle-nav2-handoff.md
Normal file
@@ -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.
|
||||||
75
docs/superpowers/plans/2026-07-20-obstacle-nav2.md
Normal file
75
docs/superpowers/plans/2026-07-20-obstacle-nav2.md
Normal file
@@ -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.
|
||||||
Reference in New Issue
Block a user