Introduction to PRM

The Probabilistic Roadmap (PRM) algorithm works by first creating a "roadmap" (a graph) of possible paths through the free space, and then using that roadmap to answer specific path planning queries. It's broken into two distinct phases:

1. Construction (or Learning) Phase: A graph is built by scattering random points (nodes) throughout the free space and connecting nearby nodes that have a clear, collision-free path between them. This phase can be time-consuming but only needs to be done once for a given static environment.
2. Query Phase: Given a specific start and goal point, the algorithm connects them to the pre-built roadmap and then uses a fast graph search algorithm (like A* or Dijkstra's) to find the shortest path on the roadmap. This phase is typically very fast.

Core Data Structures

The Node (or Vertex)

For PRM, a node is very simple. It just needs to store its location.

• position: The coordinates of the point (e.g., [x, y]).
• edges: Neighbouring nodes.

The Graph (The Roadmap)

The roadmap is a graph that you will build. A good way to represent it is with an adjacency list.

• nodes: A list of all the Node objects in your graph.
• edges: A dictionary or hash map where each key is a Node and the value is a list of its connected neighbors. The edges can also store weights, which would be the distance between the connected nodes.

// Pseudocode for Graph representation
class Graph {
  constructor() {
    this.nodes = [];
    this.edges = new Map(); // Key: Node, Value: [NeighborNode1, NeighborNode2, ...]
  }

  add_node(node) {
    this.nodes.push(node);
    this.edges.set(node, []);
  }

  add_edge(node1, node2) {
    this.edges.get(node1).push(node2);
    this.edges.get(node2).push(node1); // For an undirected graph
  }
}

Phase 1: Roadmap Construction

This is the pre-processing step where you build the graph.
Algorithm:

1. Initialization: Create an empty Graph.
2. Sampling Loop: Repeat for a set number of samples (N):
   • a. Generate a Sample: Create a random point, q_rand, in the configuration space.
   • b. Check for Collision: Verify that q_rand is in the "free space" (i.e., not inside an obstacle). You'll need an is_in_free_space helper.
   • c. Add Node: If it's in free space, create a Node for it and add it to your graph.
3. Connection Loop: After sampling, connect the nodes.
   • a. For each node in graph.nodes:
     • i. Find Neighbors: Find the k nearest neighbors to this node from the other nodes in the graph. You'll need a find_k_nearest helper.
     • ii. For each neighbor in that list of k-nearest neighbors:
       • Check Connection: Check if the straight-line path between node.position and neighbor.position is collision-free using the is_collision_free helper.
       • Add Edge: If it's collision-free, add an edge between node and neighbor in your graph.
         At the end of this phase, you have a roadmap that represents the connectivity of the free space.

Phase 2: Querying the Roadmap

Now, you use the roadmap to find a specific path.
Algorithm:

1. Define Query: You are given a start_position and a goal_position.
2. Create Start and Goal Nodes: Create start_node and goal_node objects. Check that they are in free space. If not, a path is impossible.
3. Connect Start to Roadmap:
   • Find the k nearest neighbors in the graph to the start_node.
   • Iterate through these neighbors and check for a collision-free path to the start_node.
   • Add a temporary edge from the start_node to each connectable neighbor.
4. Connect Goal to Roadmap:
   • Do the same for the goal_node: find its k nearest neighbors in the graph and add temporary edges for all collision-free connections.
5. Graph Search:
   • Now that start_node and goal_node are connected to the main graph, use a standard graph search algorithm (A* is excellent if you have a heuristic, otherwise Dijkstra's is perfect) to find the shortest path from start_node to goal_node.
   • The search algorithm will explore the graph, using the distances between nodes as edge weights.
6. Reconstruct Path:
   • If the search algorithm finds the goal_node, trace back through the parent pointers (which the search algorithm provides) from the goal_node to the start_node to get the final path.
   • If the goal is not found, no path exists on the roadmap.

Helper Functions

You will need to implement these functions to support the main algorithm.

• is_in_free_space(point): Takes a point and returns true if it is not inside any defined obstacle, false otherwise.
• find_k_nearest(graph, node, k): Takes the graph, a target node, and an integer k. It should iterate through all other nodes in the graph, calculate the distance to each, and return a list of the k nodes with the smallest distances.
• is_collision_free(pos1, pos2): The same function used in RRT. It checks if the straight-line segment between two points intersects with any obstacles.
• graph_search(graph, start_node, goal_node): Your implementation of a search algorithm like A* or Dijkstra's. It should take the graph and the start/goal nodes and return a dictionary of parent pointers and cost values for the traversed nodes.
• reconstruct_path(parents, goal_node): Takes the parent map from the search algorithm and the goal node, and traces back to the start to produce the final list of positions.
  PRM is powerful because once the expensive construction phase is done, finding new paths in the same environment is extremely fast.

Example

// Copyright (c) 2025 Junior Sundar
//
// SPDX-License-Identifier: BSD-3-Clause

//! An implementation of the PRM (Probabilistic Roadmap) algorithm.

use std::{
    collections::{HashMap, VecDeque},
    sync::Arc,
    time::{Duration, Instant},
};

use rand::Rng;

use crate::base::{
    error::PlanningError,
    goal::{Goal, GoalRegion},
    planner::{Path, Planner},
    problem_definition::ProblemDefinition,
    space::StateSpace,
    state::State,
    validity::StateValidityChecker,
};

/// Represents a node (or "milestone") in the probabilistic roadmap.
#[derive(Clone)]
struct RoadmapNode<S: State> {
    /// The state associated with this node.
    state: S,
    /// A list of indices pointing to other connected nodes in the roadmap.
    edges: Vec<usize>,
}

/// An implementation of the Probabilistic Roadmap (PRM) algorithm.
///
/// PRM is a multi-query, sampling-based algorithm that is particularly effective
/// in static environments. It works by first constructing a "roadmap" graph of valid
/// states and then querying this graph to find paths.
///
/// # Algorithm Overview
///
/// 1.  **Construction Phase**:
///     a. Sample a large number of states randomly from the state space.
///     b. For each valid sample, find all nearby nodes already in the roadmap.
///     c. If a valid, collision-free motion exists between the new sample and a
///        neighbor, add an edge connecting them in the roadmap.
/// 2.  **Query Phase**:
///     a. Connect the start and goal states to the roadmap.
///     b. Use a graph search algorithm (like Breadth-First Search or A*) to find
///        a path on the roadmap from the start to the goal.
pub struct PRM<S: State, SP: StateSpace<StateType = S>, G: Goal<S>> {
    /// The number of states to sample during the roadmap construction phase.
    pub num_samples: usize,
    /// The radius within which to search for neighbors to connect to a new sample.
    pub connection_radius: f64,

    problem_def: Option<Arc<ProblemDefinition<S, SP, G>>>,
    validity_checker: Option<Arc<dyn StateValidityChecker<S> + Send + Sync>>,
    roadmap: Vec<RoadmapNode<S>>,
}

impl<S, SP, G> PRM<S, SP, G>
where
    S: State + Clone,
    SP: StateSpace<StateType = S>,
    G: Goal<S> + GoalRegion<S>, // GoalRegion is needed for the A* heuristic
{
    pub fn new(num_samples: usize, connection_radius: f64) -> Self {
        Self {
            num_samples,
            connection_radius,
            problem_def: None,
            validity_checker: None,
            roadmap: Vec::new(),
        }
    }

    /// An internal helper to check motion validity using the stored checker.
    fn check_motion(&self, from: &S, to: &S) -> bool {
        if let (Some(pd), Some(vc)) = (&self.problem_def, &self.validity_checker) {
            let space = &pd.space;
            let dist = space.distance(from, to);
            // Use a fixed resolution for checking motions in PRM
            let num_steps = (dist / (space.get_longest_valid_segment_length() * 0.5)).ceil() as usize;

            if num_steps <= 1 { return vc.is_valid(to); }

            let mut interpolated_state = from.clone();
            for i in 1..=num_steps {
                let t = i as f64 / num_steps as f64;
                space.interpolate(from, to, t, &mut interpolated_state);
                if !vc.is_valid(&interpolated_state) { return false; }
            }
            true
        } else { false }
    }
}

impl<S, SP, G> Planner<S, SP, G> for PRM<S, SP, G>
where
    S: State + Clone,
    SP: StateSpace<StateType = S>,
    G: Goal<S> + GoalRegion<S>,
{
    fn setup(
        &mut self,
        problem_def: Arc<ProblemDefinition<S, SP, G>>,
        validity_checker: Arc<dyn StateValidityChecker<S> + Send + Sync>,
    ) {
        self.problem_def = Some(problem_def);
        self.validity_checker = Some(validity_checker);
        self.roadmap.clear();
    }

    fn solve(&mut self, timeout: Duration) -> Result<Path<S>, PlanningError> {
        let start_time = Instant::now();
        let pd = self.problem_def.as_refPlannerUninitialised?;
        let vc = self.validity_checker.as_refPlannerUninitialised?;
        let mut rng = rand::thread_rng();

        // --- 1. Construction Phase ---
        println!("PRM: Starting construction phase...");
        while self.roadmap.len() < self.num_samples {
            if start_time.elapsed() > timeout { return ErrTimeout; }

            let q_rand = pd.space.sample_uniform(&mut rng).unwrap();
            if vc.is_valid(&q_rand) {
                let new_node_idx = self.roadmap.len();
                let mut new_node = RoadmapNode { state: q_rand, edges: Vec::new() };

                // Find neighbors and attempt to connect
                for i in 0..self.roadmap.len() {
                    let neighbor = &mut self.roadmap[i];
                    if pd.space.distance(&new_node.state, &neighbor.state) < self.connection_radius {
                        if self.check_motion(&new_node.state, &neighbor.state) {
                            new_node.edges.push(i);
                            neighbor.edges.push(new_node_idx);
                        }
                    }
                }
                self.roadmap.push(new_node);
            }
        }
        println!("PRM: Roadmap constructed with {} milestones.", self.roadmap.len());

        // --- 2. Query Phase ---
        let start_state = &pd.start_states[0];
        if !vc.is_valid(start_state) { return ErrInvalidStartState; }

        // Connect start state to the roadmap
        let mut start_connections = Vec::new();
        for i in 0..self.roadmap.len() {
            if pd.space.distance(start_state, &self.roadmap[i].state) < self.connection_radius &&
               self.check_motion(start_state, &self.roadmap[i].state) {
                start_connections.push(i);
            }
        }

        // Find goal nodes in the roadmap
        let mut goal_indices = Vec::new();
        for i in 0..self.roadmap.len() {
            if pd.goal.is_satisfied(&self.roadmap[i].state) {
                goal_indices.push(i);
            }
        }

        if start_connections.is_empty() || goal_indices.is_empty() {
            return ErrNoSolutionFound;
        }

        // Graph Search (Breadth-First Search)
        let mut queue: VecDeque<Vec<usize>> = start_connections.into_iter().map(|start_idx| vec![start_idx]).collect();
        let mut visited = vec![false; self.roadmap.len()];

        while let Some(path) = queue.pop_front() {
            let current_idx = *path.last().unwrap();

            if goal_indices.contains(&current_idx) {
                // Solution found, reconstruct path
                let mut final_path = vec![start_state.clone()];
                final_path.extend(path.into_iter().map(|idx| self.roadmap[idx].state.clone()));
                return Ok(Path(final_path));
            }

            if visited[current_idx] { continue; }
            visited[current_idx] = true;

            for &neighbor_idx in &self.roadmap[current_idx].edges {
                if !visited[neighbor_idx] {
                    let mut new_path = path.clone();
                    new_path.push(neighbor_idx);
                    queue.push_back(new_path);
                }
            }
        }

        ErrNoSolutionFound
    }
}