How gaaldod/ros2_hybrid_astar_nav2 works
1. What this repository is
- hybrid_astar_planner is a ROS 2 Humble package that integrates a Python Hybrid A* global planner with Nav2, a delivery/safety layer, and Gazebo Fortress simulation for an Ackermann-style robot.
- The repo is organized around one end-to-end runtime pipeline: simulated sensors and localization in, planning and recovery logic in the middle, and a gated velocity command stream out to Gazebo.
2. Core runtime boundary
- The architecture is ROS-centric: nodes communicate through topics, actions, TF, and a few service-style compatibility hooks.
- Nav2 remains the high-level navigation framework, while this repo supplies custom planning, safety arbitration, obstacle tracking, and relocalization behavior around it.
3. Global planning subsystem
- hybrid_astar.py contains the planning algorithm itself.
- planner_node.py and nav2_hybrid_astar_server.py expose that planner to the Nav2 action flow and manage planner-specific runtime behavior such as live overlays, localization-jump guarding, and local takeover logic.
- This subsystem is the main “brains” for route generation in map coordinates.
4. Perception and obstacle abstraction
- sensor_processor_node.py filters raw lidar input from /scan into /scan_filtered.
- obstacle_tracker_node.py turns sensed obstacles into tracked obstacles in the map frame, publishing both a structured obstacle stream and tracked_obstacles_cloud.
- grid_map.py and types.py support the internal representation of occupancy, obstacle geometry, and planner-facing state.
5. Local supervision and command shaping
- local_planner_node.py sits between perception and safety, using obstacle information and speed scaling to determine when navigation should slow down or stop.
- It also asserts nav_emergency_stop, which is one of the upstream stop conditions for the final command gate.
- This is the layer that translates “the path is becoming unsafe” into immediate operational constraints.
6. Final safety gate
- ackermann_safety_controller_node.py is the last authority before motion commands reach the robot/simulator.
- It takes cmd_vel_nav from Nav2/controller outputs and publishes cmd_vel_safe after applying emergency-stop arbitration, reverse/recovery handling, and front-collision protection.
- In this repo, this node is the critical boundary between navigation intent and actual vehicle motion.
7. Recovery and replanning support
- reverse_recovery_node.py handles reverse, stall, and collision-hint recovery behaviors.
- replan_watchdog_node.py provides optional periodic or stall-triggered replanning hooks.
- motion_reason_listener.py appears to be a diagnostic/observer node for understanding why motion decisions were made, which helps explain planner-to-recovery transitions during runtime.
8. Localization and relocalization controls
- anchor_frame_guard_node.py watches for localization drift, can trigger /initialpose reseeding, and may republish the cached goal after reseed.
- initial_pose_seed_node.py provides initial pose bootstrapping.
- odom_tf_bridge_node.py helps bridge odometry into TF, while tf_event_logger_node.py records TF/clock/AMCL discontinuities for debugging localization jumps.
- These nodes form the repo’s “keep localization sane” subsystem.
9. Simulation and environment integration
- The primary simulation path is Gazebo Fortress, with world and model assets under hybrid_astar_planner/sim.
- sim/worlds/warehouse_lightweight_gz.sdf and related models define the robot, warehouse, and moving obstacle environment.
- sim/bridge_minimal.yaml bridges ROS topics to Gazebo transport, especially for the moving obstacle’s velocity and odometry.
- moving_obstacle_driver_node.py drives the simulated box obstacle by publishing velocity commands and reversing direction based on its odometry.
10. Nav2 compatibility and launch composition
- global_costmap_compat_node.py exists as a no-op compatibility service for Nav2 behavior-tree expectations.
- The launch files in launch/ are the real composition layer: they assemble Gazebo, Nav2, the Hybrid A* server, the safety stack, obstacle driver, RViz, and delivery-specific nodes.
- The current “source of truth” runtime is the Fortress + RPP path, especially gz_modern_nav2_corner_test_withRPP.launch.py and delivery_stack.launch.py.
11. Maps, visualization, and operational tooling
- maps/ contains the static warehouse map assets used by Nav2 and the planner.
- rviz/ provides preconfigured visualization layouts for planner and delivery debugging.
- scripts/ contains operator-facing helpers for goal sending, debug capture, self-tests, timing inspection, and start/stop orchestration.
- These assets don’t change the architecture, but they define how the system is exercised and observed.
12. Architectural data flow summary
- The main flow is: simulated lidar/odometry → scan filtering → obstacle tracking → planner/global route generation → controller output → safety gating → Gazebo motion.
- A parallel control loop monitors TF/AMCL stability; if localization drifts, the anchor guard can stop motion, reseed pose, and restore the goal context.
- The result is a layered navigation stack where planning, safety, recovery, and simulation transport are intentionally separated.
Main entry points: planner_node.py Planner Node · obstacle_tracker_node.py Obstacle Tracker · nav2_hybrid_astar_bringup.launch.py Nav2 Bringup · hybrid_astar.py Hybrid A*
Components 23
Planning
Global route generation and Nav2 planner integration.
- Hybrid A* · Search and trajectory generation in map coordinates.
hybrid_astar_planner/hybrid_astar_planner/hybrid_astar.py - Planner Node · Wraps the planner for runtime use and overlays/localization-jump handling.
hybrid_astar_planner/hybrid_astar_planner/planner_node.py - Nav2 Server · Exposes Hybrid A* through the Nav2 action/server flow.
hybrid_astar_planner/hybrid_astar_planner/nav2_hybrid_astar_server.py - Compat Hook · No-op compatibility service for Nav2 behavior-tree expectations.
hybrid_astar_planner/hybrid_astar_planner/global_costmap_compat_node.py
Perception
Scan filtering, obstacle tracking, and internal map state.
- Scan Filter · Filters raw lidar scans into a cleaner scan stream.
hybrid_astar_planner/hybrid_astar_planner/sensor_processor_node.py - Obstacle Tracker · Tracks obstacles in map coordinates and publishes obstacle outputs.
hybrid_astar_planner/hybrid_astar_planner/obstacle_tracker_node.py - Grid Map · Internal occupancy and obstacle-space representation used by the planner stack.
hybrid_astar_planner/hybrid_astar_planner/grid_map.py - Planner Types · Shared planner-facing types and geometry/state definitions.
hybrid_astar_planner/hybrid_astar_planner/types.py
Supervision
Local slowing, recovery, replanning, and command gating.
- Local Planner · Turns obstacle and speed context into slow/stop constraints and emergency-stop requests.
hybrid_astar_planner/hybrid_astar_planner/local_planner_node.py - Safety Gate · Last authority before motion commands; applies emergency-stop and collision protection.
hybrid_astar_planner/hybrid_astar_planner/ackermann_safety_controller_node.py - Reverse Recovery · Handles reverse, stall, and collision-hint recovery behavior.
hybrid_astar_planner/hybrid_astar_planner/reverse_recovery_node.py - Replan Watchdog · Triggers periodic or stall-based replanning hooks.
hybrid_astar_planner/hybrid_astar_planner/replan_watchdog_node.py - Motion Observer · Observes and reports reasons behind motion decisions.
hybrid_astar_planner/hybrid_astar_planner/motion_reason_listener.py
Localization
Pose, TF, and relocalization safeguards.
- Anchor Guard · Detects localization drift and can reseed pose while preserving goal context.
hybrid_astar_planner/hybrid_astar_planner/anchor_frame_guard_node.py - Pose Seed · Bootstraps the robot's initial pose.
hybrid_astar_planner/hybrid_astar_planner/initial_pose_seed_node.py - Odom TF Bridge · Bridges odometry into TF for the navigation stack.
hybrid_astar_planner/hybrid_astar_planner/odom_tf_bridge_node.py - TF Logger · Records TF, clock, and AMCL discontinuities for debugging.
hybrid_astar_planner/hybrid_astar_planner/tf_event_logger_node.py
Simulation
Gazebo worlds, models, and bridge configuration.
- Moving Obstacle · Drives the simulated box obstacle with motion and reversal behavior.
hybrid_astar_planner/hybrid_astar_planner/moving_obstacle_driver_node.py - Warehouse World · Gazebo Fortress warehouse world and SDF environment.
hybrid_astar_planner/sim/worlds/warehouse_lightweight_gz.sdf - ROS Bridge · ROS-Gazebo topic bridge configuration for simulated transport.
hybrid_astar_planner/sim/bridge_minimal.yaml
Launch
Composition entrypoints for different runtime setups.
- Delivery Stack · Composes Gazebo, Nav2, planner, safety, and delivery runtime nodes.
hybrid_astar_planner/launch/delivery_stack.launch.py - Fortress RPP · Primary modern Gazebo/Nav2 runtime composition for corner testing with RPP.
hybrid_astar_planner/launch/gz_modern_nav2_corner_test_withRPP.launch.py - Nav2 Bringup · Brings up Nav2 with the Hybrid A* planner integration.
hybrid_astar_planner/launch/nav2_hybrid_astar_bringup.launch.py
Connections 24
- Scan Filter → Obstacle Tracker: filtered scans · Cleaner lidar data feeds obstacle extraction.
- Obstacle Tracker → Grid Map: tracks into map · Tracked obstacles are represented in planner space.
- Grid Map → Hybrid A*: search space · Occupancy and obstacle state constrain path generation.
- Planner Types → Hybrid A*: state model · Shared geometry and planner state definitions are consumed by the planner core.
- Hybrid A* → Planner Node: planned path · Planner core is wrapped by runtime orchestration.
- Planner Node → Nav2 Server: Nav2 action · Planner node is exposed through the Nav2 server interface.
- Compat Hook → Nav2 Server: compat hook · Provides Nav2 expectation compatibility.
- Obstacle Tracker → Local Planner: obstacles · Tracked obstacles inform local slowdown and stop decisions.
- Local Planner → Safety Gate: stop hints · Local supervision contributes emergency-stop conditions to the final gate.
- Reverse Recovery → Safety Gate: recovery state · Recovery behavior affects final command arbitration.
- Replan Watchdog → Planner Node: replan trigger · Watchdog can force replanning when progress stalls.
- Motion Observer → Planner Node: observes · Diagnostic observer watches planner and motion decisions.
- Anchor Guard → Pose Seed: reseed · Localization drift handling can trigger pose reseeding.
- Pose Seed → Odom TF Bridge: pose boot · Initial pose bootstraps the TF/odom navigation context.
- Odom TF Bridge → TF Logger: tf events · TF continuity and discontinuities are logged for diagnosis.
- TF Logger → Anchor Guard: drift signal · Logged TF/AMCL anomalies support guard decisions.
- Warehouse World → Moving Obstacle: sim obstacle · Warehouse world contains the moving obstacle runtime.
- ROS Bridge → Moving Obstacle: transport · Bridge connects the driver to Gazebo transport and ROS topics.
- Delivery Stack → Warehouse World: includes · Delivery stack composes the simulation environment.
- Delivery Stack → Nav2 Bringup: includes · Launch composition pulls in Nav2 and planner bringup.
- Fortress RPP → Safety Gate: starts · Modern test launch includes the final motion safety gate.
- Fortress RPP → Planner Node: starts · Modern test launch includes the Hybrid A* runtime.
- Nav2 Bringup → Nav2 Server: starts · Bringup launches the planner-facing Nav2 server.
- Nav2 Bringup → Compat Hook: starts · Bringup includes Nav2 compatibility glue.