fix(crosswalk): fix occlusion detection range calculation and add deb…

…ug markers (#9121)

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/include/autoware/behavior_velocity_crosswalk_module/util.hpp

@@ -21,7 +21,6 @@
 #include <boost/geometry/geometries/linestring.hpp>
 #include <boost/geometry/geometries/point_xy.hpp>
 
-#include <limits>
 #include <memory>
 #include <optional>
 #include <set>
@@ -83,6 +82,10 @@ struct DebugData
   std::vector<geometry_msgs::msg::Point> crosswalk_polygon;
   std::vector<std::vector<geometry_msgs::msg::Point>> ego_polygons;
   std::vector<std::vector<geometry_msgs::msg::Point>> obj_polygons;
+
+  // occlusion data
+  std::vector<lanelet::BasicPolygon2d> occlusion_detection_areas;
+  geometry_msgs::msg::Point crosswalk_origin;
 };
 
 std::vector<std::pair<int64_t, lanelet::ConstLanelet>> getCrosswalksOnPath(

planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/debug.cpp

@@ -24,8 +24,6 @@
 namespace autoware::behavior_velocity_planner
 {
 
-using autoware::motion_utils::createSlowDownVirtualWallMarker;
-using autoware::motion_utils::createStopVirtualWallMarker;
 using autoware::universe_utils::appendMarkerArray;
 using autoware::universe_utils::calcOffsetPose;
 using autoware::universe_utils::createDefaultMarker;
@@ -173,6 +171,26 @@ visualization_msgs::msg::MarkerArray createCrosswalkMarkers(
     msg.markers.push_back(marker);
   }
 
+  if (!debug_data.occlusion_detection_areas.empty()) {
+    auto marker = createDefaultMarker(
+      "map", now, "occlusion_detection_areas", uid, Marker::LINE_LIST,
+      createMarkerScale(0.25, 0.25, 0.0), createMarkerColor(1.0, 1.0, 1.0, 0.5));
+    for (const auto & area : debug_data.occlusion_detection_areas) {
+      for (auto i = 0UL; i + 1 < area.size(); ++i) {
+        const auto & p1 = area[i];
+        const auto & p2 = area[i + 1];
+        marker.points.push_back(createPoint(p1.x(), p1.y(), 0.0));
+        marker.points.push_back(createPoint(p2.x(), p2.y(), 0.0));
+      }
+    }
+    msg.markers.push_back(marker);
+    marker = createDefaultMarker(
+      "map", now, "crosswalk_origin", uid, Marker::SPHERE, createMarkerScale(0.25, 0.25, 0.25),
+      createMarkerColor(1.0, 1.0, 1.0, 0.5));
+    marker.pose.position = debug_data.crosswalk_origin;
+    msg.markers.push_back(marker);
+  }
+
   return msg;
 }
 }  // namespace

planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp

@@ -14,18 +14,18 @@
 
 #include "occluded_crosswalk.hpp"
 
-#include "autoware/behavior_velocity_crosswalk_module/util.hpp"
-
 #include <autoware_grid_map_utils/polygon_iterator.hpp>
 #include <grid_map_ros/GridMapRosConverter.hpp>
 
+#include <lanelet2_core/primitives/Polygon.h>
+
 #include <algorithm>
 #include <vector>
 
 namespace autoware::behavior_velocity_planner
 {
 bool is_occluded(
-  const grid_map::GridMap & grid_map, const int min_nb_of_cells, const grid_map::Index idx,
+  const grid_map::GridMap & grid_map, const int min_nb_of_cells, const grid_map::Index & idx,
   const autoware::behavior_velocity_planner::CrosswalkModule::PlannerParam & params)
 {
   grid_map::Index idx_offset;
@@ -112,7 +112,7 @@ void clear_occlusions_behind_objects(
   };
   const lanelet::BasicPoint2d grid_map_position = grid_map.getPosition();
   const auto range = grid_map.getLength().maxCoeff() / 2.0;
-  for (auto object : objects) {
+  for (auto & object : objects) {
     const lanelet::BasicPoint2d object_position = {
       object.kinematics.initial_pose_with_covariance.pose.position.x,
       object.kinematics.initial_pose_with_covariance.pose.position.y};
@@ -137,9 +137,8 @@ void clear_occlusions_behind_objects(
 }
 
 bool is_crosswalk_occluded(
-  const lanelet::ConstLanelet & crosswalk_lanelet,
   const nav_msgs::msg::OccupancyGrid & occupancy_grid,
-  const geometry_msgs::msg::Point & path_intersection, const double detection_range,
+  const std::vector<lanelet::BasicPolygon2d> & detection_areas,
   const std::vector<autoware_perception_msgs::msg::PredictedObject> & dynamic_objects,
   const autoware::behavior_velocity_planner::CrosswalkModule::PlannerParam & params)
 {
@@ -153,8 +152,7 @@ bool is_crosswalk_occluded(
     clear_occlusions_behind_objects(grid_map, objects);
   }
   const auto min_nb_of_cells = std::ceil(params.occlusion_min_size / grid_map.getResolution());
-  for (const auto & detection_area : calculate_detection_areas(
-         crosswalk_lanelet, {path_intersection.x, path_intersection.y}, detection_range)) {
+  for (const auto & detection_area : detection_areas) {
     grid_map::Polygon poly;
     for (const auto & p : detection_area) poly.addVertex(grid_map::Position(p.x(), p.y()));
     for (autoware::grid_map_utils::PolygonIterator iter(grid_map, poly); !iter.isPastEnd(); ++iter)
@@ -169,6 +167,6 @@ double calculate_detection_range(
 {
   constexpr double min_ego_velocity = 1.0;
   const auto time_to_crosswalk = dist_ego_to_crosswalk / std::max(min_ego_velocity, ego_velocity);
-  return time_to_crosswalk > 0.0 ? time_to_crosswalk / occluded_object_velocity : 20.0;
+  return time_to_crosswalk > 0.0 ? time_to_crosswalk * occluded_object_velocity : 20.0;
 }
 }  // namespace autoware::behavior_velocity_planner

planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp

@@ -26,7 +26,6 @@
 #include <lanelet2_core/primitives/Lanelet.h>
 #include <lanelet2_core/primitives/Point.h>
 
-#include <optional>
 #include <vector>
 
 namespace autoware::behavior_velocity_planner
@@ -39,7 +38,7 @@ namespace autoware::behavior_velocity_planner
 /// @param [in] params parameters
 /// @return true if the index is occluded
 bool is_occluded(
-  const grid_map::GridMap & grid_map, const int min_nb_of_cells, const grid_map::Index idx,
+  const grid_map::GridMap & grid_map, const int min_nb_of_cells, const grid_map::Index & idx,
   const autoware::behavior_velocity_planner::CrosswalkModule::PlannerParam & params);
 
 /// @brief interpolate a point beyond the end of the given segment
@@ -50,17 +49,14 @@ lanelet::BasicPoint2d interpolate_point(
   const lanelet::BasicSegment2d & segment, const double extra_distance);
 
 /// @brief check if the crosswalk is occluded
-/// @param crosswalk_lanelet lanelet of the crosswalk
 /// @param occupancy_grid occupancy grid with the occlusion information
-/// @param path_intersection intersection between the crosswalk and the ego path
-/// @param detection_range range away from the crosswalk until occlusions are considered
+/// @param detection_areas areas to check for occlusions
 /// @param dynamic_objects dynamic objects
 /// @param params parameters
 /// @return true if the crosswalk is occluded
 bool is_crosswalk_occluded(
-  const lanelet::ConstLanelet & crosswalk_lanelet,
   const nav_msgs::msg::OccupancyGrid & occupancy_grid,
-  const geometry_msgs::msg::Point & path_intersection, const double detection_range,
+  const std::vector<lanelet::BasicPolygon2d> & detection_areas,
   const std::vector<autoware_perception_msgs::msg::PredictedObject> & dynamic_objects,
   const autoware::behavior_velocity_planner::CrosswalkModule::PlannerParam & params);
 
@@ -89,6 +85,15 @@ std::vector<autoware_perception_msgs::msg::PredictedObject> select_and_inflate_o
 void clear_occlusions_behind_objects(
   grid_map::GridMap & grid_map,
   const std::vector<autoware_perception_msgs::msg::PredictedObject> & objects);
+
+/// @brief calculate areas to check for occlusions around the given crosswalk
+/// @param crosswalk_lanelet crosswalk lanelet
+/// @param crosswalk_origin crosswalk point from which to calculate the distances
+/// @param detection_range [m] desired distance from the crosswalk origin
+/// @return detection areas within the detection range of the crosswalk
+std::vector<lanelet::BasicPolygon2d> calculate_detection_areas(
+  const lanelet::ConstLanelet & crosswalk_lanelet, const lanelet::BasicPoint2d & crosswalk_origin,
+  const double detection_range);
 }  // namespace autoware::behavior_velocity_planner
 
 #endif  // OCCLUDED_CROSSWALK_HPP_

planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp

@@ -49,13 +49,11 @@ using autoware::motion_utils::calcLongitudinalOffsetPose;
 using autoware::motion_utils::calcSignedArcLength;
 using autoware::motion_utils::calcSignedArcLengthPartialSum;
 using autoware::motion_utils::findNearestSegmentIndex;
-using autoware::motion_utils::insertTargetPoint;
 using autoware::motion_utils::resamplePath;
 using autoware::universe_utils::createPoint;
 using autoware::universe_utils::getPose;
 using autoware::universe_utils::Point2d;
 using autoware::universe_utils::Polygon2d;
-using autoware::universe_utils::pose2transform;
 using autoware::universe_utils::toHexString;
 
 namespace
@@ -936,37 +934,46 @@ void CrosswalkModule::applySlowDownByOcclusion(
   const auto is_crosswalk_ignored =
     (planner_param_.occlusion_ignore_with_traffic_light && crosswalk_has_traffic_light) ||
     crosswalk_.hasAttribute("skip_occluded_slowdown");
-  if (planner_param_.occlusion_enable && !is_crosswalk_ignored) {
-    const auto dist_ego_to_crosswalk =
-      calcSignedArcLength(output.points, ego_pos, first_path_point_on_crosswalk);
-    const auto detection_range =
-      planner_data_->vehicle_info_.max_lateral_offset_m +
-      calculate_detection_range(
-        planner_param_.occlusion_occluded_object_velocity, dist_ego_to_crosswalk,
-        planner_data_->current_velocity->twist.linear.x);
-    const auto is_ego_on_the_crosswalk =
-      dist_ego_to_crosswalk <= planner_data_->vehicle_info_.max_longitudinal_offset_m;
-    if (!is_ego_on_the_crosswalk) {
-      if (is_crosswalk_occluded(
-            crosswalk_, *planner_data_->occupancy_grid, first_path_point_on_crosswalk,
-            detection_range, objects_ptr->objects, planner_param_)) {
-        if (!current_initial_occlusion_time_) current_initial_occlusion_time_ = now;
-        if (cmp_with_time_buffer(current_initial_occlusion_time_, std::greater_equal<double>{}))
-          most_recent_occlusion_time_ = now;
-      } else if (!cmp_with_time_buffer(most_recent_occlusion_time_, std::greater<double>{})) {
-        current_initial_occlusion_time_.reset();
-      }
-
-      if (cmp_with_time_buffer(most_recent_occlusion_time_, std::less_equal<double>{})) {
-        const auto target_velocity = calcTargetVelocity(first_path_point_on_crosswalk, output);
-        applySlowDown(
-          output, first_path_point_on_crosswalk, last_path_point_on_crosswalk,
-          std::max(target_velocity, planner_param_.occlusion_slow_down_velocity));
-        debug_data_.virtual_wall_suffix = " (occluded)";
-      } else {
-        most_recent_occlusion_time_.reset();
-      }
+  if (!planner_param_.occlusion_enable || is_crosswalk_ignored) {
+    return;
+  }
+  const auto dist_ego_to_crosswalk =
+    calcSignedArcLength(output.points, ego_pos, first_path_point_on_crosswalk);
+  const auto is_ego_on_the_crosswalk =
+    dist_ego_to_crosswalk <= planner_data_->vehicle_info_.max_longitudinal_offset_m;
+  if (is_ego_on_the_crosswalk) {
+    return;
+  }
+  const auto detection_range =
+    planner_data_->vehicle_info_.max_lateral_offset_m +
+    calculate_detection_range(
+      planner_param_.occlusion_occluded_object_velocity, dist_ego_to_crosswalk,
+      planner_data_->current_velocity->twist.linear.x);
+  const auto detection_areas = calculate_detection_areas(
+    crosswalk_, {first_path_point_on_crosswalk.x, first_path_point_on_crosswalk.y},
+    detection_range);
+  debug_data_.occlusion_detection_areas = detection_areas;
+  debug_data_.crosswalk_origin = first_path_point_on_crosswalk;
+  if (is_crosswalk_occluded(
+        *planner_data_->occupancy_grid, detection_areas, objects_ptr->objects, planner_param_)) {
+    if (!current_initial_occlusion_time_) {
+      current_initial_occlusion_time_ = now;
+    }
+    if (cmp_with_time_buffer(current_initial_occlusion_time_, std::greater_equal<double>{})) {
+      most_recent_occlusion_time_ = now;
     }
+  } else if (!cmp_with_time_buffer(most_recent_occlusion_time_, std::greater<double>{})) {
+    current_initial_occlusion_time_.reset();
+  }
+
+  if (cmp_with_time_buffer(most_recent_occlusion_time_, std::less_equal<double>{})) {
+    const auto target_velocity = calcTargetVelocity(first_path_point_on_crosswalk, output);
+    applySlowDown(
+      output, first_path_point_on_crosswalk, last_path_point_on_crosswalk,
+      std::max(target_velocity, planner_param_.occlusion_slow_down_velocity));
+    debug_data_.virtual_wall_suffix = " (occluded)";
+  } else {
+    most_recent_occlusion_time_.reset();
   }
 }