Reduce MPPI computation time, add OpenMP support by adivardi · Pull Request #11 · enwaytech/navigation2

adivardi · 2026-03-20T15:22:38Z

Additions & Changes

This PR improves the runtime frequency of the MPPI controller. The goal is to reduce the occurrences of the controller's frequency dropping below the requested 20Hz, which causes degraded performance.

The main culprit in MPPI are the collision checks, which take a big percentage of the runtime, especially when driving near a long obstacles (such as a wall)

Changes:

Cost critic:

Check for collisions backwards, starting from the end of the trajectory, since collisions are more probable away from robot.
- reduces by avg of 2ms & 5000 footprint checks (avg over 50m, ~ 2000 iterations)
Check center cost before doing a collision check
Call costmap_ros_->getRobotFootprint()) once
Add OpenMP support for the collision checks
- With 8 threads, reduces time by x4

Optimizer:

Clamp exponential arguments: caps all exp() arguments at -80, where the value is already ~1e-35, slightly above float32's smallest fraction of ~1.2e-38. This eliminates potential underflows & slow paths in the exp implementation.
- reduces computation by ~6x, from 2000-3000us to ~400us
Add noalias call
- very slight reduction in computation, but no harm to add

Checklist (tick off when done or remove when not relevant):

Set good PR title for changelog
Perform a self-review if the PR has more than 50 lines of changes

gemini-code-assist · 2026-03-20T15:23:14Z

Summary of Changes

Hello, I'm Gemini Code Assist¹! I'm currently reviewing this pull request and will post my feedback shortly. In the meantime, here's a summary to help you and other reviewers quickly get up to speed!

This pull request introduces a comprehensive overhaul of the MPPI controller's visualization framework, providing users with much finer control over debugging and analysis outputs. Alongside these visualization enhancements, a new critic has been integrated to promote smoother robot motion by penalizing sudden direction changes. Significant performance gains are also achieved through the introduction of OpenMP parallelization in the core cost evaluation, making the controller more efficient and robust.

Highlights

Enhanced Visualization System: The MPPI controller's visualization capabilities have been significantly refactored, introducing granular control over what is published. This includes separate toggles for optimal trajectory messages, optimal path, optimal footprints, and candidate trajectories with total or individual critic costs. The visualization parameters are now grouped under a dedicated 'Visualization' namespace in the YAML configuration.
New Direction Change Critic: A new DirectionChangeCritic has been added to penalize trajectories that involve abrupt changes in the robot's driving direction. This critic uses the robot's actual speed from odometry feedback to assess and penalize velocity differences when crossing zero, promoting smoother motion.
Performance Improvements with OpenMP: The CostCritic now supports OpenMP parallelization, allowing it to utilize multiple CPU cores for faster collision checking. A new parameter num_threads has been introduced to configure the number of threads, with an automatic heuristic for optimal thread count.
Improved Collision Checking Logic: The CostCritic's inCollision method has been optimized to perform an early exit if the center cost already indicates a lethal or inscribed obstacle, avoiding more expensive footprint checks. The collision checking now iterates trajectories backward, prioritizing collision detection at the end of trajectories.
Refined Optimizer Stability: The softmax calculation within the optimizer has been improved to prevent exponent overflow, enhancing numerical stability, especially when dealing with very large cost differences between trajectories.

Using Gemini Code Assist

The full guide for Gemini Code Assist can be found on our documentation page, here are some quick tips.

Invoking Gemini

You can request assistance from Gemini at any point by creating a comment using either /gemini <command> or @gemini-code-assist <command>. Below is a summary of the supported commands on the current page.

Feature	Command	Description
Code Review	`/gemini review`	Performs a code review for the current pull request in its current state.
Pull Request Summary	`/gemini summary`	Provides a summary of the current pull request in its current state.
Comment	@gemini-code-assist	Responds in comments when explicitly tagged, both in pull request comments and review comments.
Help	`/gemini help`	Displays a list of available commands.

Customization

To customize Gemini Code Assist for GitHub experience, repository maintainers can create a configuration file and/or provide a custom code review style guide (such as PEP-8 for Python) by creating and adding files to a .gemini/ folder in the base of the repository. Detailed instructions can be found here.

Limitations & Feedback

Gemini Code Assist may make mistakes. Please leave feedback on any instances where its feedback is incorrect or counter productive. You can react with 👍 and 👎 on @gemini-code-assist comments. If you're interested in giving your feedback about your experience with Gemini Code Assist for GitHub and other Google products, sign up here.

Review the Privacy Notices, Generative AI Prohibited Use Policy, Terms of Service, and learn how to configure Gemini Code Assist in GitHub here. Gemini can make mistakes, so double check it and use code with caution. ↩

gemini-code-assist

Code Review

This pull request significantly enhances the nav2_mppi_controller by refactoring and expanding its visualization capabilities, introducing a new DirectionChangeCritic, and implementing performance optimizations. The TrajectoryVisualizer now supports publishing various data including candidate trajectories with total and individual critic costs, optimal trajectories, paths, and footprints, controlled by new parameters. OpenMP parallelization has been integrated into the CostCritic for improved performance, along with optimizations to collision checking and Eigen matrix operations. A new DirectionChangeCritic is added to penalize changes in driving direction. A review comment highlights an unimplemented feature related to publish_transformed_path that is being tested, suggesting either implementation or test removal. Another comment points out an unused variable in the DirectionChangeCritic implementation that should be removed for code cleanliness.

nav2_mppi_controller/test/trajectory_visualizer_tests.cpp

nav2_mppi_controller/src/critics/direction_change_critic.cpp

adivardi

self review

Use robot_speed from feedback, as it better represents the actual direction of motion. This results in more consistent critic application and longer motions in each direction

Not sure if useful

After looking into adding break early when computing the path's arc-length

The clamp (min(80 * temperature)) caps all exp() arguments at -80, where the value is already ~1e-35, slightly above float32's smallest fraction of ~1.2e-38. This eliminates potential underflows &= slow paths in the exp implementation. This reduces computation by ~6x, from 2000-300us to ~400us

Let's Eigen know there is no aliasing, very small optimization

Reduces computation time from 18-20ms to 5ms

This reverts commit 9d721b9.

georgflick

don't you need to add the openMP dependency also to the package.xml ? or are you not adding it, because it's optional?

georgflick · 2026-04-08T12:46:50Z

nav2_mppi_controller/include/nav2_mppi_controller/critics/direction_change_critic.hpp

@@ -0,0 +1,51 @@
+// Copyright (c) 2024 Enway GmbH, Adi Vardi


should be 2026?

georgflick · 2026-04-08T13:20:03Z

nav2_mppi_controller/src/critics/direction_change_critic.cpp

@@ -0,0 +1,69 @@
+// Copyright (c) 2024 Enway GmbH, Adi Vardi


adivardi · 2026-04-08T13:55:40Z

don't you need to add the openMP dependency also to the package.xml ? or are you not adding it, because it's optional?

yeah. I copied from ros-navigation#5933

What do you think? I guess I will also PR this upstream at some point

gemini-code-assist bot reviewed Mar 20, 2026

View reviewed changes

nav2_mppi_controller/test/trajectory_visualizer_tests.cpp Show resolved Hide resolved

nav2_mppi_controller/src/critics/direction_change_critic.cpp Show resolved Hide resolved

adivardi force-pushed the av/mppi_freq branch 2 times, most recently from 17a55cd to 5a96778 Compare March 23, 2026 07:52

adivardi changed the title ~~Av/mppi freq~~ Reduce MPPI computation time, add OpenMP support Mar 23, 2026

adivardi added the enhancement New feature or request label Mar 23, 2026

adivardi commented Mar 23, 2026

View reviewed changes

adivardi marked this pull request as ready for review March 23, 2026 09:59

adivardi requested a review from georgflick March 23, 2026 09:59

adivardi assigned georgflick Mar 23, 2026

adivardi added 20 commits March 27, 2026 13:47

improve comment a bit

f5b9121

Use pruned path length for path occupancy check

34c8540

[clean] use a fixed distance for occupancy check

6ac0699

parameterize & vis occupancy check distance

78bec14

Use max(distance, furthest_reached) for occupancy check

9d3f149

First version of direction change critic

5302234

Refactor direction change critic

fe6468b

Update DirectionChangeCritic after rebase

680eed1

DirectionChangeCritic: test use robot speed or last_cmd_published

3daf035

DirectionChangeCritic: use robot speed from feedback

f564a5c

Use robot_speed from feedback, as it better represents the actual direction of motion. This results in more consistent critic application and longer motions in each direction

DirectionChangeCritic: rm todo to penalize wz

8f875aa

Not sure if useful

fix comment

6659ba8

break long lines

809d679

fix line indentations

afd3190

Add comments to PathAlign

240676c

After looking into adding break early when computing the path's arc-length

add namespace critics to critics topics

b6a9d58

Add timing prints

59c0ffe

fix typo

a515e52

[tmp] add metrics to costCritic

1285552

CostCritic: get collision only once

124dd76

adivardi added 11 commits March 27, 2026 13:56

[tmp] cleaner count footprint checks

c08ec44

early exit if center is in collision

2137d3f

CostCritic: check collisions backwards

c78e04f

Add noalias to matrix multiplication

6d7992c

Let's Eigen know there is no aliasing, very small optimization

[print] fix missing fp checks value

3e230ba

[tmp] rm metrics from CostCritic

7cdcc1f

CostCritic: Add OpenMP with 4 threads

8eb3b51

Reduces computation time from 18-20ms to 5ms

Add flag to disable/enable openMP & a param to set no. of threads

6b33b0d

Revert "Add timing prints"

9d9d810

This reverts commit 9d721b9.

break long line

333a28f

adivardi force-pushed the av/mppi_freq branch from 44b83c6 to 333a28f Compare March 27, 2026 12:56

georgflick requested changes Apr 8, 2026

View reviewed changes

georgflick reviewed Apr 8, 2026

View reviewed changes

update license year

0485587

adivardi requested a review from georgflick April 8, 2026 13:56

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Reduce MPPI computation time, add OpenMP support#11

Reduce MPPI computation time, add OpenMP support#11
adivardi wants to merge 32 commits intoenway-develfrom
av/mppi_freq

adivardi commented Mar 20, 2026 •

edited

Loading

Uh oh!

gemini-code-assist bot commented Mar 20, 2026

Uh oh!

gemini-code-assist bot left a comment

Uh oh!

Uh oh!

Uh oh!

adivardi left a comment

Uh oh!

georgflick left a comment

Uh oh!

georgflick Apr 8, 2026

Uh oh!

adivardi Apr 8, 2026

Uh oh!

georgflick Apr 8, 2026

Uh oh!

adivardi Apr 8, 2026

Uh oh!

adivardi commented Apr 8, 2026

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

Conversation

adivardi commented Mar 20, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Additions & Changes

Changes:

Checklist (tick off when done or remove when not relevant):

Uh oh!

gemini-code-assist bot commented Mar 20, 2026

Summary of Changes

Highlights

Footnotes

Uh oh!

gemini-code-assist bot left a comment

Choose a reason for hiding this comment

Code Review

Uh oh!

Uh oh!

Uh oh!

adivardi left a comment

Choose a reason for hiding this comment

Uh oh!

georgflick left a comment

Choose a reason for hiding this comment

Uh oh!

georgflick Apr 8, 2026

Choose a reason for hiding this comment

Uh oh!

adivardi Apr 8, 2026

Choose a reason for hiding this comment

Uh oh!

georgflick Apr 8, 2026

Choose a reason for hiding this comment

Uh oh!

adivardi Apr 8, 2026

Choose a reason for hiding this comment

Uh oh!

adivardi commented Apr 8, 2026

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

adivardi commented Mar 20, 2026 •

edited

Loading