Scale cost critic's weight when dynamically updated (ros-navigation#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]>
Signed-off-by: stevedanomodolor <[email protected]>

Author: pepisg

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_);