feat(crosswalk): ignore occlusions in the presence of traffic lights (#…

…6604)

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

planning/behavior_velocity_crosswalk_module/README.md

@@ -209,7 +209,7 @@ This range is meant to be large when ego is far from the crosswalk and small whe
 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 occlusions when the crosswalk has a traffic light, `ignore_with_traffic_light` should be set to true.
 
 To ignore temporary occlusions caused by moving objects,
 `ignore_behind_predicted_objects` should be set to true.
@@ -230,7 +230,7 @@ To inflate the masking behind objects, their footprint can be made bigger using
 | `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_with_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) |

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_red_traffic_light: true  # [-] if true, occlusions at crosswalks with traffic lights are ignored
+        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
         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/include/behavior_velocity_crosswalk_module/util.hpp

@@ -67,6 +67,7 @@ struct DebugData
   bool ignore_crosswalk{false};
   double base_link2front;
   double stop_judge_range{};
+  std::string virtual_wall_suffix{};
 
   geometry_msgs::msg::Pose first_stop_pose;
   geometry_msgs::msg::Point nearest_collision_point;

planning/behavior_velocity_crosswalk_module/src/debug.cpp

@@ -190,6 +190,7 @@ motion_utils::VirtualWalls CrosswalkModule::createVirtualWalls()
     virtual_walls.push_back(wall);
   }
   wall.style = motion_utils::VirtualWallType::slowdown;
+  wall.text += debug_data_.virtual_wall_suffix;
   for (const auto & p : debug_data_.slow_poses) {
     wall.pose = calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0);
     virtual_walls.push_back(wall);

planning/behavior_velocity_crosswalk_module/src/manager.cpp

@@ -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_red_traffic_light =
-    getOrDeclareParameter<bool>(node, ns + ".occlusion.ignore_with_red_traffic_light");
+  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");
 

planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp

@@ -240,8 +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);
   };
+  const auto crosswalk_has_traffic_light =
+    !crosswalk_.regulatoryElementsAs<const lanelet::TrafficLight>().empty();
   const auto is_crosswalk_ignored =
-    (planner_param_.occlusion_ignore_with_red_traffic_light && isRedSignalForPedestrians()) ||
+    (planner_param_.occlusion_ignore_with_traffic_light && crosswalk_has_traffic_light) ||
     crosswalk_.hasAttribute("skip_occluded_slowdown");
   if (planner_param_.occlusion_enable && !path_intersects.empty() && !is_crosswalk_ignored) {
     const auto dist_ego_to_crosswalk =
@@ -269,6 +271,7 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto
         applySafetySlowDownSpeed(
           *path, path_intersects,
           std::max(target_velocity, planner_param_.occlusion_slow_down_velocity));
+        debug_data_.virtual_wall_suffix = " (occluded)";
       } else {
         most_recent_occlusion_time_.reset();
       }

planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp

@@ -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_red_traffic_light;
+    bool occlusion_ignore_with_traffic_light;
     bool occlusion_ignore_behind_predicted_objects;
     std::vector<double> occlusion_ignore_velocity_thresholds;
     double occlusion_extra_objects_size;