From 063a314b5913dc7277c6954867824c419c1d5352 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Fri, 23 Jun 2023 13:31:25 -0700 Subject: [PATCH] fixing path angle critic's non-directional bias (#3632) * fixing path angle critic's non-directional bias * adding reformat --- .../include/nav2_mppi_controller/tools/utils.hpp | 2 +- nav2_mppi_controller/src/critics/path_angle_critic.cpp | 9 +++++---- 2 files changed, 6 insertions(+), 5 deletions(-) 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 818d1fe357..4d2295b626 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp @@ -432,7 +432,7 @@ inline double posePointAngle( if (!forward_preference) { return std::min( abs(angles::shortest_angular_distance(yaw, pose_yaw)), - abs(angles::shortest_angular_distance(yaw, pose_yaw + M_PI))); + abs(angles::shortest_angular_distance(yaw, angles::normalize_angle(pose_yaw + M_PI)))); } return abs(angles::shortest_angular_distance(yaw, pose_yaw)); diff --git a/nav2_mppi_controller/src/critics/path_angle_critic.cpp b/nav2_mppi_controller/src/critics/path_angle_critic.cpp index 1225ccde83..85cae5838d 100644 --- a/nav2_mppi_controller/src/critics/path_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_angle_critic.cpp @@ -88,10 +88,11 @@ void PathAngleCritic::score(CriticData & data) xt::abs(utils::shortest_angular_distance(data.trajectories.yaws, yaws_between_points)); if (reversing_allowed_ && !forward_preference_) { - data.costs += xt::pow( - xt::mean( - xt::where(yaws < M_PI_2, yaws, utils::normalize_angles(yaws + M_PI)), - {1}, immediate) * weight_, power_); + const auto yaws_between_points_corrected = xt::where( + yaws < M_PI_2, yaws_between_points, utils::normalize_angles(yaws_between_points + M_PI)); + const auto corrected_yaws = xt::abs( + utils::shortest_angular_distance(data.trajectories.yaws, yaws_between_points_corrected)); + data.costs += xt::pow(xt::mean(corrected_yaws, {1}, immediate) * weight_, power_); } else { data.costs += xt::pow(xt::mean(yaws, {1}, immediate) * weight_, power_); }