fix(behavior_path_planner): fix geometric parallel parking guard (aut…

…owarefoundation#2857) (#276) Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
tier4 · Feb 10, 2023 · 50f902d · 50f902d
1 parent 2598acb
commit 50f902d
Showing 1 changed file with 4 additions and 4 deletions.
diff --git a/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp b/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp
@@ -139,8 +139,8 @@ std::vector<PathWithLaneId> GeometricParallelParking::generatePullOverPaths(
   // check the continuity of straight path and arc path
   const Pose & road_path_last_pose = straight_path.points.back().point.pose;
   const Pose & arc_path_first_pose = arc_paths.front().points.front().point.pose;
-  const double yaw_diff = tier4_autoware_utils::normalizeRadian(
-    tf2::getYaw(road_path_last_pose.orientation), tf2::getYaw(arc_path_first_pose.orientation));
+  const double yaw_diff = std::abs(tier4_autoware_utils::normalizeRadian(
+    tf2::getYaw(road_path_last_pose.orientation) - tf2::getYaw(arc_path_first_pose.orientation)));
   const double distance = calcDistance2d(road_path_last_pose, arc_path_first_pose);
   if (yaw_diff > tier4_autoware_utils::deg2rad(5.0) || distance > 0.1) {
     return std::vector<PathWithLaneId>{};
@@ -278,8 +278,8 @@ bool GeometricParallelParking::planPullOut(
     // check the continuity of straight path and arc path
     const Pose & road_path_first_pose = road_center_line_path.points.front().point.pose;
     const Pose & arc_path_last_pose = arc_paths.back().points.back().point.pose;
-    const double yaw_diff = tier4_autoware_utils::normalizeRadian(
-      tf2::getYaw(road_path_first_pose.orientation), tf2::getYaw(arc_path_last_pose.orientation));
+    const double yaw_diff = std::abs(tier4_autoware_utils::normalizeRadian(
+      tf2::getYaw(road_path_first_pose.orientation) - tf2::getYaw(arc_path_last_pose.orientation)));
     const double distance = calcDistance2d(road_path_first_pose, arc_path_last_pose);
     if (yaw_diff > tier4_autoware_utils::deg2rad(5.0) || distance > 0.1) {
       continue;