fix(intersection_occlusion): estimate dynamic occlusion source with triangle (autowarefoundation#6585)

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

Author: soblin

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.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>>

planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp

@@ -331,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) {
@@ -367,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 =
@@ -383,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);
     }
   }
 
@@ -397,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