fixing path angle critic's non-directional bias (ros-navigation#3632)

* fixing path angle critic's non-directional bias * adding reformat Signed-off-by: enricosutera <enricosutera@outlook.com>
enricosutera · May 19, 2024 · 8c41f28 · 8c41f28
1 parent 0e57cb8
commit 8c41f28
Show file tree

Hide file tree

Showing 2 changed files with 6 additions and 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
@@ -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
@@ -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_);
   }