From 858aefa745e0bf02e8b67ad86ee86a432c66c9cd Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Mon, 8 Dec 2025 11:44:04 +0100 Subject: [PATCH 1/4] [mppi] Don't reset zone-based speed limits Signed-off-by: Adi Vardi --- nav2_mppi_controller/src/optimizer.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index 8a5ab45d8b9..eeaf19260c3 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -122,7 +122,8 @@ void Optimizer::getParams() s.constraints = s.base_constraints; setMotionModel(motion_model_name); - parameters_handler_->addPostCallback([this]() {reset();}); + // Don't reset zone-based speed limits after params changes + parameters_handler_->addPostCallback([this]() { reset(false); }); double controller_frequency; getParentParam(controller_frequency, "controller_frequency", 0.0, ParameterType::Static); @@ -245,7 +246,7 @@ bool Optimizer::fallback(bool fail) return false; } - reset(); + reset(false /*Don't reset zone-based speed limits after fallback*/); if (++counter > settings_.retry_attempt_limit) { counter = 0; From 7b845a41fed8dfca7158ff9d4672651e4c706f4e Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Mon, 8 Dec 2025 16:19:23 +0100 Subject: [PATCH 2/4] [mppi] update motion model params from speed_limit Signed-off-by: Adi Vardi --- nav2_mppi_controller/src/optimizer.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index eeaf19260c3..1e751daf4e5 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -585,6 +585,7 @@ void Optimizer::setSpeedLimit(double speed_limit, bool percentage) s.constraints.wz = s.base_constraints.wz * ratio; } } + motion_model_->initialize(settings_.constraints, settings_.model_dt); } models::Trajectories & Optimizer::getGeneratedTrajectories() From d242f9d9b735125a71244bacba8a6339607a4ed2 Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Mon, 8 Dec 2025 16:25:06 +0100 Subject: [PATCH 3/4] fix linting issue Signed-off-by: Adi Vardi --- nav2_mppi_controller/src/optimizer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index 1e751daf4e5..e5a6da2c52c 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -123,7 +123,7 @@ void Optimizer::getParams() setMotionModel(motion_model_name); // Don't reset zone-based speed limits after params changes - parameters_handler_->addPostCallback([this]() { reset(false); }); + parameters_handler_->addPostCallback([this]() {reset(false);}); double controller_frequency; getParentParam(controller_frequency, "controller_frequency", 0.0, ParameterType::Static); From 63fdd88db8e4d4db1c05cdc5a1dedbdbb15454c2 Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Tue, 16 Dec 2025 14:02:40 +0100 Subject: [PATCH 4/4] Revert: reset speed limits after param change Signed-off-by: Adi Vardi --- nav2_mppi_controller/src/optimizer.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index e5a6da2c52c..ff3cc446163 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -122,8 +122,7 @@ void Optimizer::getParams() s.constraints = s.base_constraints; setMotionModel(motion_model_name); - // Don't reset zone-based speed limits after params changes - parameters_handler_->addPostCallback([this]() {reset(false);}); + parameters_handler_->addPostCallback([this]() {reset();}); double controller_frequency; getParentParam(controller_frequency, "controller_frequency", 0.0, ParameterType::Static);