Prevent MPPI controller from resetting speed limits upon goal execution. (#5165)

Signed-off-by: Leander Stephen D'Souza <[email protected]>

Author: leander-dsouza

nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp

@@ -120,8 +120,9 @@ class Optimizer
 
   /**
    * @brief Reset the optimization problem to initial conditions
+   * @param Whether to reset the constraints to its base values
    */
-  void reset();
+  void reset(bool reset_dynamic_speed_limits = true);
 
   /**
    * @brief Get the motion model time step

nav2_mppi_controller/src/controller.cpp

@@ -86,7 +86,7 @@ void MPPIController::deactivate()
 
 void MPPIController::reset()
 {
-  optimizer_.reset();
+  optimizer_.reset(false /*Don't reset zone-based speed limits between requests*/);
 }
 
 geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands(

nav2_mppi_controller/src/optimizer.cpp

@@ -132,7 +132,7 @@ void Optimizer::setOffset(double controller_frequency)
   }
 }
 
-void Optimizer::reset()
+void Optimizer::reset(bool reset_dynamic_speed_limits)
 {
   state_.reset(settings_.batch_size, settings_.time_steps);
   control_sequence_.reset(settings_.time_steps);
@@ -141,7 +141,9 @@ void Optimizer::reset()
   control_history_[2] = {0.0f, 0.0f, 0.0f};
   control_history_[3] = {0.0f, 0.0f, 0.0f};
 
-  settings_.constraints = settings_.base_constraints;
+  if (reset_dynamic_speed_limits) {
+    settings_.constraints = settings_.base_constraints;
+  }
 
   costs_.setZero(settings_.batch_size);
   generated_trajectories_.reset(settings_.batch_size, settings_.time_steps);