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

…area (autowarefoundation#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>

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_

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