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,9 @@
+# `mapf-bench`
+
+The sole purpose of this crate is to run benchmarks using the `mapf` library. Currently the benchmarks we
+run are run against [moving AI](https://www.movingai.com/benchmarks/mapf/)'s benchmarks. This benchmark is often
+seen as the defacto "mapf" banchmark. This crate contains abinary to load the benchmark files into memory and run
+a single benchmark.
+
+It is advised to use the python benchmark script located in the `scripts/` folder tot automate the downloading and
+running of the benchmakrs.

mapf-bench/src/main.rs

@@ -0,0 +1,65 @@
+mod movingai;
+
+use anyhow::Result;
+use clap::Parser;
+use std::path::PathBuf;
+use std::time::Instant;
+
+#[derive(Parser, Debug)]
+#[command(author, version, about, long_about = None)]
+struct Args {
+    #[arg(short, long)]
+    map: PathBuf,
+
+    #[arg(short, long)]
+    scen: PathBuf,
+
+    #[arg(short, long, default_value_t = 0.45)]
+    radius: f64,
+
+    #[arg(short, long, default_value_t = 1.0)]
+    speed: f64,
+
+    #[arg(short, long, default_value_t = 10)]
+    num_agents: usize,
+
+    #[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,176 @@
+/*
+ * 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,
+    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();