feat(crosswalk): add option to slowdown at occluded crosswalks

planning/behavior_velocity_crosswalk_module/CMakeLists.txt

@@ -6,10 +6,8 @@ autoware_package()
 pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml)
 
 ament_auto_add_library(${PROJECT_NAME} SHARED
-  src/debug.cpp
-  src/manager.cpp
-  src/scene_crosswalk.cpp
-  src/util.cpp
+  DIRECTORY
+  src
 )
 
 ament_auto_package(INSTALL_TO_SHARE config)

planning/behavior_velocity_crosswalk_module/README.md

@@ -193,6 +193,50 @@ document.
 | `max_slow_down_accel` | [m/ss]  | double | minimum accel deceleration for safe brake                                                                             |
 | `no_relax_velocity`   | [m/s]   | double | if the current velocity is less than X m/s, ego always stops at the stop position(not relax deceleration constraints) |
 
+### Occlusion
+
+This feature makes ego slow down for a crosswalk that is occluded.
+
+Occlusion of the crosswalk is determined using the occupancy grid.
+An occlusion is a square of size `min_size` of occluded cells
+(i.e., their values are between `free_space_max` and `occupied_min`)
+of size `min_size`.
+If an occlusion is found within range of the crosswalk,
+then the velocity limit at the crosswalk is set to `slow_down_velocity` (or more to not break limits set by `max_slow_down_jerk` and `max_slow_down_accel`).
+The range is calculated from the intersection between the ego path and the crosswalk and is equal to the time taken by ego to reach the crosswalk times the `occluded_object_velocity`.
+This range is meant to be large when ego is far from the crosswalk and small when ego is close.
+
+In order to avoid flickering decisions, a time buffer can be used such that the decision to add (or remove) the slow down is only taken
+after an occlusion is detected (or not detected) for a consecutive time defined by the `time_buffer` parameter.
+
+To ignore occlusions when the pedestrian light is red, `ignore_with_red_traffic_light` should be set to true.
+
+To ignore temporary occlusions caused by moving objects,
+`ignore_behind_predicted_objects` should be set to true.
+By default, occlusions behind an object with velocity higher than `ignore_velocity_thresholds.default` are ignored.
+This velocity threshold can be specified depending on the object type by specifying the object class label and velocity threshold in the parameter lists `ignore_velocity_thresholds.custom_labels` and `ignore_velocity_thresholds.custom_thresholds`.
+To inflate the masking behind objects, their footprint can be made bigger using `extra_predicted_objects_size`.
+
+<figure markdown>
+  ![stuck_vehicle_attention_range](docs/with_occlusion.svg){width=600}
+</figure>
+
+| Parameter                                      | Unit  | Type        | Description                                                                                                                                     |
+| ---------------------------------------------- | ----- | ----------- | ----------------------------------------------------------------------------------------------------------------------------------------------- |
+| `enable`                                       | [-]   | bool        | if true, ego will slow down around crosswalks that are occluded                                                                                 |
+| `occluded_object_velocity`                     | [m/s] | double      | assumed velocity of objects that may come out of the occluded space                                                                             |
+| `slow_down_velocity`                           | [m/s] | double      | slow down velocity                                                                                                                              |
+| `time_buffer`                                  | [s]   | double      | consecutive time with/without an occlusion to add/remove the slowdown                                                                           |
+| `min_size`                                     | [m]   | double      | minimum size of an occlusion (square side size)                                                                                                 |
+| `free_space_max`                               | [-]   | double      | maximum value of a free space cell in the occupancy grid                                                                                        |
+| `occupied_min`                                 | [-]   | double      | minimum value of an occupied cell in the occupancy grid                                                                                         |
+| `ignore_with_red_traffic_light`                | [-]   | bool        | if true, occlusions at crosswalks with traffic lights are ignored                                                                               |
+| `ignore_behind_predicted_objects`              | [-]   | bool        | if true, occlusions behind predicted objects are ignored                                                                                        |
+| `ignore_velocity_thresholds.default`           | [m/s] | double      | occlusions are only ignored behind objects with a higher or equal velocity                                                                      |
+| `ignore_velocity_thresholds.custom_labels`     | [-]   | string list | labels for which to define a non-default velocity threshold (see `autoware_auto_perception_msgs::msg::ObjectClassification` for all the labels) |
+| `ignore_velocity_thresholds.custom_thresholds` | [-]   | double list | velocities of the custom labels                                                                                                                 |
+| `extra_predicted_objects_size`                 | [m]   | double      | extra size added to the objects for masking the occlusions                                                                                      |
+
 ### Others
 
 In the `common` namespace, the following parameters are defined.

planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml

@@ -68,3 +68,20 @@
           bicycle: true # [-] whether to look and stop by BICYCLE objects
           motorcycle: true # [-] whether to look and stop by MOTORCYCLE objects (tmp: currently it is difficult for perception modules to detect bicycles and motorcycles separately.)
           pedestrian: true # [-] whether to look and stop by PEDESTRIAN objects
+
+      # param for occlusions
+      occlusion:
+        enable: true  # if true, ego will slowdown around crosswalks that are occluded
+        occluded_object_velocity: 1.0 # [m/s] assumed velocity of objects that may come out of the occluded space
+        slow_down_velocity: 1.0  # [m/s]
+        time_buffer: 1.0  # [s] consecutive time with/without an occlusion to add/remove the slowdown
+        min_size: 0.5  # [m] minimum size of an occlusion (square side size)
+        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_red_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
+        ignore_velocity_thresholds:
+          default: 0.5  # [m/s] occlusions are only ignored behind objects with a higher or equal velocity
+          custom_labels: ["PEDESTRIAN"]  # labels for which to define a non-default velocity threshold (see autoware_auto_perception_msgs::msg::ObjectClassification for all the labels)
+          custom_thresholds: [0.0]  # velocities of the custom labels
+        extra_predicted_objects_size: 0.5  # [m] extra size added to the objects for masking the occlusions