From d01ae0d5e4cf990f752f75d9c16dfadb53e22a58 Mon Sep 17 00:00:00 2001
From: Yuki TAKAGI <141538661+yuki-takagi-66@users.noreply.github.com>
Date: Fri, 11 Oct 2024 13:15:16 +0900
Subject: [PATCH] fix(crosswalk): fix passing direction calclation for the
 objects (#9071)

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>
---
 .../src/scene_crosswalk.cpp                   | 33 ++++++++-----------
 1 file changed, 13 insertions(+), 20 deletions(-)

diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp
index f56552485b5fc..4ec3e43ee91ca 100644
--- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp
+++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp
@@ -353,13 +353,16 @@ std::optional<StopFactor> CrosswalkModule::checkStopForCrosswalkUsers(
       if (collision_state != CollisionState::YIELD) {
         continue;
       }
+
       if (
         isVehicleType(object.classification) && ego_crosswalk_passage_direction &&
         collision_point.crosswalk_passage_direction) {
-        const double direction_diff = autoware::universe_utils::normalizeRadian(
+        double direction_diff = std::abs(std::fmod(
           collision_point.crosswalk_passage_direction.value() -
-          ego_crosswalk_passage_direction.value());
-        if (std::fabs(direction_diff) < planner_param_.vehicle_object_cross_angle_threshold) {
+            ego_crosswalk_passage_direction.value(),
+          M_PI_2));
+        direction_diff = std::min(direction_diff, M_PI_2 - direction_diff);
+        if (direction_diff < planner_param_.vehicle_object_cross_angle_threshold) {
           continue;
         }
       }
@@ -670,8 +673,8 @@ std::pair<double, double> CrosswalkModule::clampAttentionRangeByNeighborCrosswal
 std::optional<double> CrosswalkModule::findEgoPassageDirectionAlongPath(
   const PathWithLaneId & path) const
 {
-  auto findIntersectPoint = [&](const lanelet::ConstLineString3d line)
-    -> std::optional<std::pair<size_t, geometry_msgs::msg::Point>> {
+  auto findIntersectPoint =
+    [&](const lanelet::ConstLineString3d line) -> std::optional<geometry_msgs::msg::Point> {
     const auto line_start =
       autoware::universe_utils::createPoint(line.front().x(), line.front().y(), line.front().z());
     const auto line_end =
@@ -682,29 +685,19 @@ std::optional<double> CrosswalkModule::findEgoPassageDirectionAlongPath(
       if (const auto intersect =
             autoware::universe_utils::intersect(line_start, line_end, start, end);
           intersect.has_value()) {
-        return std::make_optional(std::make_pair(i, intersect.value()));
+        return intersect;
       }
     }
     return std::nullopt;
   };
-  const auto intersect_pt1 = findIntersectPoint(crosswalk_.leftBound());
-  const auto intersect_pt2 = findIntersectPoint(crosswalk_.rightBound());
+  const auto pt1 = findIntersectPoint(crosswalk_.leftBound());
+  const auto pt2 = findIntersectPoint(crosswalk_.rightBound());
 
-  if (!intersect_pt1 || !intersect_pt2) {
+  if (!pt1 || !pt2) {
     return std::nullopt;
   }
 
-  const auto idx1 = intersect_pt1.value().first;
-  const auto idx2 = intersect_pt2.value().first;
-
-  const auto min_idx = std::min(idx1, idx2);
-  const auto dist1 = calcSignedArcLength(path.points, min_idx, intersect_pt1.value().second);
-  const auto dist2 = calcSignedArcLength(path.points, min_idx, intersect_pt2.value().second);
-
-  const auto & front = dist1 > dist2 ? intersect_pt2.value().second : intersect_pt1.value().second;
-  const auto & back = dist1 > dist2 ? intersect_pt1.value().second : intersect_pt2.value().second;
-
-  return std::atan2(back.y - front.y, back.x - front.x);
+  return std::atan2(pt2->y - pt1->y, pt2->x - pt1->x);
 }
 
 std::optional<double> CrosswalkModule::findObjectPassageDirectionAlongVehicleLane(