Multi-Robot Path Finding (MAPF) Algorithms

This directory contains the algorithmic core for a multi-robot pathfinding system. It implements a hybrid approach combining A* (A-Star) for optimal single-agent path planning and PIBT (Priority Inheritance with Backtracking) for coordinated, collision-free multi-agent navigation in crowded environments. The system exposes a gRPC service to communicate with the simulation environment.

🧠 Algorithmic Architecture

The system operates as a gRPC service (AlgoService) that receives state updates (robot positions, map data, tasks) and returns movement commands. It dynamically switches between solvers based on congestion metrics.

1. Hybrid Solver Strategy (algoService.py)

The system monitors the mean_cost (average estimated cost to reach destinations) and the rate of cost reduction.

• Low Congestion / Smooth Flow: Uses A* for optimal, independent paths.
• High Congestion / Deadlocks: Switches to PIBT to resolve conflicts and coordinate movements when agents are blocking each other.

2. A* Solver (AstarSolver.py)

A decoupled approach where paths are planned for each robot independently.

• Backward Search: Search proceeds from the destination to the start location.
• Cost Model: Incorporates physical constraints:
  • Acceleration: Straight movements become cheaper as the robot maintains direction (1st step: cost 3, 2nd: cost 2, 3rd+: cost 1).
  • Turning: Rotation incurs a penalty (cost 2).
• Heuristic: Uses pre-computed Dijkstra maps (precompute_dijkstra.py) to estimate the true travel cost considering acceleration and turns.

3. PIBT Solver (PIBTSolver.py & pibt.py)

Priority Inheritance with Backtracking is an iterative MAPF algorithm designed for high-density grids.

• Mechanism: Agents plan one step ahead. If an agent wants to move to a cell occupied by a higher-priority agent, it waits. If it moves to a cell occupied by a lower-priority agent, it "inherits" priority to push the other agent out of the way.
• Priorities:
  • Static: Based on distance to goal (Longer distance = Higher priority).
  • Dynamic: Accumulated waiting time is added to priority to prevent starvation/deadlocks.
• Deadlock Breaking: If a robot waits too long, its priority increases, eventually forcing neighboring agents to yield.

📂 Code Structure

algorithm/multi_robot/python_demo/
├── __main__.py               # Entry point, starts the gRPC server
├── algoService.py            # Main gRPC service, handles solver switching logic
├── AstarSolver.py            # A* implementation with kinematic cost model
├── PIBTSolver.py             # Wrapper for PIBT logic, handles map parsing
├── pibt.py                   # Core PIBT algorithm implementation
├── dist_table.py             # Lazy BFS distance table for PIBT heuristics
├── precompute_dijkstra.py    # Pre-computes all-pairs shortest paths with costs
├── mapf_utils.py             # Grid utilities (neighbors, validation, loading)
└── hephaestus.proto          # Protocol Buffer definition (RPC interface)

🛠 Key Components

Distance Table (dist_table.py)

Used by PIBT, this class provides a memory-efficient, lazy-evaluated BFS. It computes the shortest path distance from any cell to a specific agent's goal only when requested, caching the results for future lookups.

Precomputed Dijkstra (precompute_dijkstra.py)

Before the simulation starts, this module computes a comprehensive routing table. Unlike standard BFS, this considers the "momentum" of the robot (acceleration/deceleration) and turning costs, providing a highly accurate heuristic for the A* solver.

🚀 Usage

The algorithm module is designed to run as a standalone process communicating via gRPC.

Prerequisites

• Python 3.7+
• grpcio, grpcio-tools
• numpy

Running the Service

Execute the main module to start the server (default port: 10022):

python3 algorithm/multi_robot/python_demo/__main__.py

Protocol Buffers

If you modify hephaestus.proto, regenerate the python stubs:

python3 -m grpc_tools.protoc -I. --python_out=. --grpc_python_out=. hephaestus.proto