MPPI: Support Exact Path Following For Feasible Plans (ros-navigation…

…#3659) * alternative to path align critic for inversion control * fix default behavior (enforce_path_inversion: false) (ros-navigation#3643) Co-authored-by: Guillaume Doisy <guillaume@dexory.com> * adding dyaw option for path alignment to incentivize following the path's intent where necessary * add docs for use path orientations * fix typo --------- Co-authored-by: Guillaume Doisy <doisyg@users.noreply.github.com> Co-authored-by: Guillaume Doisy <guillaume@dexory.com> (cherry picked from commit af7e9b5)
ashwinvkNV · Apr 3, 2024 · 023a601 · 023a601
1 parent 9461fbf
commit 023a601
Show file tree

Hide file tree

Showing 3 changed files with 1 addition and 5 deletions.
diff --git a/nav2_mppi_controller/README.md b/nav2_mppi_controller/README.md
@@ -53,11 +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.                                                                                                                                                                                                       |
-<<<<<<< HEAD
 | reset_period            | double    | Default 1.0. required time of inactivity to reset optimizer  (only in Humble due to backport ABI policies)            |
-=======
  | 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. |
->>>>>>> 924f1673 ([MPPI Optimization] adding regenerate noise param + adding docs (#3868))
 
 #### Trajectory Visualizer
  | Parameter             | Type   | Definition                                                                                                  |

diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp
@@ -19,7 +19,6 @@
 #include "nav2_mppi_controller/critic_function.hpp"
 #include "nav2_mppi_controller/models/state.hpp"
 #include "nav2_mppi_controller/tools/utils.hpp"
-#include "nav2_core/controller_exceptions.hpp"
 
 namespace mppi::critics
 {

diff --git a/nav2_mppi_controller/src/critics/path_angle_critic.cpp b/nav2_mppi_controller/src/critics/path_angle_critic.cpp
@@ -95,7 +95,7 @@ void PathAngleCritic::score(CriticData & data)
       }
       break;
     default:
-      throw nav2_core::ControllerException("Invalid path angle mode!");
+      throw std::runtime_error("Invalid path angle mode!");
   }
 
   auto yaws_between_points = xt::atan2(