From 50f902d080e9e11174cdbf8e0fd9764ec114f492 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Fri, 10 Feb 2023 20:54:07 +0900 Subject: [PATCH] fix(behavior_path_planner): fix geometric parallel parking guard (#2857) (#276) Signed-off-by: kosuke55 --- .../src/scene_module/utils/geometric_parallel_parking.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 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 index 6da97e1cb510..eb0203908b9d 100644 --- 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 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{}; @@ -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;