feat(intersection_occlusion): quickly delete occlusion wall on change from green to yellow/red (#6608)

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

Author: soblin

planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

@@ -148,17 +148,18 @@ static std::string formatOcclusionType(const IntersectionModule::OcclusionType &
 intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail(
   PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason)
 {
-  const auto traffic_prioritized_level = getTrafficPrioritizedLevel();
-  const bool is_prioritized =
-    traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED;
-
-  const auto prepare_data = prepareIntersectionData(is_prioritized, path);
+  const auto prepare_data = prepareIntersectionData(path);
   if (!prepare_data) {
     return prepare_data.err();
   }
   const auto [interpolated_path_info, intersection_stoplines, path_lanelets] = prepare_data.ok();
   const auto & intersection_lanelets = intersection_lanelets_.value();
 
+  // NOTE: this level is based on the updateTrafficSignalObservation() which is latest
+  const auto traffic_prioritized_level = getTrafficPrioritizedLevel();
+  const bool is_prioritized =
+    traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED;
+
   // ==========================================================================================
   // stuck detection
   //

planning/behavior_velocity_intersection_module/src/scene_intersection.hpp

@@ -442,6 +442,9 @@ class IntersectionModule : public SceneModuleInterface
   //! unavailable)
   std::optional<std::pair<lanelet::Id, lanelet::ConstPoint3d>> tl_id_and_point_;
   std::optional<TrafficSignalStamped> last_tl_valid_observation_{std::nullopt};
+
+  //! save previous priority level to detect change from NotPrioritized to Prioritized
+  TrafficPrioritizedLevel previous_prioritized_level_{TrafficPrioritizedLevel::NOT_PRIORITIZED};
   /** @} */
 
 private:
@@ -548,7 +551,7 @@ class IntersectionModule : public SceneModuleInterface
    * To simplify modifyPathVelocityDetail(), this function is used at first
    */
   intersection::Result<BasicData, intersection::InternalError> prepareIntersectionData(
-    const bool is_prioritized, PathWithLaneId * path);
+    PathWithLaneId * path);
 
   /**
    * @brief find the associated stopline road marking of assigned lanelet

planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp

@@ -63,10 +63,23 @@ IntersectionModule::getOcclusionStatus(
      !is_amber_or_red_or_no_tl_info_ever)
       ? detectOcclusion(interpolated_path_info)
       : NotOccluded{};
-  occlusion_stop_state_machine_.setStateWithMarginTime(
-    std::holds_alternative<NotOccluded>(occlusion_status) ? StateMachine::State::GO
-                                                          : StateMachine::STOP,
-    logger_.get_child("occlusion_stop"), *clock_);
+
+  // ==========================================================================================
+  // if the traffic light changed from green to yellow/red, hysteresis time for occlusion is
+  // unnecessary
+  // ==========================================================================================
+  const auto transition_to_prioritized =
+    (previous_prioritized_level_ == TrafficPrioritizedLevel::NOT_PRIORITIZED &&
+     traffic_prioritized_level != TrafficPrioritizedLevel::NOT_PRIORITIZED);
+  if (transition_to_prioritized) {
+    occlusion_stop_state_machine_.setState(StateMachine::State::GO);
+  } else {
+    occlusion_stop_state_machine_.setStateWithMarginTime(
+      std::holds_alternative<NotOccluded>(occlusion_status) ? StateMachine::State::GO
+                                                            : StateMachine::STOP,
+      logger_.get_child("occlusion_stop"), *clock_);
+  }
+
   const bool is_occlusion_cleared_with_margin =
     (occlusion_stop_state_machine_.getState() == StateMachine::State::GO);  // module's detection
   // distinguish if ego detected occlusion or RTC detects occlusion

planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp

@@ -166,7 +166,7 @@ using intersection::make_ok;
 using intersection::Result;
 
 Result<IntersectionModule::BasicData, intersection::InternalError>
-IntersectionModule::prepareIntersectionData(const bool is_prioritized, PathWithLaneId * path)
+IntersectionModule::prepareIntersectionData(PathWithLaneId * path)
 {
   const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr();
   const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr();
@@ -175,6 +175,18 @@ IntersectionModule::prepareIntersectionData(const bool is_prioritized, PathWithL
   const auto footprint = planner_data_->vehicle_info_.createFootprint(0.0, 0.0);
   const auto & current_pose = planner_data_->current_odometry->pose;
 
+  // ==========================================================================================
+  // update traffic light information
+  // updateTrafficSignalObservation() must be called at first because other traffic signal
+  // fuctions use last_valid_observation_
+  // ==========================================================================================
+  // save previous information before calling updateTrafficSignalObservation()
+  previous_prioritized_level_ = getTrafficPrioritizedLevel();
+  updateTrafficSignalObservation();
+  const auto traffic_prioritized_level = getTrafficPrioritizedLevel();
+  const bool is_prioritized =
+    traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED;
+
   // spline interpolation
   const auto interpolated_path_info_opt = util::generateInterpolatedPath(
     lane_id_, associative_ids_, *path, planner_param_.common.path_interpolation_ds, logger_);
@@ -264,13 +276,7 @@ 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_
-  // ==========================================================================================
   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();