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

docs: design odometry-only obstacle nav2 package

Browse Source
This commit is contained in:
Orange
2026-07-20 16:41:40 +08:00
parent f4f0e16f1f
commit a5ddfeb451
1 changed files with 37 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

37
docs/superpowers/specs/2026-07-20-obstacle-nav2-design.md Normal file
View File

@@ -0,0 +1,37 @@
# Obstacle Nav2 Design
## Goal
Add a ROS 2 package under `/home/sunrise/yiliao_ws/src/navigation` that converts `obstacle_scanner/msg/ObstacleArray` into a Nav2-compatible dynamic costmap and runs Nav2 Hybrid A* plus Ackermann MPPI without `slam_toolbox` or a static map.
## Architecture
The package will be named `obstacle_nav2`. Its `ObstacleArrayLayer` is a pluginlib costmap layer used by both the rolling global and local costmaps. It subscribes to `/obstacles`, transforms each obstacle from the message header frame (normally `laser_frame`) into the configured costmap global frame (`odom_combined`), and writes circular lethal regions into an internal layer grid. The layer clears its previous grid before applying each fresh obstacle set, so disappeared obstacles do not remain indefinitely.
Both costmaps use `odom_combined` as their global frame and rolling windows. No `map_server`, AMCL, or SLAM node is started. The global costmap is bounded by configurable width and height and is consumed by `nav2_smac_planner/SmacPlannerHybrid` using the `REEDS_SHEPP` motion model. The local controller is `nav2_mppi_controller::MPPIController` with `motion_model: Ackermann`; Nav2 emits `Twist(v, omega)` and exactly one existing base adapter converts it to Ackermann speed and steering.
The launch interface reserves `use_static_map` and `map_yaml` arguments and keeps `global_frame` configurable. They default to the odometry-only mode and are not used to start a map stack yet, allowing a later map/AMCL launch extension without changing the obstacle layer or planner/controller configuration.
## Components
* `src/obstacle_array_layer.cpp` and header: costmap plugin, TF conversion, obstacle rasterization, timeout handling, and plugin export.
* `test/test_obstacle_array_layer.cpp`: deterministic tests for circle rasterization, stale obstacle clearing, and message-frame filtering.
* `config/nav2_params.yaml`: rolling costmaps, custom layer, inflation, Smac Hybrid A*, MPPI Ackermann, lifecycle and BT parameters.
* `launch/obstacle_nav2.launch.py`: starts the existing base bringup and obstacle scanner optionally, then Nav2 navigation without map or SLAM nodes.
* `behavior_tree/nav_to_pose_ackermann.xml`: Nav2 recovery tree without in-place spin recovery.
## Data Flow
`/obstacles (ObstacleArray)` -> TF transform -> `ObstacleArrayLayer` -> rolling costmap -> Smac Hybrid A* -> MPPI Ackermann -> `/cmd_vel` -> one base conversion -> Ackermann drive.
## Safety and Failure Handling
* Messages with missing or unavailable TF are skipped and throttled in logs.
* A configurable `obstacle_timeout` clears all dynamic obstacles when no fresh message arrives.
* Invalid, non-finite, or non-positive radii are replaced with `default_obstacle_radius` and clamped to configured limits.
* Obstacles outside the rolling window are ignored; the planner treats outside-window space as unavailable through costmap bounds.
* The launch defaults to `enable_motion:=false` so planning can be inspected before driving.
## Verification
Unit tests run without ROS graph activity and validate the rasterization helpers. The package is built with `colcon build --packages-select obstacle_nav2`. A launch smoke test checks that the pluginlib class can be loaded and that `/global_costmap/costmap` and `/local_costmap/costmap` are published after lifecycle activation. Runtime verification uses a synthetic `/obstacles` publisher and confirms lethal cells appear and disappear after timeout.