prox_mpc_controller

A Nav2 nav2_core::Controller plugin that drives a robot along the global plan by solving a nonlinear Model Predictive Control problem each control step, built on the prox_mpc_core SQP/QP engine.

The plugin owns the ROS integration: it loads a prox_mpc::Model by name, builds the state and control reference from the global plan, reduces the local costmap (and, optionally, tracked dynamic obstacles) to the engine’s obstacle triples, solves one SQP cycle, maps the first optimal control to a body Twist, and decelerates within the robot’s limits when a solve fails. The engine math is unchanged and lives in the core.

This plugin is verified in simulation: it runs inside a live controller_server driving a TurtleBot3 waffle under a full Nav2 stack in Gazebo Harmonic (see prox_mpc_demo/doc/nav2-simulation.md).

Table of Contents

• Documentation
• Key Features
• Prerequisites
• Build
• Use in a Nav2 stack
• Testing
• Troubleshooting
• License

Documentation

• doc/architecture.md - the Nav2 integration design: responsibility split, the controller lifecycle, the per-cycle data flow, the interfaces and QoS, the full parameter reference, and the two safety layers.
• doc/control-law.md - the controller-side math: reference construction, costmap reduction, predictive obstacle propagation, the deceleration fallback, speed limits, and the discrete-time CBF coupling.
• Engine math is in the core: NMPC/SQP/QP and obstacle avoidance.

Key Features

• NMPC behind nav2_core::Controller: one SQP cycle per control step over the ProxQP solver; linear models converge in a single QP solve.
• Model selected by configuration: the vehicle model is loaded with pluginlib (model_plugin, e.g. prox_mpc_core/Bicycle or prox_mpc_core/Unicycle), so switching the robot model needs no code change.
• Plan-following reference: arc-length sampling of the global plan with a continuous (unwrapped) heading, a curvature-aware steering reference for the bicycle, optional curvature-based cruise reduction, and goal-checker approach easing.
• Two-layer obstacle avoidance: a fast in-loop disc constraint built from the local costmap (clustered, windowed scan) shapes the trajectory, and an exact polygon-footprint check vetoes any command that would collide.
• Predictive (dynamic) obstacle avoidance (opt-in): consumes tracked obstacles, follows each track’s tracker-sampled predicted trajectory over the horizon (a constant-velocity ray when no samples are provided), binds it to a fixed constraint slot, and fills the remaining slots from the costmap (hybrid); off by default, reproducing the costmap-only behavior bit-for-bit.
• Safe failure handling: a non-converged or non-finite solve decelerates from the measured velocity at the robot’s limit and escalates to a Nav2 recovery after max_solver_failures consecutive failures; cancel() ramps to a stop and setSpeedLimit() applies a runtime bound from the next control cycle. A model that declares no control (u) or control-rate (du) bound cannot be braked or driven, so it fails configure() instead of coming up degraded.

Prerequisites

• ROS 2 Jazzy on Ubuntu 24.04.
• prox_mpc_core and prox_mpc_msgs (workspace packages).
• Nav2: nav2_core, nav2_costmap_2d, nav2_util.
• Eigen 3 and ProxQP / proxsuite (transitively, through the core).
• tf2, tf2_ros, visualization_msgs, rclcpp_lifecycle (resolved by rosdep).

Build

This package requires Nav2, so it is not built by the core-only overlay unless Nav2 is installed:

sudo apt install ros-$ROS_DISTRO-nav2-core ros-$ROS_DISTRO-nav2-costmap-2d ros-$ROS_DISTRO-nav2-util

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

Confirm the plugin is discoverable:

ros2 plugin list --package prox_mpc_controller   # lists prox_mpc_controller::ProxMpcController

The --package value is the package that installs the plugin description, not the nav2_core base the plugin registers against.

Use in a Nav2 stack

Select the plugin in the controller_server parameters and load its settings

File truncated at 100 lines see the full file

Changelog for package prox_mpc_controller

1.0.0 (2026-07-28)

• Initial release: nav2_core::Controller plugin wrapping the ProxMPC core, with global-plan reference building, costmap and predictive obstacle fills, a measured-velocity deceleration ramp, and an exact footprint veto.
• Fail-safe escalation to NoValidControl on a persistent solver failure or a persistent footprint veto; structural parameter validation with tuning clamps.
• Added max_solve_time and publish_diagnostics defaults to the shipped prox_mpc_controller.yaml config.
• Contributors: Simone Contorno