Orientation check

Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>

planning/behavior_path_lane_change_module/src/utils/utils.cpp

@@ -17,6 +17,7 @@
 #include "behavior_path_lane_change_module/utils/data_structs.hpp"
 #include "behavior_path_lane_change_module/utils/path.hpp"
 #include "behavior_path_planner_common/marker_utils/utils.hpp"
+#include "tier4_autoware_utils/math/unit_conversion.hpp"
 #include "behavior_path_planner_common/parameters.hpp"
 #include "behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp"
 #include "behavior_path_planner_common/utils/path_shifter/path_shifter.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,21 @@ std::optional<LaneChangePath> constructCandidatePath(
     return std::nullopt;
   }
 
+  const auto & lane_change_start_point_from_prepare = prepare_segment.points.back().point.pose;
+
+  const auto & lane_change_start_point_from_prepare2 = 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_diff = std::abs(tier4_autoware_utils::normalizeRadian(
+    tf2::getYaw(lane_change_start_point_from_prepare.orientation) -
+    tf2::getYaw(lane_change_start_from_shifted.orientation)));
+  const auto yaw_diff2 = std::abs(tier4_autoware_utils::normalizeRadian(
+    tf2::getYaw(lane_change_start_point_from_prepare2.orientation) -
+    tf2::getYaw(lane_change_start_from_shifted.orientation)));
+  std::cerr << __func__ << " " << yaw_diff << " " << yaw_diff2 << '\n';
+  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;