feat(intersection): memorize last observed valid traffic light inform…

…ation

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

planning/behavior_velocity_intersection_module/src/debug.cpp

@@ -111,7 +111,7 @@ visualization_msgs::msg::MarkerArray createPoseMarkerArray(
   return msg;
 }
 
-visualization_msgs::msg::MarkerArray createLineMarkerArray(
+visualization_msgs::msg::MarkerArray createArrowLineMarkerArray(
   const geometry_msgs::msg::Point & point_start, const geometry_msgs::msg::Point & point_end,
   const std::string & ns, const int64_t id, const double r, const double g, const double b)
 {
@@ -137,6 +137,28 @@ visualization_msgs::msg::MarkerArray createLineMarkerArray(
   return msg;
 }
 
+visualization_msgs::msg::MarkerArray createLineMarkerArray(
+  const geometry_msgs::msg::Point & point_start, const geometry_msgs::msg::Point & point_end,
+  const std::string & ns, const int64_t id, const double r, const double g, const double b)
+{
+  visualization_msgs::msg::MarkerArray msg;
+
+  visualization_msgs::msg::Marker marker;
+  marker.header.frame_id = "map";
+  marker.ns = ns + "_line";
+  marker.id = id;
+  marker.lifetime = rclcpp::Duration::from_seconds(0.3);
+  marker.type = visualization_msgs::msg::Marker::LINE_STRIP;
+  marker.action = visualization_msgs::msg::Marker::ADD;
+  marker.scale.x = 0.1;
+  marker.color = createMarkerColor(r, g, b, 0.999);
+  marker.points.push_back(point_start);
+  marker.points.push_back(point_end);
+
+  msg.markers.push_back(marker);
+  return msg;
+}
+
 [[maybe_unused]] visualization_msgs::msg::Marker createPointMarkerArray(
   const geometry_msgs::msg::Point & point, const std::string & ns, const int64_t id, const double r,
   const double g, const double b)
@@ -362,10 +384,27 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray(
   if (debug_data_.nearest_occlusion_projection) {
     const auto [point_start, point_end] = debug_data_.nearest_occlusion_projection.value();
     appendMarkerArray(
-      ::createLineMarkerArray(
+      ::createArrowLineMarkerArray(
         point_start, point_end, "nearest_occlusion_projection", lane_id_, 0.5, 0.5, 0.0),
       &debug_marker_array, now);
   }
+
+  if (debug_data_.traffic_light_observation) {
+    const auto GREEN = autoware_perception_msgs::msg::TrafficSignalElement::GREEN;
+    const auto YELLOW = autoware_perception_msgs::msg::TrafficSignalElement::AMBER;
+
+    const auto [ego, tl_point, id, color] = debug_data_.traffic_light_observation.value();
+    geometry_msgs::msg::Point tl_point_point;
+    tl_point_point.x = tl_point.x();
+    tl_point_point.y = tl_point.y();
+    tl_point_point.z = tl_point.z();
+    const auto tl_color = (color == GREEN) ? green : (color == YELLOW ? yellow : red);
+    const auto [r, g, b] = tl_color;
+    appendMarkerArray(
+      ::createLineMarkerArray(
+        ego.position, tl_point_point, "intersection_traffic_light", lane_id_, r, g, b),
+      &debug_marker_array, now);
+  }
   return debug_marker_array;
 }
 

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
-    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
+  if (!last_tl_valid_observation_) {
     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,88 @@ 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;
+  };
+
+  // ==========================================================================================
+  // if no traffic light information has been available, it is UNKNOWN state which is treated as
+  // NOT_PRIORITIZED
+  //
+  // if there has been any information available in the past more than once, the last valid
+  // information is kept and used for planning
+  // ==========================================================================================
+  auto level = TrafficPrioritizedLevel::NOT_PRIORITIZED;
+  if (last_tl_valid_observation_) {
+    auto color = TrafficSignalElement::GREEN;
+    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, tl_id, 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;
-  }
-  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;
+    // the info of this traffic light is not available
+    return;
   }
-  return TrafficPrioritizedLevel::NOT_PRIORITIZED;
+  last_tl_valid_observation_ = tl_info_opt.value();
 }
 
 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, lanelet::Id, 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,19 +258,27 @@ IntersectionModule::prepareIntersectionData(const bool is_prioritized, PathWithL
       planner_data_->occupancy_grid->info.resolution);
   }
 
-  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();
-    const bool approached_assigned_lane =
-      motion_utils::calcSignedArcLength(
-        path->points, closest_idx,
-        tier4_autoware_utils::createPoint(
-          assigned_lane_begin_point.x(), assigned_lane_begin_point.y(),
-          assigned_lane_begin_point.z())) <
-      planner_param_.collision_detection.yield_on_green_traffic_light
-        .distance_to_assigned_lanelet_start;
-    if (approached_assigned_lane) {
-      initial_green_light_observed_time_ = clock_->now();
+  // ==========================================================================================
+  // update traffic light information
+  // updateTrafficSignalObservation() must be called at first to because other traffic signal
+  // fuctions use last_valid_observation_
+  // ==========================================================================================
+  if (has_traffic_light_) {
+    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();
+      const bool approached_assigned_lane =
+        motion_utils::calcSignedArcLength(
+          path->points, closest_idx,
+          tier4_autoware_utils::createPoint(
+            assigned_lane_begin_point.x(), assigned_lane_begin_point.y(),
+            assigned_lane_begin_point.z())) <
+        planner_param_.collision_detection.yield_on_green_traffic_light
+          .distance_to_assigned_lanelet_start;
+      if (approached_assigned_lane) {
+        initial_green_light_observed_time_ = clock_->now();
+      }
     }
   }