cyy/yiliao2026
1
0
Fork 0
forked from zbw/yiliao2026
Code Pull Requests Activity

docs: add obstacle nav2 implementation handoff

Browse Source
This commit is contained in:
Orange
2026-07-20 16:51:11 +08:00
parent a5ddfeb451
commit 6c66789c78
2 changed files with 348 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

273
docs/superpowers/handoffs/2026-07-20-obstacle-nav2-handoff.md Normal file
View 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
View 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.