From 81f54802034c5879b0fc6c3b207b6e17ddecf863 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 11 Mar 2024 16:05:15 -0700 Subject: [PATCH] adding in path angle update --- .../src/critics/cost_critic.cpp | 8 +++---- .../src/critics/path_align_critic.cpp | 3 +-- .../src/critics/path_angle_critic.cpp | 21 ++++++++++++------- 3 files changed, 18 insertions(+), 14 deletions(-) diff --git a/nav2_mppi_controller/src/critics/cost_critic.cpp b/nav2_mppi_controller/src/critics/cost_critic.cpp index d22de10722..409f9df9d9 100644 --- a/nav2_mppi_controller/src/critics/cost_critic.cpp +++ b/nav2_mppi_controller/src/critics/cost_critic.cpp @@ -118,9 +118,9 @@ void CostCritic::score(CriticData & data) bool all_trajectories_collide = true; for (size_t i = 0; i < data.trajectories.x.shape(0); ++i) { bool trajectory_collide = false; - const auto & traj_x = xt::view(data.trajectories.x, i, xt::all()); - const auto & traj_y = xt::view(data.trajectories.y, i, xt::all()); - const auto & traj_yaw = xt::view(data.trajectories.yaws, i, xt::all()); + const auto traj_x = xt::view(data.trajectories.x, i, xt::all()); + const auto traj_y = xt::view(data.trajectories.y, i, xt::all()); + const auto traj_yaw = xt::view(data.trajectories.yaws, i, xt::all()); pose_cost = 0.0f; for (size_t j = 0; j < traj_len; j++) { @@ -157,7 +157,7 @@ void CostCritic::score(CriticData & data) } } - data.costs += xt::pow((weight_ * repulsive_cost / traj_len), power_); + data.costs += xt::pow((std::move(repulsive_cost) * (weight_ / static_cast(traj_len))), power_); data.fail_flag = all_trajectories_collide; } diff --git a/nav2_mppi_controller/src/critics/path_align_critic.cpp b/nav2_mppi_controller/src/critics/path_align_critic.cpp index e76fba1c8c..7dd8196a4b 100644 --- a/nav2_mppi_controller/src/critics/path_align_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_align_critic.cpp @@ -79,6 +79,7 @@ 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)}); + cost.fill(0.0f); // Find integrated distance in the path std::vector path_integrated_distances(path_segments_count, 0.0f); @@ -126,8 +127,6 @@ void PathAlignCritic::score(CriticData & data) } if (num_samples > 0) { cost[t] = summed_path_dist / num_samples; - } else { - cost[t] = 0.0f; } } diff --git a/nav2_mppi_controller/src/critics/path_angle_critic.cpp b/nav2_mppi_controller/src/critics/path_angle_critic.cpp index be86e42779..8f28b09ec3 100644 --- a/nav2_mppi_controller/src/critics/path_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_angle_critic.cpp @@ -99,24 +99,28 @@ void PathAngleCritic::score(CriticData & data) } auto yaws_between_points = xt::atan2( - goal_y - data.trajectories.y, - goal_x - data.trajectories.x); - - auto yaws = - xt::abs(utils::shortest_angular_distance(data.trajectories.yaws, yaws_between_points)); + goal_y - xt::view(data.trajectories.y, xt::all(), -1), + goal_x - xt::view(data.trajectories.x, xt::all(), -1)); switch (mode_) { case PathAngleMode::FORWARD_PREFERENCE: { - data.costs += xt::pow(xt::mean(yaws, {1}, immediate) * weight_, power_); + auto yaws = + xt::abs(utils::shortest_angular_distance( + xt::view(data.trajectories.yaws, xt::all(), -1), yaws_between_points)); + data.costs += xt::pow(yaws * weight_, power_); return; } case PathAngleMode::NO_DIRECTIONAL_PREFERENCE: { + auto yaws = + xt::abs(utils::shortest_angular_distance( + xt::view(data.trajectories.yaws, xt::all(), -1), yaws_between_points)); const auto yaws_between_points_corrected = xt::where( yaws < M_PIF_2, yaws_between_points, utils::normalize_angles(yaws_between_points + M_PIF)); const auto corrected_yaws = xt::abs( - utils::shortest_angular_distance(data.trajectories.yaws, yaws_between_points_corrected)); + utils::shortest_angular_distance( + xt::view(data.trajectories.yaws, xt::all(), -1), yaws_between_points_corrected)); data.costs += xt::pow(xt::mean(corrected_yaws, {1}, immediate) * weight_, power_); return; } @@ -126,7 +130,8 @@ void PathAngleCritic::score(CriticData & data) xt::abs(utils::shortest_angular_distance(yaws_between_points, goal_yaw)) < M_PIF_2, yaws_between_points, utils::normalize_angles(yaws_between_points + M_PIF)); const auto corrected_yaws = xt::abs( - utils::shortest_angular_distance(data.trajectories.yaws, yaws_between_points_corrected)); + utils::shortest_angular_distance( + xt::view(data.trajectories.yaws, xt::all(), -1), yaws_between_points_corrected)); data.costs += xt::pow(xt::mean(corrected_yaws, {1}, immediate) * weight_, power_); return; }