forked from zbw/yiliao2026
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.
|
||||
Reference in New Issue
Block a user