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/config/crosswalk.param.yaml

@@ -79,7 +79,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/src/manager.cpp

@@ -137,8 +137,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/occluded_crosswalk.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.hpp

@@ -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/scene_crosswalk.cpp

@@ -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.hpp

@@ -163,7 +163,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;