normalize weight

ros-navigation · Feb 1, 2024 · faa2b55 · faa2b55
1 parent 1664fce
commit faa2b55
Show file tree

Hide file tree

Showing 2 changed files with 5 additions and 2 deletions.
diff --git a/nav2_mppi_controller/README.md b/nav2_mppi_controller/README.md
@@ -119,7 +119,7 @@ Uses inflated costmap cost directly to avoid obstacles
  | Parameter            | Type   | Definition                                                                                                  |
  | ---------------      | ------ | ----------------------------------------------------------------------------------------------------------- |
  | consider_footprint   | bool   | Default: False. Whether to use point cost (if robot is circular or low compute power) or compute SE2 footprint cost. |
- | cost_weight          | double | Default 0.015. Wight to apply to critic to avoid obstacles. This is lower than others since it is normalized by cost range (ie 254 max cost, versus other critics using SI units)                                       | 
+ | cost_weight          | double | Default 3.81. Wight to apply to critic to avoid obstacles.                                       | 
  | cost_power           | int    | Default 1. Power order to apply to term.                                                                    |
  | collision_cost       | double | Default 1000000.0. Cost to apply to a true collision in a trajectory.                                          |
  | critical_cost       | double | Default 300.0. Cost to apply to a pose with any point in in inflated space to prefer distance from obstacles.                                          |

diff --git a/nav2_mppi_controller/src/critics/cost_critic.cpp b/nav2_mppi_controller/src/critics/cost_critic.cpp
@@ -24,12 +24,15 @@ void CostCritic::initialize()
   auto getParam = parameters_handler_->getParamGetter(name_);
   getParam(consider_footprint_, "consider_footprint", false);
   getParam(power_, "cost_power", 1);
-  getParam(weight_, "cost_weight", 0.015);
+  getParam(weight_, "cost_weight", 3.81);
   getParam(critical_cost_, "critical_cost", 300.0);
   getParam(collision_cost_, "collision_cost", 1000000.0);
   getParam(near_goal_distance_, "near_goal_distance", 0.5);
   getParam(inflation_layer_name_, "inflation_layer_name", std::string(""));
 
+  // Normalized by cost value to put in same regime as other weights
+  weight_ /= 254.0f;
+
   collision_checker_.setCostmap(costmap_);
   possibly_inscribed_cost_ = findCircumscribedCost(costmap_ros_);