Add option to ignore crosswalk with traffic light + apply max decel/jerk

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

planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml

@@ -79,3 +79,4 @@
         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

planning/behavior_velocity_crosswalk_module/src/manager.cpp

@@ -131,11 +131,13 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node)
   cp.occlusion_occluded_object_velocity =
     getOrDeclareParameter<double>(node, ns + ".occlusion.occluded_object_velocity");
   cp.occlusion_slow_down_velocity =
-    getOrDeclareParameter<double>(node, ns + ".occlusion.slow_down_velocity");
+    getOrDeclareParameter<float>(node, ns + ".occlusion.slow_down_velocity");
   cp.occlusion_time_buffer = getOrDeclareParameter<double>(node, ns + ".occlusion.time_buffer");
   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");
 }
 
 void CrosswalkModuleManager::launchNewModules(const PathWithLaneId & path)

planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp

@@ -51,6 +51,16 @@ 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;
+}
+
 bool is_crosswalk_occluded(
   const lanelet::ConstLanelet & crosswalk_lanelet,
   const nav_msgs::msg::OccupancyGrid & occupancy_grid,

planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp

@@ -48,6 +48,13 @@ 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

@@ -235,7 +235,9 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto
       static_cast<float>(crosswalk_.attribute("safety_slow_down_speed").asDouble().get()));
   }
   // Apply safety slow down speed if the crosswalk is occluded
-  if (planner_param_.occlusion_enable && !path_intersects.empty()) {
+  if (
+    planner_param_.occlusion_enable && !path_intersects.empty() &&
+    !is_crosswalk_ignored(crosswalk_, planner_param_.occlusion_ignore_with_traffic_light)) {
     const auto dist_ego_to_crosswalk =
       calcSignedArcLength(path->points, ego_pos, path_intersects.front());
     const auto detection_range = calculate_detection_range(
@@ -256,9 +258,12 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto
       const auto is_last_occlusion_within_time_buffer =
         most_recent_occlusion_time_ && (clock_->now() - *most_recent_occlusion_time_).seconds() <=
                                          planner_param_.occlusion_time_buffer;
-      if (is_last_occlusion_within_time_buffer)
+      if (is_last_occlusion_within_time_buffer) {
+        const auto target_velocity = calcTargetVelocity(path_intersects.front(), *path);
         applySafetySlowDownSpeed(
-          *path, path_intersects, planner_param_.occlusion_slow_down_velocity);
+          *path, path_intersects,
+          std::max(target_velocity, planner_param_.occlusion_slow_down_velocity));
+      }
     }
   }
   recordTime(2);

planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp

@@ -158,11 +158,12 @@ class CrosswalkModule : public SceneModuleInterface
     // param for occlusions
     bool occlusion_enable;
     double occlusion_occluded_object_velocity;
-    double occlusion_slow_down_velocity;
+    float occlusion_slow_down_velocity;
     double occlusion_time_buffer;
     double occlusion_min_size;
     int occlusion_free_space_max;
     int occlusion_occupied_min;
+    bool occlusion_ignore_with_traffic_light;
   };
 
   struct ObjectInfo