[WIP] option to ignore occlusions behind predicted objects

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

Author: maxime-clem

planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml

@@ -80,3 +80,5 @@
         free_space_max: 43  # [-] maximum value of a free space cell in the occupancy grid
         occupied_min: 58    # [-] minimum value of an occupied cell in the occupancy grid
         ignore_with_traffic_light: true  # [-] if true, occlusions at crosswalks with traffic lights are ignored
+        ignore_behind_predicted_objects: true  # [-] if true, occlusions behind predicted objects are ignored
+        min_objects_velocity: 0.5  # [m/s] minimum velocity for a predicted object to be ignored

planning/behavior_velocity_crosswalk_module/src/manager.cpp

@@ -138,6 +138,10 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node)
   cp.occlusion_occupied_min = getOrDeclareParameter<int>(node, ns + ".occlusion.occupied_min");
   cp.occlusion_ignore_with_traffic_light =
     getOrDeclareParameter<bool>(node, ns + ".occlusion.ignore_with_traffic_light");
+  cp.occlusion_ignore_behind_predicted_objects =
+    getOrDeclareParameter<bool>(node, ns + ".occlusion.ignore_behind_predicted_objects");
+  cp.occlusion_min_objects_velocity =
+    getOrDeclareParameter<double>(node, ns + ".occlusion.min_objects_velocity");
 }
 
 void CrosswalkModuleManager::launchNewModules(const PathWithLaneId & path)

planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp

@@ -94,15 +94,47 @@ std::vector<lanelet::BasicPolygon2d> calculate_detection_areas(
   return detection_areas;
 }
 
+void clear_behind_objects(
+  grid_map::GridMap & grid_map,
+  const std::vector<autoware_auto_perception_msgs::msg::PredictedObject> & objects,
+  const double min_object_velocity)
+{
+  const lanelet::BasicPoint2d grid_map_position = grid_map.getPosition();
+  const auto range = grid_map.getLength().maxCoeff() / 2.0;
+  for (const 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};
+    if (
+      object.kinematics.initial_twist_with_covariance.twist.linear.x >= min_object_velocity &&
+      lanelet::geometry::distance2d(grid_map_position, object_position) < range) {
+      lanelet::BasicPoints2d points;
+      for (const auto & edge_point : tier4_autoware_utils::toPolygon2d(object).outer())
+        points.push_back(edge_point);
+      for (const auto & edge_point : tier4_autoware_utils::toPolygon2d(object).outer())
+        points.push_back(interpolate_point({object_position, edge_point}, range));
+      lanelet::BasicPolygon2d polygon_behind_object;
+      boost::geometry::convex_hull(points, polygon_behind_object);
+      grid_map::Polygon polygon_to_clear;
+      for (const auto & p : polygon_behind_object) polygon_to_clear.addVertex(p);
+      for (grid_map_utils::PolygonIterator it(grid_map, polygon_to_clear); !it.isPastEnd(); ++it)
+        grid_map.at("layer", *it) = 0;
+    }
+  }
+}
+
 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<autoware_auto_perception_msgs::msg::PredictedObject> & dynamic_objects,
   const behavior_velocity_planner::CrosswalkModule::PlannerParam & params)
 {
   grid_map::GridMap grid_map;
   grid_map::GridMapRosConverter::fromOccupancyGrid(occupancy_grid, "layer", grid_map);
 
+  if (params.occlusion_ignore_behind_predicted_objects)
+    clear_behind_objects(grid_map, dynamic_objects, params.occlusion_min_objects_velocity);
   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)) {
@@ -115,11 +147,12 @@ bool is_crosswalk_occluded(
 }
 
 double calculate_detection_range(
-  const double object_velocity, const double dist_ego_to_crosswalk, const double ego_velocity)
+  const double occluded_object_velocity, const double dist_ego_to_crosswalk,
+  const double ego_velocity)
 {
   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 / object_velocity : 20.0;
+  return time_to_crosswalk > 0.0 ? time_to_crosswalk / occluded_object_velocity : 20.0;
 }
 
 void update_occlusion_timers(

planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp

@@ -27,6 +27,7 @@
 #include <lanelet2_core/primitives/Point.h>
 
 #include <optional>
+#include <vector>
 
 namespace behavior_velocity_planner
 {
@@ -60,16 +61,23 @@ bool is_crosswalk_ignored(
 /// @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 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<autoware_auto_perception_msgs::msg::PredictedObject> & dynamic_objects,
   const behavior_velocity_planner::CrosswalkModule::PlannerParam & params);
 
+/// @brief calculate the distance away from the crosswalk that should be checked for occlusions
+/// @param occluded_objects_velocity assumed velocity of the objects coming out of occlusions
+/// @param dist_ego_to_crosswalk distance between ego and the crosswalk
+/// @param ego_velocity current velocity of ego
 double calculate_detection_range(
-  const double object_velocity, const double dist_ego_to_crosswalk, const double ego_velocity);
+  const double occluded_object_velocity, const double dist_ego_to_crosswalk,
+  const double ego_velocity);
 
 /// @brief update timers so that the slowdown activates if the initial time is older than the buffer
 /// @param initial_time initial time

planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp

@@ -241,14 +241,14 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto
     const auto dist_ego_to_crosswalk =
       calcSignedArcLength(path->points, ego_pos, path_intersects.front());
     const auto detection_range = calculate_detection_range(
-      planner_param_.min_object_velocity, dist_ego_to_crosswalk,
+      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, path_intersects.front(), detection_range,
-            planner_param_))
+            objects_ptr->objects, planner_param_))
         update_occlusion_timers(
           current_initial_occlusion_time_, most_recent_occlusion_time_, clock_->now(),
           planner_param_.occlusion_time_buffer);

planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp

@@ -164,6 +164,8 @@ class CrosswalkModule : public SceneModuleInterface
     int occlusion_free_space_max;
     int occlusion_occupied_min;
     bool occlusion_ignore_with_traffic_light;
+    bool occlusion_ignore_behind_predicted_objects;
+    double occlusion_min_objects_velocity;
   };
 
   struct ObjectInfo