Reduce MPPI computation time, add OpenMP support

nav2_mppi_controller/CMakeLists.txt

@@ -22,6 +22,14 @@ find_package(visualization_msgs REQUIRED)
 find_package(nav2_ros_common REQUIRED)
 find_package(Eigen3 REQUIRED)
 
+option(ENABLE_OPENMP "Enable OpenMP parallelization" OFF)
+if(ENABLE_OPENMP)
+  find_package(OpenMP REQUIRED)
+  message(STATUS "OpenMP ENABLED for nav2_mppi_controller (${OpenMP_CXX_VERSION})")
+else()
+  message(STATUS "OpenMP DISABLED for nav2_mppi_controller")
+endif()
+
 include_directories(
   include
   ${EIGEN3_INCLUDE_DIR}
@@ -69,6 +77,7 @@ target_link_libraries(mppi_controller PUBLIC
   tf2_geometry_msgs::tf2_geometry_msgs
   tf2_ros::tf2_ros
   ${visualization_msgs_TARGETS}
+  Eigen3::Eigen
 )
 
 add_library(mppi_critics SHARED
@@ -83,6 +92,7 @@ add_library(mppi_critics SHARED
   src/critics/prefer_forward_critic.cpp
   src/critics/twirling_critic.cpp
   src/critics/velocity_deadband_critic.cpp
+  src/critics/direction_change_critic.cpp
 )
 if(CMAKE_CXX_COMPILER_ID STREQUAL "Clang" AND APPLE)
     # Apple Clang: use C++20 and optimization, omit -fconcepts
@@ -114,6 +124,11 @@ target_link_libraries(mppi_critics PUBLIC
 target_link_libraries(mppi_critics PRIVATE
   pluginlib::pluginlib
 )
+if(ENABLE_OPENMP)
+  target_link_libraries(mppi_critics PRIVATE OpenMP::OpenMP_CXX)
+else()
+  target_compile_options(mppi_critics PRIVATE -Wno-unknown-pragmas)
+endif()
 
 add_library(mppi_trajectory_validators SHARED
   src/trajectory_validators/optimal_trajectory_validator.cpp

nav2_mppi_controller/critics.xml

@@ -45,5 +45,9 @@
       <description>mppi critic for restricting command velocities in deadband range</description>
     </class>
 
+    <class type="mppi::critics::DirectionChangeCritic" base_class_type="mppi::critics::CriticFunction">
+      <description>mppi critic for penalizing changes in driving direction</description>
+    </class>
+
   </library>
 </class_libraries>

nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp

@@ -18,6 +18,10 @@
 #include <memory>
 #include <string>
 
+#ifdef _OPENMP
+#include <omp.h>
+#endif
+
 #include "nav2_costmap_2d/footprint_collision_checker.hpp"
 #include "nav2_costmap_2d/inflation_layer.hpp"
 
@@ -56,16 +60,23 @@ class CostCritic : public CriticFunction
     * @param theta theta of pose
     * @return bool if in collision
     */
-  inline bool inCollision(float cost, float x, float y, float theta)
+  inline bool inCollision(float cost, float x, float y, float theta,
+    const nav2_costmap_2d::Footprint & footprint)
   {
-    // If consider_footprint_ check footprint scort for collision
+    // if the center cost guarantees a collision, return before doing an expensive footprint check
+    if (cost == nav2_costmap_2d::LETHAL_OBSTACLE ||
+      cost == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE) {
+      return true;
+    }
+
+    // If consider_footprint_ check footprint score for collision
     float score_cost = cost;
     if (consider_footprint_ &&
       (cost >= possible_collision_cost_ || possible_collision_cost_ < 1.0f))
     {
       score_cost = static_cast<float>(collision_checker_.footprintCostAtPose(
           static_cast<double>(x), static_cast<double>(y), static_cast<double>(theta),
-          costmap_ros_->getRobotFootprint()));
+          footprint));
     }
 
     switch (static_cast<unsigned char>(score_cost)) {
@@ -89,6 +100,15 @@ class CostCritic : public CriticFunction
     */
   inline float findCircumscribedCost(std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap);
 
+  /**
+   * @brief Determine the number of threads to use for OpenMP parallelization.
+   *        If num_threads_ > 0, use that value directly.
+   *        If num_threads_ == -1 (auto), use half the available cores (memory-bound heuristic).
+   *        If OpenMP is disabled at build time, always returns 1.
+   * @return Number of threads to use
+   */
+  int getOptimalThreadCount();
+
   /**
     * @brief An implementation of worldToMap fully using floats
     * @param wx Float world X coord
@@ -143,6 +163,7 @@ class CostCritic : public CriticFunction
   float near_goal_distance_;
   std::string inflation_layer_name_;
 
+  int num_threads_{-1};
   unsigned int power_{0};
 };
 

nav2_mppi_controller/include/nav2_mppi_controller/critics/direction_change_critic.hpp

@@ -0,0 +1,51 @@
+// Copyright (c) 2026 Enway GmbH, Adi Vardi
+//
+// 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.
+
+#ifndef NAV2_MPPI_CONTROLLER__CRITICS__DIRECTION_CHANGE_CRITIC_HPP_
+#define NAV2_MPPI_CONTROLLER__CRITICS__DIRECTION_CHANGE_CRITIC_HPP_
+
+#include "nav2_mppi_controller/critic_function.hpp"
+#include "nav2_mppi_controller/tools/utils.hpp"
+
+namespace mppi::critics
+{
+
+/**
+ * @class mppi::critics::DirectionChangeCritic
+ * @brief Critic objective function for penalizing changes in driving direction.
+ */
+class DirectionChangeCritic : public CriticFunction
+{
+public:
+  /**
+    * @brief Initialize critic
+    */
+  void initialize() override;
+
+  /**
+   * @brief Evaluate cost related to changing driving direction
+   *
+   * @param costs [out] add cost values to this tensor
+   */
+  void score(CriticData & data) override;
+
+protected:
+  unsigned int power_{0};
+  float weight_{0};
+  float threshold_to_consider_{0};
+};
+
+}  // namespace mppi::critics
+
+#endif  // NAV2_MPPI_CONTROLLER__CRITICS__DIRECTION_CHANGE_CRITIC_HPP_

nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp

@@ -51,13 +51,17 @@ class PathAlignCritic : public CriticFunction
   size_t offset_from_furthest_{0};
   int trajectory_point_step_{0};
   float threshold_to_consider_{0};
+  float occupancy_check_min_distance_{0};
   float max_path_occupancy_ratio_{0};
   bool use_path_orientations_{false};
   unsigned int power_{0};
   float weight_{0};
 
   bool visualize_furthest_point_{false};
   nav2::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr furthest_point_pub_;
+
+  bool visualize_occupancy_check_distance_{false};
+  nav2::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr occupancy_check_dist_pub_;
 };
 
 }  // namespace mppi::critics

nav2_mppi_controller/include/nav2_mppi_controller/models/state.hpp

@@ -39,7 +39,9 @@ struct State
   Eigen::ArrayXXf cwz;
 
   geometry_msgs::msg::PoseStamped pose;
+  // current speed or last command published, depends on open_loop setting
   geometry_msgs::msg::Twist speed;
+  geometry_msgs::msg::Twist robot_speed;  // current speed from odometry
   float local_path_length;
 
   /**

nav2_mppi_controller/src/critics/cost_critic.cpp

@@ -13,6 +13,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include <algorithm>
 #include <cmath>
 #include "nav2_mppi_controller/critics/cost_critic.hpp"
 #include "nav2_costmap_2d/inflation_layer_interface.hpp"
@@ -33,6 +34,7 @@ void CostCritic::initialize()
   getParam(collision_cost_, "collision_cost", 1000000.0f);
   getParam(near_goal_distance_, "near_goal_distance", 0.5f);
   getParam(inflation_layer_name_, "inflation_layer_name", std::string(""));
+  getParam(num_threads_, "num_threads", -1);
   getParam(trajectory_point_step_, "trajectory_point_step", 2);
 
   // Normalized by cost value to put in same regime as other weights
@@ -128,6 +130,32 @@ float CostCritic::findCircumscribedCost(
   return circumscribed_cost_;
 }
 
+int CostCritic::getOptimalThreadCount()
+{
+#ifdef _OPENMP
+  if (num_threads_ > 0) {
+    RCLCPP_INFO_ONCE(
+      logger_,
+      "OpenMP: Using configured num_threads: %d",
+      num_threads_);
+    return num_threads_;
+  }
+  // Auto (-1): use half of available cores — costmap lookups are memory-bound,
+  // so more threads hit diminishing returns and increase cache contention.
+  // omp_get_max_threads() respects OMP_NUM_THREADS if set.
+  const int cpu_cores = omp_get_max_threads();
+  const int optimal = std::max(1, cpu_cores / 2);
+  RCLCPP_INFO_ONCE(
+    logger_,
+    "OpenMP: %d cores available, using %d threads (auto)",
+    cpu_cores, optimal);
+
+  return std::max(1, optimal);
+#else
+  return 1;
+#endif
+}
+
 void CostCritic::score(CriticData & data)
 {
   if (!enabled_) {
@@ -143,9 +171,11 @@ void CostCritic::score(CriticData & data)
   size_x_ = costmap->getSizeInCellsX();
   size_y_ = costmap->getSizeInCellsY();
 
+  nav2_costmap_2d::Footprint footprint;
   if (consider_footprint_) {
     // footprint may have changed since initialization if user has dynamic footprints
     possible_collision_cost_ = findCircumscribedCost(costmap_ros_);
+    footprint = costmap_ros_->getRobotFootprint();
   }
 
   // If near the goal, don't apply the preferential term since the goal is near obstacles
@@ -175,12 +205,23 @@ void CostCritic::score(CriticData & data)
     data.trajectories.yaws.data(), strided_traj_rows, strided_traj_cols,
     Eigen::Stride<-1, -1>(outer_stride, 1));
 
+#ifdef _OPENMP
+  #pragma omp parallel for \
+    default(none) \
+    shared(repulsive_cost, costmap, near_goal, footprint, \
+      strided_traj_rows, strided_traj_cols, traj_x, traj_y, traj_yaw) \
+    reduction(&&:all_trajectories_collide) \
+    schedule(dynamic) \
+    num_threads(getOptimalThreadCount())
+#endif
   for (int i = 0; i < strided_traj_rows; ++i) {
     bool trajectory_collide = false;
     float pose_cost = 0.0f;
     float & traj_cost = repulsive_cost(i);
 
-    for (int j = 0; j < strided_traj_cols; j++) {
+    // iterate over the trajectory backwards, as collisions are more likely towards the end of
+    // the trajectory
+    for (int j = strided_traj_cols - 1; j >=0; j--) {
       float Tx = traj_x(i, j);
       float Ty = traj_y(i, j);
       unsigned int x_i = 0u, y_i = 0u;
@@ -197,7 +238,7 @@ void CostCritic::score(CriticData & data)
         }
       }
 
-      if (inCollision(pose_cost, Tx, Ty, traj_yaw(i, j))) {
+      if (inCollision(pose_cost, Tx, Ty, traj_yaw(i, j), footprint)) {
         traj_cost = collision_cost_;
         trajectory_collide = true;
         break;

nav2_mppi_controller/src/critics/direction_change_critic.cpp

@@ -0,0 +1,69 @@
+// Copyright (c) 2026 Enway GmbH, Adi Vardi
+//
+// 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.
+
+#include "nav2_mppi_controller/critics/direction_change_critic.hpp"
+
+#include <Eigen/Dense>
+
+namespace mppi::critics
+{
+
+void DirectionChangeCritic::initialize()
+{
+  auto getParentParam = parameters_handler_->getParamGetter(parent_name_);
+  auto getParam = parameters_handler_->getParamGetter(name_);
+  getParam(power_, "cost_power", 1);
+  getParam(weight_, "cost_weight", 5.0f);
+  getParam(
+    threshold_to_consider_,
+    "threshold_to_consider", 0.5f);
+
+  RCLCPP_INFO(
+    logger_, "DirectionChangeCritic instantiated with %d power and %f weight.", power_, weight_);
+}
+
+void DirectionChangeCritic::score(CriticData & data)
+{
+  if (!enabled_) {
+    return;
+  }
+
+  if (data.state.local_path_length < threshold_to_consider_) {
+    return;
+  }
+
+  // Penalize the magnitude of velocity difference when crossing zero (direction change)
+  // Calculate |vx - current_speed| only where signs differ, otherwise 0
+  // Use robot_speed from feedback, as it better represents the actual direction of motion
+  constexpr size_t penalize_up_to_idx = 2;
+  const float current_speed = data.state.robot_speed.linear.x;
+  // Process in-place using Eigen views to avoid allocations
+  auto vx_view = data.state.vx.leftCols(penalize_up_to_idx);
+
+  if (power_ > 1u) {
+    data.costs += ((vx_view * current_speed < 0.0f).select(
+        (vx_view - current_speed).abs(), 0.0f).rowwise().sum() * weight_).pow(power_);
+  } else {
+    data.costs += (vx_view * current_speed < 0.0f).select(
+      (vx_view - current_speed).abs(), 0.0f).rowwise().sum() * weight_;
+  }
+}
+
+}  // namespace mppi::critics
+
+#include <pluginlib/class_list_macros.hpp>
+
+PLUGINLIB_EXPORT_CLASS(
+  mppi::critics::DirectionChangeCritic,
+  mppi::critics::CriticFunction)