feat(intersection): memorize last observed valid traffic light information

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

Author: soblin

planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

@@ -18,6 +18,7 @@
 
 #include <behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp>  // for toGeomPoly
 #include <behavior_velocity_planner_common/utilization/util.hpp>
+#include <lanelet2_extension/regulatory_elements/autoware_traffic_light.hpp>
 #include <lanelet2_extension/utility/utilities.hpp>
 #include <motion_utils/trajectory/trajectory.hpp>
 #include <tier4_autoware_utils/geometry/boost_polygon_utils.hpp>  // for toPolygon2d
@@ -1143,25 +1144,11 @@ void IntersectionModule::reactRTCApproval(
 bool IntersectionModule::isGreenSolidOn() const
 {
   using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement;
-  const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr();
-  const auto & lane = lanelet_map_ptr->laneletLayer.get(lane_id_);
 
-  std::optional<lanelet::Id> tl_id = std::nullopt;
-  for (auto && tl_reg_elem : lane.regulatoryElementsAs<lanelet::TrafficLight>()) {
-    tl_id = tl_reg_elem->id();
-    break;
-  }
-  if (!tl_id) {
-    // this lane has no traffic light
+  if (!last_tl_valid_observation_) {
     return false;
   }
-  const auto tl_info_opt = planner_data_->getTrafficSignal(
-    tl_id.value(), true /* traffic light module keeps last observation*/);
-  if (!tl_info_opt) {
-    // the info of this traffic light is not available
-    return false;
-  }
-  const auto & tl_info = tl_info_opt.value();
+  const auto & tl_info = last_tl_valid_observation_.value();
   for (auto && tl_light : tl_info.signal.elements) {
     if (
       tl_light.color == TrafficSignalElement::GREEN &&
@@ -1176,38 +1163,81 @@ IntersectionModule::TrafficPrioritizedLevel IntersectionModule::getTrafficPriori
 {
   using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement;
 
+  auto corresponding_arrow = [&](const TrafficSignalElement & element) {
+    if (turn_direction_ == "straight" && element.shape == TrafficSignalElement::UP_ARROW) {
+      return true;
+    }
+    if (turn_direction_ == "left" && element.shape == TrafficSignalElement::LEFT_ARROW) {
+      return true;
+    }
+    if (turn_direction_ == "right" && element.shape == TrafficSignalElement::RIGHT_ARROW) {
+      return true;
+    }
+    return false;
+  };
+  auto level = TrafficPrioritizedLevel::NOT_PRIORITIZED;
+  auto color = TrafficSignalElement::GREEN;
+  if (last_tl_valid_observation_) {
+    const auto & tl_info = last_tl_valid_observation_.value();
+    bool has_amber_signal{false};
+    for (auto && tl_light : tl_info.signal.elements) {
+      if (
+        tl_light.color == TrafficSignalElement::AMBER &&
+        tl_light.shape == TrafficSignalElement::CIRCLE) {
+        has_amber_signal = true;
+      }
+      if (
+        (tl_light.color == TrafficSignalElement::RED &&
+         tl_light.shape == TrafficSignalElement::CIRCLE) ||
+        (tl_light.color == TrafficSignalElement::GREEN && corresponding_arrow(tl_light))) {
+        // NOTE: Return here since the red signal has the highest priority.
+        level = TrafficPrioritizedLevel::FULLY_PRIORITIZED;
+        color = TrafficSignalElement::RED;
+        break;
+      }
+    }
+    if (has_amber_signal) {
+      level = TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED;
+      color = TrafficSignalElement::AMBER;
+    }
+  }
+  if (tl_id_and_point_) {
+    const auto [tl_id, point] = tl_id_and_point_.value();
+    debug_data_.traffic_light_observation =
+      std::make_tuple(planner_data_->current_odometry->pose, point, color);
+  }
+  return level;
+}
+
+void IntersectionModule::updateTrafficSignalObservation()
+{
   const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr();
   const auto & lane = lanelet_map_ptr->laneletLayer.get(lane_id_);
 
-  std::optional<lanelet::Id> tl_id = std::nullopt;
-  for (auto && tl_reg_elem : lane.regulatoryElementsAs<lanelet::TrafficLight>()) {
-    tl_id = tl_reg_elem->id();
-    break;
+  if (!tl_id_and_point_) {
+    for (auto && tl_reg_elem :
+         lane.regulatoryElementsAs<lanelet::autoware::AutowareTrafficLight>()) {
+      for (const auto & ls : tl_reg_elem->lightBulbs()) {
+        if (ls.hasAttribute("traffic_light_id")) {
+          tl_id_and_point_ = std::make_pair(tl_reg_elem->id(), ls.front());
+          break;
+        }
+      }
+    }
   }
-  if (!tl_id) {
+  if (!tl_id_and_point_) {
     // this lane has no traffic light
-    return TrafficPrioritizedLevel::NOT_PRIORITIZED;
+    return;
   }
-  const auto tl_info_opt = planner_data_->getTrafficSignal(
-    tl_id.value(), true /* traffic light module keeps last observation*/);
+  const auto [tl_id, point] = tl_id_and_point_.value();
+  const auto tl_info_opt =
+    planner_data_->getTrafficSignal(tl_id, true /* traffic light module keeps last observation*/);
   if (!tl_info_opt) {
-    return TrafficPrioritizedLevel::NOT_PRIORITIZED;
+    // the info of this traffic light is not available
+    return;
   }
   const auto & tl_info = tl_info_opt.value();
-  bool has_amber_signal{false};
-  for (auto && tl_light : tl_info.signal.elements) {
-    if (tl_light.color == TrafficSignalElement::AMBER) {
-      has_amber_signal = true;
-    }
-    if (tl_light.color == TrafficSignalElement::RED) {
-      // NOTE: Return here since the red signal has the highest priority.
-      return TrafficPrioritizedLevel::FULLY_PRIORITIZED;
-    }
-  }
-  if (has_amber_signal) {
-    return TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED;
-  }
-  return TrafficPrioritizedLevel::NOT_PRIORITIZED;
+  last_tl_valid_observation_ = tl_info;
 }
 
 IntersectionModule::PassJudgeStatus IntersectionModule::isOverPassJudgeLinesStatus(

planning/behavior_velocity_intersection_module/src/scene_intersection.hpp

@@ -209,6 +209,9 @@ class IntersectionModule : public SceneModuleInterface
     std::optional<std::pair<geometry_msgs::msg::Point, geometry_msgs::msg::Point>>
       nearest_occlusion_projection{std::nullopt};
     std::optional<double> static_occlusion_with_traffic_light_timeout{std::nullopt};
+
+    std::optional<std::tuple<geometry_msgs::msg::Pose, lanelet::ConstPoint3d, uint8_t>>
+      traffic_light_observation{std::nullopt};
   };
 
   using TimeDistanceArray = std::vector<std::pair<double /* time*/, double /* distance*/>>;
@@ -388,6 +391,20 @@ class IntersectionModule : public SceneModuleInterface
   intersection::ObjectInfoManager object_info_manager_;
   /** @} */
 
+private:
+  /**
+   ***********************************************************
+   ***********************************************************
+   ***********************************************************
+   * @defgroup collision-variables [var] collision detection
+   * @{
+   */
+  //! save the last UNKNOWN traffic light information of this intersection(keep even if the info got
+  //! unavailable)
+  std::optional<std::pair<lanelet::Id, lanelet::ConstPoint3d>> tl_id_and_point_;
+  std::optional<TrafficSignalStamped> last_tl_valid_observation_{std::nullopt};
+  /** @} */
+
 private:
   /**
    ***********************************************************
@@ -556,6 +573,13 @@ class IntersectionModule : public SceneModuleInterface
    * @brief find TrafficPrioritizedLevel
    */
   TrafficPrioritizedLevel getTrafficPrioritizedLevel() const;
+
+  /**
+   * @brief update the valid traffic signal information if still available, otherwise keep last
+   * observation
+   */
+  void updateTrafficSignalObservation();
+
   /** @} */
 
 private:

planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp

@@ -258,6 +258,12 @@ IntersectionModule::prepareIntersectionData(const bool is_prioritized, PathWithL
       planner_data_->occupancy_grid->info.resolution);
   }
 
+  // ==========================================================================================
+  // update traffic light information
+  // updateTrafficSignalObservation() must be called at first to because other traffic signal
+  // fuctions use last_valid_observation_
+  // ==========================================================================================
+  updateTrafficSignalObservation();
   const bool is_green_solid_on = isGreenSolidOn();
   if (is_green_solid_on && !initial_green_light_observed_time_) {
     const auto assigned_lane_begin_point = assigned_lanelet.centerline().front();