diff --git a/nav2_mppi_controller/src/critics/constraint_critic.cpp b/nav2_mppi_controller/src/critics/constraint_critic.cpp index 886c0e4b05..fc26f89d11 100644 --- a/nav2_mppi_controller/src/critics/constraint_critic.cpp +++ b/nav2_mppi_controller/src/critics/constraint_critic.cpp @@ -65,6 +65,7 @@ void ConstraintCritic::score(CriticData & data) std::move(out_of_min_bounds_motion) + std::move(out_of_turning_rad_motion)) * data.model_dt, {1}, immediate) * weight_, power_); + return; } data.costs += xt::pow(