diff --git a/nav2_mppi_controller/README.md b/nav2_mppi_controller/README.md index ba0f0722e4..3aaf4a8049 100644 --- 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 index 090e11b34a..246a843796 100644 --- 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_);