diff --git a/planning/behavior_velocity_crosswalk_module/README.md b/planning/behavior_velocity_crosswalk_module/README.md index 4c7b214763c31..7d6acbfc6aa9a 100644 --- a/planning/behavior_velocity_crosswalk_module/README.md +++ b/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`. + +
+ ![stuck_vehicle_attention_range](docs/with_occlusion.svg){width=600} +
+ +| 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. diff --git a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml index f8b9e5dbf1327..09ce64593ff46 100644 --- a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml +++ b/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 diff --git a/planning/behavior_velocity_crosswalk_module/docs/with_occlusion.svg b/planning/behavior_velocity_crosswalk_module/docs/with_occlusion.svg new file mode 100644 index 0000000000000..a98995e0da3a9 --- /dev/null +++ b/planning/behavior_velocity_crosswalk_module/docs/with_occlusion.svg @@ -0,0 +1,86 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ occluded area +
+
+
+ + + + + + + + + + + +
+
+
obstacle
+
+
+ + + + + + + diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp index aeff5e27cea7b..39f88d5389c2b 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -135,8 +135,8 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) cp.occlusion_min_size = getOrDeclareParameter(node, ns + ".occlusion.min_size"); cp.occlusion_free_space_max = getOrDeclareParameter(node, ns + ".occlusion.free_space_max"); cp.occlusion_occupied_min = getOrDeclareParameter(node, ns + ".occlusion.occupied_min"); - cp.occlusion_ignore_with_traffic_light = - getOrDeclareParameter(node, ns + ".occlusion.ignore_with_traffic_light"); + cp.occlusion_ignore_with_red_traffic_light = + getOrDeclareParameter(node, ns + ".occlusion.ignore_with_red_traffic_light"); cp.occlusion_ignore_behind_predicted_objects = getOrDeclareParameter(node, ns + ".occlusion.ignore_behind_predicted_objects"); diff --git a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp index 5d0ecd810d845..2455b36d5883f 100644 --- a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp +++ b/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 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 calculate_detection_areas( const lanelet::ConstLanelet & crosswalk_lanelet, const lanelet::BasicPoint2d & crosswalk_origin, const double detection_range) diff --git a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp index 3005263c23f43..a76fdeb549b88 100644 --- a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp +++ b/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 diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 519f2673465d8..00ace56f41d7c 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/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 = diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index d2468e7f31aa7..5ce5d4020928a 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/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_traffic_light; + bool occlusion_ignore_with_red_traffic_light; bool occlusion_ignore_behind_predicted_objects; std::vector occlusion_ignore_velocity_thresholds; double occlusion_extra_objects_size;