diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp index feddd81a58..b21125f1a4 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp @@ -318,7 +318,7 @@ inline size_t findPathFurthestReachedPoint(const CriticData & data) min_id_by_path = 0; min_distance_by_path = std::numeric_limits::max(); for (size_t j = max_id_by_trajectories; j < dists.shape(1); j++) { - const float & cur_dist = dists(i, j); + const float cur_dist = dists(i, j); if (cur_dist < min_distance_by_path) { min_distance_by_path = cur_dist; min_id_by_path = j; diff --git a/nav2_mppi_controller/src/critics/constraint_critic.cpp b/nav2_mppi_controller/src/critics/constraint_critic.cpp index a270426038..cf485e05ea 100644 --- a/nav2_mppi_controller/src/critics/constraint_critic.cpp +++ b/nav2_mppi_controller/src/critics/constraint_critic.cpp @@ -52,13 +52,13 @@ void ConstraintCritic::score(CriticData & data) if (power_ > 1u) { data.costs += xt::pow( xt::sum( - (std::move(xt::maximum(data.state.vx - max_vel_, 0.0f)) + - std::move(xt::maximum(min_vel_ - data.state.vx, 0.0f))) * + (std::move(xt::maximum(data.state.vx - max_vel_, 0.0f) + + xt::maximum(min_vel_ - data.state.vx, 0.0f))) * data.model_dt, {1}, immediate) * weight_, power_); } else { data.costs += xt::sum( - (std::move(xt::maximum(data.state.vx - max_vel_, 0.0f)) + - std::move(xt::maximum(min_vel_ - data.state.vx, 0.0f))) * + (std::move(xt::maximum(data.state.vx - max_vel_, 0.0f) + + xt::maximum(min_vel_ - data.state.vx, 0.0f))) * data.model_dt, {1}, immediate) * weight_; } return; @@ -72,13 +72,13 @@ void ConstraintCritic::score(CriticData & data) if (power_ > 1u) { data.costs += xt::pow( xt::sum( - (std::move(xt::maximum(vel_total - max_vel_, 0.0f)) + - std::move(xt::maximum(min_vel_ - vel_total, 0.0f))) * + (std::move(xt::maximum(vel_total - max_vel_, 0.0f) + + xt::maximum(min_vel_ - vel_total, 0.0f))) * data.model_dt, {1}, immediate) * weight_, power_); } else { data.costs += xt::sum( - (std::move(xt::maximum(vel_total - max_vel_, 0.0f)) + - std::move(xt::maximum(min_vel_ - vel_total, 0.0f))) * + (std::move(xt::maximum(vel_total - max_vel_, 0.0f) + + xt::maximum(min_vel_ - vel_total, 0.0f))) * data.model_dt, {1}, immediate) * weight_; } return; @@ -94,15 +94,13 @@ void ConstraintCritic::score(CriticData & data) if (power_ > 1u) { data.costs += xt::pow( xt::sum( - (std::move(xt::maximum(data.state.vx - max_vel_, 0.0f)) + - std::move(xt::maximum(min_vel_ - data.state.vx, 0.0f)) + - std::move(out_of_turning_rad_motion)) * + (std::move(xt::maximum(data.state.vx - max_vel_, 0.0f) + + xt::maximum(min_vel_ - data.state.vx, 0.0f) + out_of_turning_rad_motion)) * data.model_dt, {1}, immediate) * weight_, power_); } else { data.costs += xt::sum( - (std::move(xt::maximum(data.state.vx - max_vel_, 0.0f)) + - std::move(xt::maximum(min_vel_ - data.state.vx, 0.0f)) + - std::move(out_of_turning_rad_motion)) * + (std::move(xt::maximum(data.state.vx - max_vel_, 0.0f) + + xt::maximum(min_vel_ - data.state.vx, 0.0f) + out_of_turning_rad_motion)) * data.model_dt, {1}, immediate) * weight_; } return; diff --git a/nav2_mppi_controller/src/critics/path_align_critic.cpp b/nav2_mppi_controller/src/critics/path_align_critic.cpp index 3ab8382462..7c21d8e4f5 100644 --- a/nav2_mppi_controller/src/critics/path_align_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_align_critic.cpp @@ -70,7 +70,6 @@ void PathAlignCritic::score(CriticData & data) } const size_t batch_size = data.trajectories.x.shape(0); - const size_t time_steps = data.trajectories.x.shape(1); auto && cost = xt::xtensor::from_shape({data.costs.shape(0)}); // Find integrated distance in the path @@ -96,13 +95,13 @@ void PathAlignCritic::score(CriticData & data) // Get strided trajectory information const auto T_x = xt::view( data.trajectories.x, xt::all(), - xt::range(trajectory_point_step_, time_steps, trajectory_point_step_)); + xt::range(trajectory_point_step_, _, trajectory_point_step_)); const auto T_y = xt::view( data.trajectories.y, xt::all(), - xt::range(trajectory_point_step_, time_steps, trajectory_point_step_)); + xt::range(trajectory_point_step_, _, trajectory_point_step_)); const auto T_yaw = xt::view( data.trajectories.yaws, xt::all(), - xt::range(trajectory_point_step_, time_steps, trajectory_point_step_)); + xt::range(trajectory_point_step_, _, trajectory_point_step_)); const auto traj_sampled_size = T_x.shape(1); for (size_t t = 0; t < batch_size; ++t) {