diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp index e0821e548d6..2ced1fe2fc4 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp +++ b/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 diff --git a/nav2_mppi_controller/src/controller.cpp b/nav2_mppi_controller/src/controller.cpp index 57effc952ee..81f8aa5c1c0 100644 --- a/nav2_mppi_controller/src/controller.cpp +++ b/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( diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index 6acbd6223c0..b03160cfd6a 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/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);