Merge pull request autowarefoundation#1198 from tier4/cherry-pick/pr6…

…608+6585

Cherry pick/pr6608+6585

planning/behavior_velocity_intersection_module/src/debug.cpp

@@ -389,6 +389,22 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray(
       &debug_marker_array, now);
   }
 
+  if (debug_data_.nearest_occlusion_triangle) {
+    const auto [p1, p2, p3] = debug_data_.nearest_occlusion_triangle.value();
+    const auto color = debug_data_.static_occlusion ? green : red;
+    geometry_msgs::msg::Polygon poly;
+    poly.points.push_back(
+      geometry_msgs::build<geometry_msgs::msg::Point32>().x(p1.x).y(p1.y).z(p1.z));
+    poly.points.push_back(
+      geometry_msgs::build<geometry_msgs::msg::Point32>().x(p2.x).y(p2.y).z(p2.z));
+    poly.points.push_back(
+      geometry_msgs::build<geometry_msgs::msg::Point32>().x(p3.x).y(p3.y).z(p3.z));
+    appendMarkerArray(
+      debug::createPolygonMarkerArray(
+        poly, "nearest_occlusion_triangle", lane_id_, now, 0.3, 0.0, 0.0, std::get<0>(color),
+        std::get<1>(color), std::get<2>(color)),
+      &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;

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

@@ -235,6 +235,10 @@ class IntersectionModule : public SceneModuleInterface
     std::vector<geometry_msgs::msg::Polygon> occlusion_polygons;
     std::optional<std::pair<geometry_msgs::msg::Point, geometry_msgs::msg::Point>>
       nearest_occlusion_projection{std::nullopt};
+    std::optional<
+      std::tuple<geometry_msgs::msg::Point, geometry_msgs::msg::Point, geometry_msgs::msg::Point>>
+      nearest_occlusion_triangle{std::nullopt};
+    bool static_occlusion{false};
     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>>
@@ -438,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:
@@ -544,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

@@ -47,10 +47,23 @@ IntersectionModule::getOcclusionStatus(
     (planner_param_.occlusion.enable && !occlusion_attention_lanelets.empty() && !is_amber_or_red)
       ? 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
@@ -318,13 +331,14 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion(
     }
     return nearest;
   };
-  struct NearestOcclusionPoint
+  struct NearestOcclusionInterval
   {
     int64 division_index{0};
     int64 point_index{0};
     double dist{0.0};
     geometry_msgs::msg::Point point;
     geometry_msgs::msg::Point projection;
+    geometry_msgs::msg::Point visible_end;
   } nearest_occlusion_point;
   double min_dist = std::numeric_limits<double>::infinity();
   for (unsigned division_index = 0; division_index < lane_divisions.size(); ++division_index) {
@@ -354,6 +368,8 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion(
       continue;
     }
     double acc_dist = 0.0;
+    bool found_min_dist_for_this_division = false;
+    bool is_prev_occluded = false;
     auto acc_dist_it = projection_it;
     for (auto point_it = projection_it; point_it != division.end(); point_it++) {
       const double dist =
@@ -370,11 +386,24 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion(
         if (acc_dist < min_dist) {
           min_dist = acc_dist;
           nearest_occlusion_point = {
-            division_index, std::distance(division.begin(), point_it), acc_dist,
+            division_index,
+            std::distance(division.begin(), point_it),
+            acc_dist,
             tier4_autoware_utils::createPoint(point_it->x(), point_it->y(), origin.z),
-            tier4_autoware_utils::createPoint(projection_it->x(), projection_it->y(), origin.z)};
+            tier4_autoware_utils::createPoint(projection_it->x(), projection_it->y(), origin.z),
+            tier4_autoware_utils::createPoint(
+              projection_it->x(), projection_it->y(),
+              origin.z) /* initialize with projection point at first*/};
+          found_min_dist_for_this_division = true;
+        } else if (found_min_dist_for_this_division && is_prev_occluded) {
+          // although this cell is not "nearest" cell, we have found the "nearest" cell on this
+          // division previously in this iteration, and the iterated cells are still OCCLUDED since
+          // then
+          nearest_occlusion_point.visible_end =
+            tier4_autoware_utils::createPoint(point_it->x(), point_it->y(), origin.z);
         }
       }
+      is_prev_occluded = (pixel == OCCLUDED);
     }
   }
 
@@ -384,16 +413,24 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion(
 
   debug_data_.nearest_occlusion_projection =
     std::make_pair(nearest_occlusion_point.point, nearest_occlusion_point.projection);
-  LineString2d ego_occlusion_line;
-  ego_occlusion_line.emplace_back(current_pose.position.x, current_pose.position.y);
-  ego_occlusion_line.emplace_back(nearest_occlusion_point.point.x, nearest_occlusion_point.point.y);
+  debug_data_.nearest_occlusion_triangle = std::make_tuple(
+    current_pose.position, nearest_occlusion_point.point, nearest_occlusion_point.visible_end);
+  Polygon2d ego_occlusion_triangle;
+  ego_occlusion_triangle.outer().emplace_back(current_pose.position.x, current_pose.position.y);
+  ego_occlusion_triangle.outer().emplace_back(
+    nearest_occlusion_point.point.x, nearest_occlusion_point.point.y);
+  ego_occlusion_triangle.outer().emplace_back(
+    nearest_occlusion_point.visible_end.x, nearest_occlusion_point.visible_end.y);
+  bg::correct(ego_occlusion_triangle);
   for (const auto & attention_object_info : object_info_manager_.allObjects()) {
     const auto obj_poly =
       tier4_autoware_utils::toPolygon2d(attention_object_info->predicted_object());
-    if (bg::intersects(obj_poly, ego_occlusion_line)) {
+    if (bg::intersects(obj_poly, ego_occlusion_triangle)) {
+      debug_data_.static_occlusion = false;
       return DynamicallyOccluded{min_dist};
     }
   }
+  debug_data_.static_occlusion = true;
   return StaticallyOccluded{min_dist};
 }
 }  // namespace behavior_velocity_planner

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();