Scale cost critic's weight when dynamically updated (#4246)

* Scale cost critic's weight when dynamically updated Signed-off-by: pepisg <[email protected]> * sign off Signed-off-by: pepisg <[email protected]> --------- Signed-off-by: pepisg <[email protected]>
ros-navigation · Apr 8, 2024 · f815e8b · f815e8b
1 parent 0be2f25
commit f815e8b
Showing 1 changed file with 6 additions and 0 deletions.
diff --git 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_);