prox_mpc_benchmark

Scenario-driven benchmarking harness for the ProxMPC stack. It measures three metric classes - accuracy (cross-track / goal error), precision (mean ± std over repeats), and real-time / feasibility (solver diagnostics) - across a matrix of scenario x model x controller x run mode.

Table of Contents

• Overview
• Prerequisites
• Build
• Project Structure
• Run modes
• Coverage and status
• Notes on the metrics
• Configuration
• Usage
  • Quick start
  • Standalone matrix (b1)
  • Cross-controller comparison (b2)
  • Aggregate the tables
• Demonstration videos
• Results
• License

Overview

The package contains a controller-agnostic C++ live metrics node (src/metrics_node.cpp) plus installed Python tooling (scripts/) for orchestration, map generation, goal sending, bag reduction, and aggregation. It reuses the demo worlds/maps/models rather than duplicating them, and owns the result artifacts, which stay local and gitignored - the framework performs no git operations.

The narrative companion - how ProxMPC compares against the stock Nav2 controllers and what the suite concluded - is in doc/controller-comparison-results.md.

Prerequisites

• Operating system: Ubuntu 24.04 (Noble).
• ROS 2 distribution: Jazzy.
• Build system: ament_cmake.
• Always needed: prox_mpc_core, prox_mpc_msgs, and prox_mpc_demo (the reused worlds, maps, and models).
• Modes a / b2: additionally Nav2 and the stock Nav2 controllers under comparison (DWB, MPPI, Regulated Pure Pursuit, Graceful, and Vector Pursuit - the one external community peer), plus prox_mpc_controller; mode a also needs Gazebo Harmonic and ros_gz.

ROS dependencies are declared in package.xml and resolved by rosdep install.

Build

Build the harness and its dependencies in an overlay workspace:

colcon build --symlink-install --packages-select \
  prox_mpc_msgs prox_mpc_core prox_mpc_controller prox_mpc_demo prox_mpc_benchmark
source install/setup.bash

Project Structure

• config/scenarios/ - eleven scenarios, one YAML each. The four single-obstacle motion cells driven by the standalone matrix (static_box, dynamic_circle, dynamic_line_forward, dynamic_line_backward); nav2_open, the obstacle-free cross-controller cell; and the six multi-obstacle cells that carry the simultaneous two-mover collision comparison - dynamic_multi, dynamic_multi_noise, and blind_multi_0 through blind_multi_3 (generated by gen_blind_multi.py).
• config/controllers/ - one preset per Nav2 controller under test: proxmpc, proxmpc_pred (the predictive ProxMPC variant), dwb, mppi, regulated_pure_pursuit, graceful, and vector_pursuit.
• config/robots/ - robot <-> prox_mpc model pairing (waffle->Unicycle, ackermann->Bicycle).
• config/metrics.yaml - metric set, pass thresholds, repeats, shared control params.
• config/nav2_b2_base.yaml - the shared Nav2 stack the mode-b2 launch injects each controller preset into.
• src/metrics_node.cpp - live cross-track/goal-error + SolverDiagnostics tap; writes a per-run JSON.
• src/kinematic_plant.cpp - mode (b2) plant: integrates /cmd_vel as a unicycle, publishes /odom + TF.
• src/scan_simulator.cpp - synthesises the LaserScan the mode (b2) costmaps and the obstacle tracker perceive, so every controller sees the same sensor stream without Gazebo.
• src/timing_controller_wrapper.cpp - a nav2_core::Controller decorator that wall-clock times the wrapped controller’s computeVelocityCommands so every controller’s per-cycle compute is measured identically.
• timing_controller_plugin.xml - the pluginlib export for that decorator.
• include/prox_mpc_benchmark/ - metrics_math.hpp (the ROS-free metric math), obstacle_field.hpp (scenario obstacle geometry), and timing_controller_wrapper.hpp.
• test/ - GoogleTest suites test_metrics.cpp and test_obstacle_field.cpp.
• launch/benchmark.launch.py - standalone (b1) sim + metrics node for one scenario x model.
• launch/benchmark_nav2.launch.py - mode (b2) Nav2 + kinematic plant with the selected controller preset.
• launch/interactive.launch.py - the click-a-goal interactive Nav2 bring-up on the kinematic plant (no Gazebo).
• scripts/run_matrix.py - orchestrate the standalone (b1) matrix x repeats.
• scripts/run_nav2.py - orchestrate the mode (b2) cross-controller comparison (Nav2 + plant, no Gazebo).
• scripts/resource_sampler.py - sample the controller_server process CPU/RSS + /cmd_vel rate (b2).
• scripts/generate_map.py - world+map generation for the scale presets (7/15/30 m).
• scripts/gen_blind_multi.py - generate the blind_multi_* two-mover scenario YAMLs.
• scripts/goal_sender.py - auto-send NavigateToPose / NavigateThroughPoses (modes a/b2).
• scripts/gt_obstacle_publisher.py - publish the scenario’s ground-truth obstacle states (the --oracle feed and the collision scoring reference).

File truncated at 100 lines see the full file

Changelog for package prox_mpc_benchmark

1.0.0 (2026-07-28)

• Initial release: scenario-driven benchmarking harness with the standalone matrix (mode a/b1, four scenarios x bicycle/unicycle models) and the Nav2 cross-controller comparison (mode b2) against DWB, MPPI, Graceful, Regulated Pure Pursuit, and Vector Pursuit, reusing the prox_mpc_demo simulation node and prox_mpc_open map and the shared bicycle/unicycle/waffle robots, and shipping its own scalable world and Ackermann robot model.
• Live C++ metrics node tapping cross-track/goal error and SolverDiagnostics, plus installed Python tooling for orchestration, map generation, goal sending, bag reduction, and aggregation.
• Added a nav2_core::Controller timing decorator (timing_controller_wrapper) so every controller's per-cycle computeVelocityCommands compute is measured identically, plus fair-tuned per-cycle compute/resource metrics (compute_ms_p50/p95/max) alongside the existing CPU/RSS/control-rate sampling for the cross-controller comparison.
• vector_pursuit_controller (apt ros-jazzy-vector-pursuit-controller v2.0.0, Apache-2.0) is the comparison's single external fair peer, with a config/controllers/vector_pursuit.yaml preset, the exec_depend in package.xml, and the controller in DEFAULT_CONTROLLERS in run_nav2.py and in every scenario's controllers: list.
• Contributors: Simone Contorno