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/migration.md - what changed on the released surface since 1.0.0, and what a deployment has to do about it.
• 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/BicycleFrontAxle 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 outline-only footprint check, walked along the predicted trajectory as far as the robot’s own stopping distance, vetoes a command whose rasterised footprint perimeter crosses a lethal cell. It is a backstop, not a guarantee: it depends on costmap inflation sized to the robot and on the local costmap’s unknown-space tracking to be effective.
• Predictive (dynamic) obstacle avoidance (on by default): 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); predict_obstacles: false reproduces the costmap-only behavior bit-for-bit. The costmap-only fill reads one present-time costmap for every horizon node, so a moving obstacle is constrained where it was rather than where it will be. It is validated for single-obstacle environments; with more than one mover the error closes the gap the plan was routed through, and the tracker is required.
• 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:

File truncated at 100 lines see the full file

Changelog for package prox_mpc_controller

2.0.0 (2026-09-13)

• Breaking: predict_obstacles defaults to true. With no tracker publishing, or a stale message, the fill degrades to the costmap-only path, so the default costs a subscription rather than a behavior change. Set it false to reproduce the previous behavior bit-for-bit.
• Breaking: model_plugin defaults to prox_mpc_core/Unicycle rather than prox_mpc_core/Bicycle.
• Breaking: new allow_reversing parameter, default false, bounds the solver's linear control to [0, v_max], so it cannot plan reverse travel at all. 1.0.0 had no such switch; its reference was forward-only, but its solver always admitted reverse down to -v_max, so a deployment that upgrades without setting it loses reverse. With it true and no explicit model_params.v_min, reverse is capped at 0.15 m/s: both guards follow the predicted trajectory and so do cover a reversing one, but they see only what the costmap holds, and whether the platform sweeps behind itself is a property of its sensor rather than of this plugin. model_params.v_min is also forwarded on its own when negative.
• New reverse_from_plan_orientation parameter, default false: with it and allow_reversing set, the plan's pose orientations sign the reference into reverse and truncate it at the first direction change. It defaults off because only a planner that sets pose orientations means anything by them, and a plan carries nothing that reports which planner produced it: NavFn and Smac 2D emit the identity quaternion on every pose, which is indistinguishable from a straight reverse plan. Trusting them would read any path running against that one fixed heading as a reverse traverse, so the robot would drive the whole path backwards instead of turning around, and a path whose heading component changes sign would flip the reference from cycle to cycle. Set it true with a cusp-emitting planner (Smac Hybrid-A*, State Lattice).
• The reference is pinned to the goal pose inside the goal-checker xy tolerance instead of tracking the robot's own projection onto the plan. The cruise taper and the sampling step composed to give the reference horizon an arc reach of remaining^2 / xy_tol, shorter than remaining at every point inside the tolerance, so the goal region was tracked against a stub a few millimetres ahead of the projection that the projection then carried along: a reference with no fixed point, which let a small tracking error be traded down as cheaply one way as the other, and whose horizon never reached the plan end, so the goal's own orientation never entered it. Pinning gives the last stretch a fixed setpoint and a standing heading error. The mode is latched and released only past the new goal_settle_hysteresis_m, default 0.10 m.
• Once the goal checker's xy condition is met and only the heading is left, a platform with no steering channel holds station and turns on the spot rather than reversing and re-advancing as the body sweeps round. A steering model is excluded, having no way to turn on the spot.
• Travel direction is a latched mode rather than a fresh read of the plan geometry each cycle. A change is accepted only once the platform is at or below direction_switch_standstill_speed_mps (default 0.05 m/s) and direction_switch_dwell_s (default 0.5 s) has elapsed since the last change; until then the reference is held at the arc length under the robot, so the commanded speed falls to zero and the platform stops first. A cusp is driven the way a vehicle drives one: arrive, stop, shift, pull away. Both gates are inert unless reverse_from_plan_orientation is set.
• New prediction_forward_shadow_s parameter, default 0.0 (inert). The predictive keep-out is a disc about the predicted position and the constraint normal runs from the obstacle to the robot, so the formulation does not distinguish the space a mover is about to occupy from the space it is vacating; passing in front and passing behind cost the same, and because the reference carries no obstacle term, the front is the side that does not require lagging it. Set positive, the disc is biased forward by that many seconds of the track's own travel, with the radius grown by the same distance so the obstacle's own position stays covered. Left off by default: the mechanism is unit-tested, but an A/B on the demo walker was directionally favourable and statistically inconclusive at four runs per arm.
• New obstacle_yield_band_m parameter, default 0.0 (inert): eases the cruise when where the robot is heading - the plan at the intended cruise, or the trajectory the solver planned last cycle, whichever breaches deeper -would cut into a tracked mover's predicted keep-out, so the robot waits for the mover instead of racing it. The release is rate-limited so it cannot snap back to full cruise. Enable it with allow_reversing: with reversing off a waiting robot is boxed in by a second mover, which is why it stays off by default. It is not to be combined with light stage weights.
• New obstacle_yield_caps_speed parameter, default false: the obstacle-aware yield also caps the solver's forward speed bound, so it slows the robot rather than only lowering a cruise target the obstacle term can override. The cap falls no faster than the platform can brake, so the solver always has a feasible first control, and leaves the reverse bound alone.
• The footprint veto walks the predicted trajectory as far as the robot's stopping distance rather than checking a single pose one step ahead.
• Obstacle slots are bound to one object per cycle and ranked by time to encounter; the scan is centred on the predicted state and the keep-out on base_link.
• Solver acceptance is transactional: a candidate is committed only once the finiteness, veto and command gates accept, and convergence is reported past those gates.
• The solver-failure brake ramps in control space through the model,

File truncated at 100 lines see the full file