From 3043812c92483e1e423d8e8a5b4522254e44183e Mon Sep 17 00:00:00 2001
From: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
Date: Tue, 6 Feb 2024 20:50:10 +0900
Subject: [PATCH] fix(lane_change): orientation check when constructing path

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
---
 .../src/utils/utils.cpp                          | 16 ++++++++++++++++
 1 file changed, 16 insertions(+)

diff --git a/planning/behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_lane_change_module/src/utils/utils.cpp
index e6a162d2bdad5..a331d418d5e46 100644
--- a/planning/behavior_path_lane_change_module/src/utils/utils.cpp
+++ b/planning/behavior_path_lane_change_module/src/utils/utils.cpp
@@ -23,6 +23,7 @@
 #include "behavior_path_planner_common/utils/path_utils.hpp"
 #include "behavior_path_planner_common/utils/utils.hpp"
 #include "object_recognition_utils/predicted_path_utils.hpp"
+#include "tier4_autoware_utils/math/unit_conversion.hpp"
 
 #include <lanelet2_extension/utility/query.hpp>
 #include <lanelet2_extension/utility/utilities.hpp>
@@ -40,7 +41,9 @@
 #include <tf2_ros/transform_listener.h>
 
 #include <algorithm>
+#include <iterator>
 #include <limits>
+#include <optional>
 #include <string>
 #include <vector>
 
@@ -385,6 +388,19 @@ std::optional<LaneChangePath> constructCandidatePath(
     return std::nullopt;
   }
 
+  if (prepare_segment.points.size() > 1 && shifted_path.path.points.size() > 1) {
+    const auto & prepare_segment_second_last_point =
+      std::prev(prepare_segment.points.end() - 1)->point.pose;
+    const auto & lane_change_start_from_shifted =
+      std::next(shifted_path.path.points.begin())->point.pose;
+    const auto yaw_diff2 = std::abs(tier4_autoware_utils::normalizeRadian(
+      tf2::getYaw(prepare_segment_second_last_point.orientation) -
+      tf2::getYaw(lane_change_start_from_shifted.orientation)));
+    if (yaw_diff2 > tier4_autoware_utils::deg2rad(5.0)) {
+      return std::nullopt;
+    }
+  }
+
   candidate_path.path = utils::combinePath(prepare_segment, shifted_path.path);
   candidate_path.shifted_path = shifted_path;