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.

3.4 KiB Raw Blame History