From 0dc16f48d43106a675669e36ef9d46619b510f3a Mon Sep 17 00:00:00 2001
From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com>
Date: Thu, 11 Jan 2024 00:32:30 +0900
Subject: [PATCH] feat(avoidance): check traffic light info in order to limit
 drivable area (#6016)

* feat(avoidance): don't use opposite lane before intersection

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* feat(avoidance): check traffic light info in order to limit drivable area

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

---------

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
---
 .../utils/traffic_light_utils.hpp             | 15 ++++++++++++++
 .../src/utils/traffic_light_utils.cpp         | 20 +++++++++++++++++++
 2 files changed, 35 insertions(+)

diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/traffic_light_utils.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/traffic_light_utils.hpp
index 6380830806c33..62fd87bbd91bc 100644
--- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/traffic_light_utils.hpp
+++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/traffic_light_utils.hpp
@@ -92,6 +92,21 @@ bool isStoppedAtRedTrafficLightWithinDistance(
   const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path,
   const std::shared_ptr<const PlannerData> & planner_data,
   const double distance_threshold = std::numeric_limits<double>::infinity());
+
+/**
+ * @brief Determines if a traffic signal indicates a stop for the given lanelet.
+ *
+ * Evaluates the current state of the traffic light, considering if it's green or unknown,
+ * which would not necessitate a stop. Then, it checks the turn direction attribute of the lanelet
+ * against the traffic light's arrow shapes to determine whether a vehicle must stop or if it can
+ * proceed based on allowed turn directions.
+ *
+ * @param lanelet The lanelet to check for a stop signal at its traffic light.
+ * @param planner_data Shared pointer to the planner data containing vehicle state information.
+ * @return True if the traffic signal indicates a stop is required, false otherwise.
+ */
+bool isTrafficSignalStop(
+  const lanelet::ConstLanelets & lanelets, const std::shared_ptr<const PlannerData> & planner_data);
 }  // namespace behavior_path_planner::utils::traffic_light
 
 #endif  // BEHAVIOR_PATH_PLANNER_COMMON__UTILS__TRAFFIC_LIGHT_UTILS_HPP_
diff --git a/planning/behavior_path_planner_common/src/utils/traffic_light_utils.cpp b/planning/behavior_path_planner_common/src/utils/traffic_light_utils.cpp
index f77fafb42d829..ecd02bfd63485 100644
--- a/planning/behavior_path_planner_common/src/utils/traffic_light_utils.cpp
+++ b/planning/behavior_path_planner_common/src/utils/traffic_light_utils.cpp
@@ -127,4 +127,24 @@ bool isStoppedAtRedTrafficLightWithinDistance(
   return (distance_to_red_traffic_light < distance_threshold);
 }
 
+bool isTrafficSignalStop(
+  const lanelet::ConstLanelets & lanelets, const std::shared_ptr<const PlannerData> & planner_data)
+{
+  for (const auto & lanelet : lanelets) {
+    for (const auto & element : lanelet.regulatoryElementsAs<TrafficLight>()) {
+      const auto traffic_signal_stamped = planner_data->getTrafficSignal(element->id());
+      if (!traffic_signal_stamped.has_value()) {
+        continue;
+      }
+
+      if (traffic_light_utils::isTrafficSignalStop(
+            lanelet, traffic_signal_stamped.value().signal)) {
+        return true;
+      }
+    }
+  }
+
+  return false;
+}
+
 }  // namespace behavior_path_planner::utils::traffic_light