[MPPI Optimization] adding regenerate noise param + adding docs (ros-…

…navigation#3868)

* adding regenerate noise param + adding docs

* fix tests

* remove unnecessary normalization

* Update optimizer.cpp

nav2_mppi_controller/CMakeLists.txt

@@ -6,7 +6,10 @@ add_definitions(-DXTENSOR_USE_XSIMD)
 
 set(XTENSOR_USE_TBB 0)
 set(XTENSOR_USE_OPENMP 0)
+set(XTENSOR_USE_XSIMD 1)
 
+# set(XTENSOR_DEFAULT_LAYOUT column_major)  # row_major, column_major
+# set(XTENSOR_DEFAULT_TRAVERSAL row_major)  # row_major, column_major
 
 find_package(ament_cmake REQUIRED)
 find_package(xtensor REQUIRED)
@@ -86,7 +89,7 @@ set(libraries mppi_controller mppi_critics)
 
 foreach(lib IN LISTS libraries)
   target_compile_options(${lib} PUBLIC -fconcepts)
-  target_include_directories(${lib} PUBLIC include ${xsimd_INCLUDE_DIRS} ${OpenMP_INCLUDE_DIRS})
+  target_include_directories(${lib} PUBLIC include ${xsimd_INCLUDE_DIRS})  #  ${OpenMP_INCLUDE_DIRS}
   target_link_libraries(${lib} xtensor xtensor::optimize xtensor::use_xsimd)
   ament_target_dependencies(${lib} ${dependencies_pkgs})
 endforeach()

nav2_mppi_controller/README.md

@@ -29,6 +29,7 @@ This process is then repeated a number of times and returns a converged solution
 - Includes fallback mechanisms to handle soft-failures before escalating to recovery behaviors
 - High-quality code implementation with Doxygen, high unit test coverage, documentation, and parameter guide
 - Easily extensible to support modern research variants of MPPI
+- Comes pre-tuned for good out-of-the-box behavior
 
 ## Configuration
 
@@ -52,6 +53,8 @@ This process is then repeated a number of times and returns a converged solution
  | gamma                      | double | Default: 0.015. A trade-off between smoothness (high) and low energy (low). This is a complex parameter that likely won't need to be changed from the default of `0.1` which works well for a broad range of cases. See Section 3D-2 in "Information Theoretic Model Predictive Control: Theory and Applications to Autonomous Driving" for detailed information.       |
  | visualize                  | bool   | Default: false. Publish visualization of trajectories, which can slow down the controller significantly. Use only for debugging.                                                                                                                                       |
  | retry_attempt_limit        | int    | Default 1. Number of attempts to find feasible trajectory on failure for soft-resets before reporting failure.                                                                                                                                                                                                       |
+ | regenerate_noises          | bool   | Default false. Whether to regenerate noises each iteration or use single noise distribution computed on initialization and reset. Practically, this is found to work fine since the trajectories are being sampled stochastically from a normal distribution and reduces compute jittering at run-time due to thread wake-ups to resample normal distribution. |
+
 #### Trajectory Visualizer
  | Parameter             | Type   | Definition                                                                                                  |
  | ---------------       | ------ | ----------------------------------------------------------------------------------------------------------- |
@@ -254,6 +257,8 @@ The most common parameters you might want to start off changing are the velocity
 
 If you don't require path following behavior (e.g. just want to follow a goal pose and let the model predictive elements decide the best way to accomplish that), you may easily remove the PathAlign, PathFollow and PathAngle critics. 
 
+By default, the controller is tuned and has the capabilities established in the PathAlign/Obstacle critics to generally follow the path closely when no obstacles prevent it, but able to deviate from the path when blocked. See `PathAlignCritic::score()` for details, but it is disabled when the local path is blocked so the obstacle critic takes over in that state.
+
 ### Prediction Horizon, Costmap Sizing, and Offsets
 
 As this is a predictive planner, there is some relationship between maximum speed, prediction times, and costmap size that users should keep in mind while tuning for their application. If a controller server costmap is set to 3.0m in size, that means that with the robot in the center, there is 1.5m of information on either side of the robot. When your prediction horizon (time_steps * model_dt) at maximum speed (vx_max) is larger than this, then your robot will be artificially limited in its maximum speeds and behavior by the costmap limitation. For example, if you predict forward 3 seconds (60 steps @ 0.05s per step) at 0.5m/s maximum speed, the **minimum** required costmap radius is 1.5m - or 3m total width.

nav2_mppi_controller/include/nav2_mppi_controller/tools/noise_generator.hpp

@@ -25,8 +25,9 @@
 #include <xtensor/xview.hpp>
 
 #include "nav2_mppi_controller/models/optimizer_settings.hpp"
-#include <nav2_mppi_controller/models/control_sequence.hpp>
-#include <nav2_mppi_controller/models/state.hpp>
+#include "nav2_mppi_controller/tools/parameters_handler.hpp"
+#include "nav2_mppi_controller/models/control_sequence.hpp"
+#include "nav2_mppi_controller/models/state.hpp"
 
 namespace mppi
 {
@@ -47,8 +48,12 @@ class NoiseGenerator
    * @brief Initialize noise generator with settings and model types
    * @param settings Settings of controller
    * @param is_holonomic If base is holonomic
+   * @param name Namespace for configs
+   * @param param_handler Get parameters util
    */
-  void initialize(mppi::models::OptimizerSettings & settings, bool is_holonomic);
+  void initialize(
+    mppi::models::OptimizerSettings & settings,
+    bool is_holonomic, const std::string & name, ParametersHandler * param_handler);
 
   /**
    * @brief Shutdown noise generator thread
@@ -99,7 +104,7 @@ class NoiseGenerator
   std::thread noise_thread_;
   std::condition_variable noise_cond_;
   std::mutex noise_lock_;
-  bool active_{false}, ready_{false};
+  bool active_{false}, ready_{false}, regenerate_noises_{false};
 };
 
 }  // namespace mppi

nav2_mppi_controller/src/critics/goal_angle_critic.cpp

@@ -35,11 +35,7 @@ void GoalAngleCritic::initialize()
 
 void GoalAngleCritic::score(CriticData & data)
 {
-  if (!enabled_) {
-    return;
-  }
-
-  if (!utils::withinPositionGoalTolerance(
+  if (!enabled_ || !utils::withinPositionGoalTolerance(
       threshold_to_consider_, data.state.pose.pose, data.path))
   {
     return;

nav2_mppi_controller/src/critics/goal_critic.cpp

@@ -35,11 +35,7 @@ void GoalCritic::initialize()
 
 void GoalCritic::score(CriticData & data)
 {
-  if (!enabled_) {
-    return;
-  }
-
-  if (!utils::withinPositionGoalTolerance(
+  if (!enabled_ || !utils::withinPositionGoalTolerance(
       threshold_to_consider_, data.state.pose.pose, data.path))
   {
     return;

nav2_mppi_controller/src/critics/obstacles_critic.cpp

@@ -152,7 +152,7 @@ void ObstaclesCritic::score(CriticData & data)
     }
 
     if (!trajectory_collide) {all_trajectories_collide = false;}
-    raw_cost[i] = static_cast<float>(trajectory_collide ? collision_cost_ : traj_cost);
+    raw_cost[i] = trajectory_collide ? collision_cost_ : traj_cost;
   }
 
   data.costs += xt::pow(

nav2_mppi_controller/src/critics/prefer_forward_critic.cpp

@@ -33,12 +33,9 @@ void PreferForwardCritic::initialize()
 void PreferForwardCritic::score(CriticData & data)
 {
   using xt::evaluation_strategy::immediate;
-
-  if (!enabled_) {
-    return;
-  }
-
-  if (utils::withinPositionGoalTolerance(threshold_to_consider_, data.state.pose.pose, data.path)) {
+  if (!enabled_ ||
+    utils::withinPositionGoalTolerance(threshold_to_consider_, data.state.pose.pose, data.path))
+  {
     return;
   }
 

nav2_mppi_controller/src/critics/twirling_critic.cpp

@@ -31,11 +31,9 @@ void TwirlingCritic::initialize()
 void TwirlingCritic::score(CriticData & data)
 {
   using xt::evaluation_strategy::immediate;
-  if (!enabled_) {
-    return;
-  }
-
-  if (utils::withinPositionGoalTolerance(data.goal_checker, data.state.pose.pose, data.path)) {
+  if (!enabled_ ||
+    utils::withinPositionGoalTolerance(data.goal_checker, data.state.pose.pose, data.path))
+  {
     return;
   }
 

nav2_mppi_controller/src/noise_generator.cpp

@@ -23,12 +23,22 @@
 namespace mppi
 {
 
-void NoiseGenerator::initialize(mppi::models::OptimizerSettings & settings, bool is_holonomic)
+void NoiseGenerator::initialize(
+  mppi::models::OptimizerSettings & settings, bool is_holonomic,
+  const std::string & name, ParametersHandler * param_handler)
 {
   settings_ = settings;
   is_holonomic_ = is_holonomic;
   active_ = true;
-  noise_thread_ = std::thread(std::bind(&NoiseGenerator::noiseThread, this));
+
+  auto getParam = param_handler->getParamGetter(name);
+  getParam(regenerate_noises_, "regenerate_noises", false);
+
+  if (regenerate_noises_) {
+    noise_thread_ = std::thread(std::bind(&NoiseGenerator::noiseThread, this));
+  } else {
+    generateNoisedControls();
+  }
 }
 
 void NoiseGenerator::shutdown()
@@ -44,7 +54,7 @@ void NoiseGenerator::shutdown()
 void NoiseGenerator::generateNextNoises()
 {
   // Trigger the thread to run in parallel to this iteration
-  // to generate the next iteration's noises.
+  // to generate the next iteration's noises (if applicable).
   {
     std::unique_lock<std::mutex> guard(noise_lock_);
     ready_ = true;
@@ -76,7 +86,12 @@ void NoiseGenerator::reset(mppi::models::OptimizerSettings & settings, bool is_h
     xt::noalias(noises_wz_) = xt::zeros<float>({settings_.batch_size, settings_.time_steps});
     ready_ = true;
   }
-  noise_cond_.notify_all();
+
+  if (regenerate_noises_) {
+    noise_cond_.notify_all();
+  } else {
+    generateNoisedControls();
+  }
 }
 
 void NoiseGenerator::noiseThread()

nav2_mppi_controller/src/optimizer.cpp

@@ -19,6 +19,7 @@
 #include <stdexcept>
 #include <string>
 #include <vector>
+#include <cmath>
 #include <xtensor/xmath.hpp>
 #include <xtensor/xrandom.hpp>
 #include <xtensor/xnoalias.hpp>
@@ -49,7 +50,7 @@ void Optimizer::initialize(
   getParams();
 
   critic_manager_.on_configure(parent_, name_, costmap_ros_, parameters_handler_);
-  noise_generator_.initialize(settings_, isHolonomic());
+  noise_generator_.initialize(settings_, isHolonomic(), name_, parameters_handler_);
 
   reset();
 }
@@ -268,7 +269,7 @@ void Optimizer::integrateStateVelocities(
   xt::xtensor<float, 2> & trajectory,
   const xt::xtensor<float, 2> & sequence) const
 {
-  double initial_yaw = tf2::getYaw(state_.pose.pose.orientation);
+  float initial_yaw = tf2::getYaw(state_.pose.pose.orientation);
 
   const auto vx = xt::view(sequence, xt::all(), 0);
   const auto vy = xt::view(sequence, xt::all(), 2);
@@ -278,8 +279,7 @@ void Optimizer::integrateStateVelocities(
   auto traj_y = xt::view(trajectory, xt::all(), 1);
   auto traj_yaws = xt::view(trajectory, xt::all(), 2);
 
-  xt::noalias(traj_yaws) =
-    utils::normalize_angles(xt::cumsum(wz * settings_.model_dt, 0) + initial_yaw);
+  xt::noalias(traj_yaws) = xt::cumsum(wz * settings_.model_dt, 0 + initial_yaw);
 
   auto && yaw_cos = xt::xtensor<float, 1>::from_shape(traj_yaws.shape());
   auto && yaw_sin = xt::xtensor<float, 1>::from_shape(traj_yaws.shape());
@@ -307,10 +307,10 @@ void Optimizer::integrateStateVelocities(
   models::Trajectories & trajectories,
   const models::State & state) const
 {
-  const double initial_yaw = tf2::getYaw(state.pose.pose.orientation);
+  const float initial_yaw = tf2::getYaw(state.pose.pose.orientation);
 
   xt::noalias(trajectories.yaws) =
-    utils::normalize_angles(xt::cumsum(state.wz * settings_.model_dt, 1) + initial_yaw);
+    xt::cumsum(state.wz * settings_.model_dt, 1) + initial_yaw;
 
   const auto yaws_cutted = xt::view(trajectories.yaws, xt::all(), xt::range(0, -1));
 

nav2_mppi_controller/test/noise_generator_test.cpp

@@ -42,13 +42,21 @@ TEST(NoiseGeneratorTest, NoiseGeneratorLifecycle)
   settings.batch_size = 100;
   settings.time_steps = 25;
 
-  generator.initialize(settings, false);
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("node");
+  node->declare_parameter("test_name.regenerate_noises", rclcpp::ParameterValue(false));
+  ParametersHandler handler(node);
+
+  generator.initialize(settings, false, "test_name", &handler);
+  generator.reset(settings, false);
   generator.shutdown();
 }
 
 TEST(NoiseGeneratorTest, NoiseGeneratorMain)
 {
   // Tests shuts down internal thread cleanly
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("node");
+  node->declare_parameter("test_name.regenerate_noises", rclcpp::ParameterValue(true));
+  ParametersHandler handler(node);
   NoiseGenerator generator;
   mppi::models::OptimizerSettings settings;
   settings.batch_size = 100;
@@ -70,7 +78,7 @@ TEST(NoiseGeneratorTest, NoiseGeneratorMain)
   state.reset(settings.batch_size, settings.time_steps);
 
   // Request an update with no noise yet generated, should result in identical outputs
-  generator.initialize(settings, false);
+  generator.initialize(settings, false, "test_name", &handler);
   generator.reset(settings, false);  // sets initial sizing and zeros out noises
   generator.setNoisedControls(state, control_sequence);
   EXPECT_EQ(state.cvx(0), 0);