diff --git a/.gitignore b/.gitignore
index 96ef6c0..26324e7 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,2 +1,4 @@
 /target
 Cargo.lock
+cache/
+benchmark_report.json
diff --git a/Cargo.toml b/Cargo.toml
index 8dd3043..aff6efd 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -2,6 +2,7 @@
 members = [
     "mapf",
     "mapf-viz",
+    "mapf-bench",
 ]
 
 resolver = "2"
diff --git a/mapf-bench/Cargo.toml b/mapf-bench/Cargo.toml
new file mode 100644
index 0000000..94b5e2d
--- /dev/null
+++ b/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"
diff --git a/mapf-bench/README.md b/mapf-bench/README.md
new file mode 100644
index 0000000..340310d
--- /dev/null
+++ b/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).
diff --git a/mapf-bench/src/main.rs b/mapf-bench/src/main.rs
new file mode 100644
index 0000000..e842e85
--- /dev/null
+++ b/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(())
+}
diff --git a/mapf-bench/src/movingai.rs b/mapf-bench/src/movingai.rs
new file mode 100644
index 0000000..4bd0760
--- /dev/null
+++ b/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,
+        }
+    }
+}
diff --git a/mapf-viz/examples/grid.rs b/mapf-viz/examples/grid.rs
index f2fdba5..69cc401 100644
--- a/mapf-viz/examples/grid.rs
+++ b/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();
diff --git a/scripts/benchmark.py b/scripts/benchmark.py
new file mode 100755
index 0000000..baa0252
--- /dev/null
+++ b/scripts/benchmark.py
@@ -0,0 +1,186 @@
+#!/usr/bin/env python3
+
+# Copyright 2024 Open Source Robotics Foundation, Inc.
+#
+# 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 tool downloads the moving ai benchmarks for mapf and runs the current
+# solver against the benchmarks to measure MAPF performance.
+# Usage is like so:
+#
+# From the project root run:
+#
+# python3 scripts/benchmark.py --timeout [number of seconds till timeout]
+#   \ --max-scenarios [number of random scenarios]
+#   \ --maps [maps to be run on]
+#
+# The benchmark mapfiles and names can be found here:
+# https://www.movingai.com/benchmarks/mapf/
+
+import os
+import subprocess
+import zipfile
+import urllib.request
+import json
+import time
+import argparse
+from pathlib import Path
+
+# Configuration
+CACHE_DIR = Path("cache")
+MAPS_ZIP_URL = "https://www.movingai.com/benchmarks/mapf/mapf-map.zip"
+SCEN_ZIP_URL = "https://www.movingai.com/benchmarks/mapf/mapf-scen-random.zip"
+DEFAULT_AGENT_COUNTS = [2, 5, 10, 15, 20, 25, 30, 35, 40, 45, 50]
+
+def download_file(url, dest):
+    if dest.exists():
+        return
+    print(f"Downloading {url} to {dest}...")
+    urllib.request.urlretrieve(url, dest)
+
+def unzip_file(zip_path, extract_to):
+    print(f"Unzipping {zip_path} to {extract_to}...")
+    with zipfile.ZipFile(zip_path, 'r') as zip_ref:
+        zip_ref.extractall(extract_to)
+
+def run_benchmark(map_path, scen_path, num_agents, timeout):
+    cmd = [
+        "cargo", "run", "-p", "mapf-bench", "--release", "--",
+        "--map", str(map_path),
+        "--scen", str(scen_path),
+        "--num-agents", str(num_agents),
+        "--timeout", str(timeout)
+    ]
+    
+    start_time = time.time()
+    try:
+        # Strict timeout enforcement via subprocess
+        result = subprocess.run(cmd, capture_output=True, text=True, timeout=timeout + 5)
+        duration = time.time() - start_time
+        
+        success = "Negotiation successful" in result.stdout
+        return {
+            "success": success,
+            "duration": duration,
+            "stdout": result.stdout,
+            "stderr": result.stderr
+        }
+    except subprocess.TimeoutExpired:
+        return {
+            "success": False,
+            "duration": timeout,
+            "error": "Timeout"
+        }
+    except Exception as e:
+        return {
+            "success": False,
+            "error": str(e)
+        }
+
+def main():
+    parser = argparse.ArgumentParser(
+        description=(
+            "Download Moving AI benchmarks for MAPF and run the solver against them to measure performance.\n\n"
+            "This script automates the benchmarking workflow:\n"
+            "1. Downloads map and scenario zip files from Moving AI website if not present in cache.\n"
+            "2. Unzips them into the cache directory.\n"
+            "3. Builds the `mapf-bench` Rust crate in release mode.\n"
+            "4. Runs the benchmark for specified maps and scenarios with a range of agent counts.\n"
+            "5. Generates a JSON report (`benchmark_report.json`) and prints a summary table.\n\n"
+            "For more details on the underlying benchmark runner, see `mapf-bench/README.md`."
+        ),
+        formatter_class=argparse.RawDescriptionHelpFormatter
+    )
+    parser.add_argument("--timeout", type=int, default=30, help="Timeout in seconds per run")
+    parser.add_argument("--max-scenarios", type=int, default=1, help="Max random scenarios per map")
+    parser.add_argument("--maps", nargs="+", default=["empty-32-32.map", "room-32-32-4.map", "maze-32-32-2.map"])
+    args = parser.parse_args()
+
+    CACHE_DIR.mkdir(exist_ok=True)
+    
+    maps_zip = CACHE_DIR / "mapf-map.zip"
+    scen_zip = CACHE_DIR / "mapf-scen-random.zip"
+    
+    download_file(MAPS_ZIP_URL, maps_zip)
+    download_file(SCEN_ZIP_URL, scen_zip)
+    
+    maps_dir = CACHE_DIR / "maps"
+    scen_dir = CACHE_DIR / "scenarios"
+    
+    if not maps_dir.exists():
+        unzip_file(maps_zip, maps_dir)
+    if not scen_dir.exists():
+        unzip_file(scen_zip, scen_dir)
+    
+    report = {}
+
+    print("Building mapf-bench in release mode...")
+    subprocess.run(["cargo", "build", "-p", "mapf-bench", "--release"], check=True)
+
+    for map_name in args.maps:
+        map_path = maps_dir / map_name
+        if not map_path.exists():
+            print(f"Map {map_name} not found.")
+            continue
+            
+        base_name = map_name.replace(".map", "")
+        
+        # Find all matching scenario files
+        scen_pattern = f"{base_name}-random-*.scen"
+        scen_files = list((scen_dir / "scen-random").glob(scen_pattern))
+        scen_files.sort()
+        
+        if not scen_files:
+            print(f"No scenarios found for {map_name} with pattern {scen_pattern}")
+            continue
+
+        selected_scens = scen_files[:args.max_scenarios]
+        
+        for scen_path in selected_scens:
+            scen_name = scen_path.name
+            print(f"\nBenchmarking {map_name} with scenario {scen_name}...")
+            
+            key = f"{map_name}:{scen_name}"
+            report[key] = []
+            
+            for count in DEFAULT_AGENT_COUNTS:
+                print(f"  Agents: {count}", end=" ", flush=True)
+                res = run_benchmark(map_path, scen_path, count, args.timeout)
+                if res["success"]:
+                    print(f"✅ ({res['duration']:.2f}s)")
+                else:
+                    error_msg = res.get("error", "FAILED")
+                    print(f"❌ ({error_msg})")
+                
+                report[key].append({
+                    "agents": count,
+                    "success": res["success"],
+                    "duration": res.get("duration", 0),
+                    "error": res.get("error")
+                })
+
+    # Save report
+    with open("benchmark_report.json", "w") as f:
+        json.dump(report, f, indent=2)
+    
+    # Print summary table
+    print("\nBenchmark Summary:")
+    print(f"{'Scenario':<40} | {'Agents':<6} | {'Status':<8} | {'Time':<8}")
+    print("-" * 75)
+    for key, results in report.items():
+        for res in results:
+            status = "SUCCESS" if res["success"] else (res.get("error") if res.get("error") else "FAILED")
+            print(f"{key[:40]:<40} | {res['agents']:<6} | {status:<8} | {res['duration']:>7.2f}s")
+
+if __name__ == "__main__":
+    main()