Adds mapf benchmark tools to this repo

.gitignore

@@ -1,2 +1,4 @@
 /target
 Cargo.lock
+cache/
+benchmark_report.json

Cargo.toml

@@ -2,6 +2,7 @@
 members = [
     "mapf",
     "mapf-viz",
+    "mapf-bench",
 ]
 
 resolver = "2"

mapf-bench/Cargo.toml

@@ -0,0 +1,13 @@
+[package]
+name = "mapf-bench"
+version = "0.1.0"
+edition = "2021"
+
+[dependencies]
+mapf = { path = "../mapf" }
+movingai = "0.2"
+anyhow = "1.0"
+clap = { version = "4.0", features = ["derive"] }
+serde = { version = "1.0", features = ["derive"] }
+serde_json = "1.0"
+indicatif = "0.17"

mapf-bench/README.md

@@ -0,0 +1,50 @@
+# `mapf-bench`
+
+`mapf-bench` is a binary crate designed to run single Multi-Agent Path Finding (MAPF) benchmark scenarios using the `mapf` library's negotiation solver.
+
+Currently, it supports benchmarks from the [Moving AI MAPF benchmark collection](https://www.movingai.com/benchmarks/mapf.html).
+
+## Relationship with `scripts/benchmark.py`
+
+While `mapf-bench` can be run directly to test a single scenario, it is intended to be used in conjunction with the Python orchestrator script located at `scripts/benchmark.py`.
+
+*   **`scripts/benchmark.py`**: Automates downloading the Moving AI benchmark files (maps and scenarios), unzipping them, building `mapf-bench` in release mode, and running it across multiple configurations (different maps, scenarios, and agent counts) to generate a comprehensive performance report.
+*   **`mapf-bench`**: The low-level runner that loads a specific map and scenario file, sets up the negotiation scenario, and runs the solver for a single run.
+
+## Intended Workflow
+
+To run a full benchmark suite:
+
+1.  Use the Python script from the root directory:
+    ```bash
+    python3 scripts/benchmark.py --timeout 30 --max-scenarios 1 --maps empty-32-32.map
+    ```
+    This script will handle building this crate and running it.
+
+To run a single scenario manually for debugging:
+
+1.  Build the crate:
+    ```bash
+    cargo build -p mapf-bench --release
+    ```
+2.  Run the binary, providing paths to the map and scenario files:
+    ```bash
+    cargo run -p mapf-bench --release -- --map cache/maps/empty-32-32.map --scen cache/scenarios/scen-random/empty-32-32-random-1.scen --num-agents 10
+    ```
+
+## Command Line Arguments
+
+`mapf-bench` accepts the following arguments:
+
+*   `-m`, `--map <MAP>`: Path to the Moving AI map file (`.map`).
+*   `-s`, `--scen <SCEN>`: Path to the Moving AI scenario file (`.scen`).
+*   `-r`, `--radius <RADIUS>`: Radius of the agents (default: 0.45).
+*   `--speed <SPEED>`: Speed of the agents (default: 1.0). (Note: no short option to avoid conflict with `--scen`).
+*   `-n`, `--num-agents <NUM_AGENTS>`: Number of agents to include in the scenario (default: 10).
+*   `-t`, `--timeout <TIMEOUT>`: Timeout in seconds (default: 30). Note: This timeout is currently enforced by the parent process (`benchmark.py`) rather than the binary itself.
+
+## Map Format
+
+The benchmark loader parses Moving AI `.map` files. The characters in the grid are interpreted as follows:
+*   `.` : Passable terrain.
+*   Any other character : Obstacle (impassable).

mapf-bench/src/main.rs

@@ -0,0 +1,72 @@
+mod movingai;
+
+use anyhow::Result;
+use clap::Parser;
+use std::path::PathBuf;
+use std::time::Instant;
+
+/// Run a single MAPF benchmark scenario using the negotiation solver.
+#[derive(Parser, Debug)]
+#[command(author, version, about, long_about = None)]
+struct Args {
+    /// Path to the Moving AI map file (.map)
+    #[arg(short, long)]
+    map: PathBuf,
+
+    /// Path to the Moving AI scenario file (.scen)
+    #[arg(short, long)]
+    scen: PathBuf,
+
+    /// Radius of the agents
+    #[arg(short, long, default_value_t = 0.45)]
+    radius: f64,
+
+    /// Speed of the agents
+    #[arg(long, default_value_t = 1.0)]
+    speed: f64,
+
+    /// Number of agents to include in the benchmark
+    #[arg(short, long, default_value_t = 10)]
+    num_agents: usize,
+
+    /// Timeout in seconds (enforced by parent process)
+    #[arg(short, long, default_value_t = 30)]
+    timeout: u64,
+}
+
+fn main() -> Result<()> {
+    let args = Args::parse();
+
+    println!("Loading map: {:?}", args.map);
+    let map = movingai::Map::from_file(&args.map)?;
+
+    println!("Loading scenario: {:?}", args.scen);
+    let scenario = movingai::MovingAIScenario::from_file(&args.scen)?;
+
+    let negotiation_scenario = scenario.to_negotiation_scenario(
+        &map,
+        args.num_agents,
+        args.radius,
+        args.speed,
+        60_f64.to_radians(),
+    );
+
+    println!("Running negotiation with {} agents", args.num_agents);
+    let start_time = Instant::now();
+    // We don't have a clean way to pass wall-clock timeout into negotiate yet,
+    // so we'll rely on the parent script to enforce strict timeouts for now,
+    // or we could add a QueueLengthLimit as a proxy.
+    let result = mapf::negotiation::negotiate(&negotiation_scenario, None);
+    let duration = start_time.elapsed();
+
+    match result {
+        Ok((_solution, _arena, _name_map)) => {
+            println!("Negotiation successful in {:?}", duration);
+        }
+        Err(e) => {
+            println!("Negotiation failed: {:?}", e);
+        }
+    }
+
+    Ok(())
+}

mapf-bench/src/movingai.rs

@@ -0,0 +1,181 @@
+/*
+ * Copyright (C) 2026 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+//! This module contains a simple parser for the movingai benchmarks.
+//! This benchmark is widely seen as the de-facto mapf benchmark.
+//!
+//! The benchmarks consist of *.map files and *.scen files. The *.map
+//! files are occupancy grids while the *.scen files contain
+//! exact mapf scenarios.
+
+use anyhow::Result;
+use std::collections::HashMap;
+use std::fs::File;
+use std::io::{BufRead, BufReader};
+use std::path::Path;
+
+use mapf::negotiation::{Agent, Scenario};
+use std::collections::BTreeMap;
+
+pub struct Map {
+    pub width: usize,
+    pub height: usize,
+    /// The grid representation of the map.
+    ///
+    /// The characters in the grid represent different terrain types based on the Moving AI format:
+    /// - `.` : Passable terrain
+    /// - `@` : Obstacle
+    pub grid: Vec<Vec<char>>,
+}
+
+impl Map {
+    pub fn from_file<P: AsRef<Path>>(path: P) -> Result<Self> {
+        let file = File::open(path)?;
+        let reader = BufReader::new(file);
+        let mut lines = reader.lines();
+
+        let mut width = 0;
+        let mut height = 0;
+
+        // Parse header
+        while let Some(line) = lines.next() {
+            let line = line?;
+            if line.starts_with("type") {
+                continue;
+            } else if line.starts_with("height") {
+                height = line.split_whitespace().nth(1).unwrap().parse()?;
+            } else if line.starts_with("width") {
+                width = line.split_whitespace().nth(1).unwrap().parse()?;
+            } else if line.starts_with("map") {
+                break;
+            }
+        }
+
+        let mut grid = Vec::with_capacity(height);
+        for _ in 0..height {
+            if let Some(line) = lines.next() {
+                let line = line?;
+                grid.push(line.chars().collect());
+            }
+        }
+
+        Ok(Map {
+            width,
+            height,
+            grid,
+        })
+    }
+
+    pub fn to_occupancy_map(&self) -> HashMap<i64, Vec<i64>> {
+        let mut occupancy = HashMap::new();
+        for y in 0..self.height {
+            let mut row = Vec::new();
+            for x in 0..self.width {
+                let c = self.grid[y][x];
+                if c != '.' && c != 'G' && c != 'S' {
+                    row.push(x as i64);
+                }
+            }
+            if !row.is_empty() {
+                occupancy.insert(y as i64, row);
+            }
+        }
+        occupancy
+    }
+}
+
+pub struct ScenarioEntry {
+    pub _bucket: usize,
+    pub _map_file: String,
+    pub _map_width: usize,
+    pub _map_height: usize,
+    pub start: [i64; 2],
+    pub goal: [i64; 2],
+    pub _optimal_length: f64,
+}
+
+pub struct MovingAIScenario {
+    pub entries: Vec<ScenarioEntry>,
+}
+
+impl MovingAIScenario {
+    pub fn from_file<P: AsRef<Path>>(path: P) -> Result<Self> {
+        let file = File::open(path)?;
+        let reader = BufReader::new(file);
+        let mut lines = reader.lines();
+
+        // Skip version line
+        lines.next();
+
+        let mut entries = Vec::new();
+        for line in lines {
+            let line = line?;
+            let parts: Vec<&str> = line.split_whitespace().collect();
+            if parts.len() < 9 {
+                continue;
+            }
+
+            let start: [i64; 2] = [parts[4].parse()?, parts[5].parse()?];
+            let goal: [i64; 2] = [parts[6].parse()?, parts[7].parse()?];
+
+            entries.push(ScenarioEntry {
+                _bucket: parts[0].parse()?,
+                _map_file: parts[1].to_string(),
+                _map_width: parts[2].parse()?,
+                _map_height: parts[3].parse()?,
+                start,
+                goal,
+                _optimal_length: parts[8].parse()?,
+            });
+        }
+
+        Ok(MovingAIScenario { entries })
+    }
+
+    pub fn to_negotiation_scenario(
+        &self,
+        map: &Map,
+        num_agents: usize,
+        radius: f64,
+        speed: f64,
+        spin: f64,
+    ) -> Scenario {
+        let mut agents = BTreeMap::new();
+        for i in 0..num_agents.min(self.entries.len()) {
+            let entry = &self.entries[i];
+            agents.insert(
+                format!("agent_{}", i),
+                Agent {
+                    start: entry.start,
+                    yaw: 0.0,
+                    goal: entry.goal,
+                    radius,
+                    speed,
+                    spin,
+                },
+            );
+        }
+
+        Scenario {
+            agents,
+            obstacles: Vec::new(),
+            occupancy: map.to_occupancy_map(),
+            cell_size: 1.0,
+            camera_bounds: None,
+        }
+    }
+}

mapf-viz/examples/grid.rs

@@ -1316,7 +1316,14 @@ impl App {
         let elapsed = start_time.elapsed();
         println!("Successful planning took {} seconds", elapsed.as_secs_f64());
         dbg!(node_history.len());
-
+        let mut total_length = 0.0;
+        for solution in solution_node.proposals.values() {
+            total_length += solution.meta.trajectory.windows(2).fold(0.0, |acc, w| {
+                (w[0].position.translation.vector - w[1].position.translation.vector).magnitude() + acc
+            });
+
+        }
+        println!("Total length: {total_length}");
         assert!(self.canvas.program.layers.3.solutions.is_empty());
         for (i, proposal) in &solution_node.proposals {
             let name = name_map.get(i).unwrap();