From 73cd82aee75f8c40646d6ffe47a6295329b6db19 Mon Sep 17 00:00:00 2001 From: pepisg Date: Mon, 8 Apr 2024 17:05:26 +0000 Subject: [PATCH 1/2] Scale cost critic's weight when dynamically updated Signed-off-by: pepisg --- nav2_mppi_controller/src/critics/cost_critic.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/nav2_mppi_controller/src/critics/cost_critic.cpp b/nav2_mppi_controller/src/critics/cost_critic.cpp index 7a848fe7abb..f348d7780a1 100644 --- a/nav2_mppi_controller/src/critics/cost_critic.cpp +++ b/nav2_mppi_controller/src/critics/cost_critic.cpp @@ -34,6 +34,12 @@ void CostCritic::initialize() // Normalized by cost value to put in same regime as other weights weight_ /= 254.0f; + // Normalize weight when parameter is changed dynamically as well + auto weightDynamicCb = [&](const rclcpp::Parameter & weight) { + weight_ = weight.as_double() / 254.0f; + }; + parameters_handler_->addDynamicParamCallback(name_ + ".cost_weight", weightDynamicCb); + collision_checker_.setCostmap(costmap_); possible_collision_cost_ = findCircumscribedCost(costmap_ros_); From 3e071c250a8f979563cc387e7fe80ef20b2d922c Mon Sep 17 00:00:00 2001 From: pepisg Date: Mon, 8 Apr 2024 18:11:38 +0000 Subject: [PATCH 2/2] sign off Signed-off-by: pepisg