ignore occlusions only if the traffic light is red

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

Author: maxime-clem

planning/behavior_velocity_crosswalk_module/README.md

@@ -193,6 +193,33 @@ 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 occluded cells
+(i.e., their values are between `free_space_max` and `occupied_min`)
+of size `min_size`.
+If an occlusion is found on the crosswalk or in the areas where pedestrians can enter 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`).
+
+If the pedestrian light is red, then the occlusion is ignored and ego does not slow down.
+
+<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                      |
+| slow_down_velocity | [m/s] | double | slow down velocity                                                                   |
+| time_buffer        | [s]   | double | consecutive time with/without an occlusion to add/remove the slow down               |
+| min_size           | 0.5   | [m]    | double                                                                               | minimum size of an occlusion (square side size) |
+| detection_range    | [m]   | double | distance away from the crosswalk-ego path intersection that is checked for occlusion |
+| 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                              |
+
 ### Others
 
 In the `common` namespace, the following parameters are defined.

planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml

@@ -78,7 +78,7 @@
         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_traffic_light: true  # [-] if true, occlusions at crosswalks with traffic lights are ignored
+        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

planning/behavior_velocity_crosswalk_module/docs/with_occlusion.svg

@@ -135,8 +135,8 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node)
   cp.occlusion_min_size = getOrDeclareParameter<double>(node, ns + ".occlusion.min_size");
   cp.occlusion_free_space_max = getOrDeclareParameter<int>(node, ns + ".occlusion.free_space_max");
   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_with_red_traffic_light =
+    getOrDeclareParameter<bool>(node, ns + ".occlusion.ignore_with_red_traffic_light");
   cp.occlusion_ignore_behind_predicted_objects =
     getOrDeclareParameter<bool>(node, ns + ".occlusion.ignore_behind_predicted_objects");
 

planning/behavior_velocity_crosswalk_module/src/manager.cpp

@@ -51,16 +51,6 @@ lanelet::BasicPoint2d interpolate_point(
   return segment.second + extra_distance * direction_vector;
 }
 
-bool is_crosswalk_ignored(
-  const lanelet::ConstLanelet & crosswalk_lanelet, const bool ignore_with_traffic_light)
-{
-  const auto traffic_lights_reg_elems =
-    crosswalk_lanelet.regulatoryElementsAs<const lanelet::TrafficLight>();
-  const bool has_traffic_light = !traffic_lights_reg_elems.empty();
-  const bool has_skip_attribute = crosswalk_lanelet.hasAttribute("skip_occluded_slowdown");
-  return (ignore_with_traffic_light && has_traffic_light) || has_skip_attribute;
-}
-
 std::vector<lanelet::BasicPolygon2d> calculate_detection_areas(
   const lanelet::ConstLanelet & crosswalk_lanelet, const lanelet::BasicPoint2d & crosswalk_origin,
   const double detection_range)

planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp

@@ -49,13 +49,6 @@ bool is_occluded(
 lanelet::BasicPoint2d interpolate_point(
   const lanelet::BasicSegment2d & segment, const double extra_distance);
 
-/// @brief check if the crosswalk should be ignored
-/// @param crosswalk_lanelet lanelet of the crosswalk
-/// @param ignore_with_traffic_light if true, ignore the crosswalk if it has a traffic light
-/// @return true if the crosswalk should be ignored
-bool is_crosswalk_ignored(
-  const lanelet::ConstLanelet & crosswalk_lanelet, const bool ignore_with_traffic_light);
-
 /// @brief check if the crosswalk is occluded
 /// @param crosswalk_lanelet lanelet of the crosswalk
 /// @param occupancy_grid occupancy grid with the occlusion information

planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp

@@ -240,9 +240,10 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto
   const auto cmp_with_time_buffer = [&](const auto & t, const auto cmp_fn) {
     return t && cmp_fn((now - *t).seconds(), planner_param_.occlusion_time_buffer);
   };
-  if (
-    planner_param_.occlusion_enable && !path_intersects.empty() &&
-    !is_crosswalk_ignored(crosswalk_, planner_param_.occlusion_ignore_with_traffic_light)) {
+  const auto is_crosswalk_ignored =
+    (planner_param_.occlusion_ignore_with_red_traffic_light && isRedSignalForPedestrians()) ||
+    crosswalk_.hasAttribute("skip_occluded_slowdown");
+  if (planner_param_.occlusion_enable && !path_intersects.empty() && !is_crosswalk_ignored) {
     const auto dist_ego_to_crosswalk =
       calcSignedArcLength(path->points, ego_pos, path_intersects.front());
     const auto detection_range =

planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp

@@ -162,7 +162,7 @@ class CrosswalkModule : public SceneModuleInterface
     double occlusion_min_size;
     int occlusion_free_space_max;
     int occlusion_occupied_min;
-    bool occlusion_ignore_with_traffic_light;
+    bool occlusion_ignore_with_red_traffic_light;
     bool occlusion_ignore_behind_predicted_objects;
     std::vector<double> occlusion_ignore_velocity_thresholds;
     double occlusion_extra_objects_size;