Refactor and add param to NOT ignore behind pedestrians

Signed-off-by: Maxime CLEMENT <[email protected]>

planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml

@@ -81,5 +81,6 @@
         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
+        do_not_ignore_behind_pedestrians: true  # [-] if true, occlusions behind pedestrians are not ignored
         min_objects_velocity: 0.5  # [m/s] minimum velocity for a predicted object to be ignored
         extra_objects_size: 0.5  # [m] extra size added to the objects when masking the occlusions

planning/behavior_velocity_crosswalk_module/src/manager.cpp

@@ -140,10 +140,12 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node)
     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");
+  cp.occlusion_do_not_ignore_behind_pedestrians =
+    getOrDeclareParameter<bool>(node, ns + ".occlusion.do_not_ignore_behind_pedestrians");
+  cp.occlusion_ignore_velocity_threshold =
+    getOrDeclareParameter<double>(node, ns + ".occlusion.ignore_velocity_threshold");
   cp.occlusion_extra_objects_size =
-    getOrDeclareParameter<double>(node, ns + ".occlusion.extra_objects_size");
+    getOrDeclareParameter<double>(node, ns + ".occlusion.extra_predicted_objects_size");
 }
 
 void CrosswalkModuleManager::launchNewModules(const PathWithLaneId & path)

planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp

@@ -94,10 +94,30 @@ std::vector<lanelet::BasicPolygon2d> calculate_detection_areas(
   return detection_areas;
 }
 
-void clear_behind_objects(
-  grid_map::GridMap & grid_map,
+std::vector<autoware_auto_perception_msgs::msg::PredictedObject> select_and_inflate_objects(
   const std::vector<autoware_auto_perception_msgs::msg::PredictedObject> & objects,
-  const double min_object_velocity, const double extra_object_size)
+  const double velocity_threshold, const bool skip_pedestrians, const double inflate_size)
+{
+  std::vector<autoware_auto_perception_msgs::msg::PredictedObject> selected_objects;
+  for (const auto & o : objects) {
+    const auto skip =
+      (skip_pedestrians &&
+       o.classification.front().label ==
+         autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN) ||
+      o.kinematics.initial_twist_with_covariance.twist.linear.x >= velocity_threshold;
+    if (!skip) {
+      auto selected_object = o;
+      selected_object.shape.dimensions.x += inflate_size;
+      selected_object.shape.dimensions.y += inflate_size;
+      selected_objects.push_back(selected_object);
+    }
+  }
+  return selected_objects;
+}
+
+void clear_occlusions_behind_objects(
+  grid_map::GridMap & grid_map,
+  const std::vector<autoware_auto_perception_msgs::msg::PredictedObject> & objects)
 {
   const auto angle_cmp = [&](const auto & p1, const auto & p2) {
     const auto d1 = p1 - grid_map.getPosition();
@@ -107,14 +127,10 @@ void clear_behind_objects(
   const lanelet::BasicPoint2d grid_map_position = grid_map.getPosition();
   const auto range = grid_map.getLength().maxCoeff() / 2.0;
   for (auto object : objects) {
-    object.shape.dimensions.x += extra_object_size;
-    object.shape.dimensions.y += extra_object_size;
     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) {
+    if (lanelet::geometry::distance2d(grid_map_position, object_position) < range) {
       lanelet::BasicPoints2d edge_points;
       const auto object_polygon = tier4_autoware_utils::toPolygon2d(object);
       for (const auto & edge_point : object_polygon.outer()) edge_points.push_back(edge_point);
@@ -143,10 +159,12 @@ bool is_crosswalk_occluded(
   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,
-      params.occlusion_extra_objects_size);
+  if (params.occlusion_ignore_behind_predicted_objects) {
+    const auto objects = select_and_inflate_objects(
+      dynamic_objects, params.occlusion_ignore_velocity_threshold,
+      params.occlusion_do_not_ignore_behind_pedestrians, params.occlusion_extra_objects_size);
+    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)) {

planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp

@@ -79,6 +79,24 @@ double calculate_detection_range(
   const double occluded_object_velocity, const double dist_ego_to_crosswalk,
   const double ego_velocity);
 
+/// @brief select a subset of objects meeting the velocity threshold and inflate their shape
+/// @param objects input objects
+/// @param velocity_threshold minimum velocity for an object to be selected
+/// @param skip_pedestrians if true, pedestrians are not selected regardless of their velocities
+/// @param inflate_size [m] size by which the shape of the selected objects are inflated
+/// @return selected and inflated objects
+std::vector<autoware_auto_perception_msgs::msg::PredictedObject> select_and_inflate_objects(
+  const std::vector<autoware_auto_perception_msgs::msg::PredictedObject> & objects,
+  const double velocity_threshold, const bool skip_pedestrians, const double inflate_size);
+
+/// @brief clear occlusions behind the given objects
+/// @details masks behind the object assuming rays from the center of the grid map
+/// @param grid_map grid map
+/// @param objects objects
+void clear_occlusions_behind_objects(
+  grid_map::GridMap & grid_map,
+  const std::vector<autoware_auto_perception_msgs::msg::PredictedObject> & objects);
+
 /// @brief update timers so that the slowdown activates if the initial time is older than the buffer
 /// @param initial_time initial time
 /// @param most_recent_slowdown_time time to set only if initial_time is older than the buffer

planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp

@@ -165,7 +165,8 @@ class CrosswalkModule : public SceneModuleInterface
     int occlusion_occupied_min;
     bool occlusion_ignore_with_traffic_light;
     bool occlusion_ignore_behind_predicted_objects;
-    double occlusion_min_objects_velocity;
+    bool occlusion_do_not_ignore_behind_pedestrians;
+    double occlusion_ignore_velocity_threshold;
     double occlusion_extra_objects_size;
   };