Skip to content
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.

Commit e8ef9a2

Browse files
committedFeb 19, 2024·
ignore occlusions only if the traffic light is red
Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>
1 parent b681fe7 commit e8ef9a2

File tree

8 files changed

+121
-24
lines changed

8 files changed

+121
-24
lines changed
 

‎planning/behavior_velocity_crosswalk_module/README.md

+27
Original file line numberDiff line numberDiff line change
@@ -193,6 +193,33 @@ document.
193193
| `max_slow_down_accel` | [m/ss] | double | minimum accel deceleration for safe brake |
194194
| `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) |
195195

196+
### Occlusion
197+
198+
This feature makes ego slow down for a crosswalk that is occluded.
199+
200+
Occlusion of the crosswalk is determined using the occupancy grid.
201+
An occlusion is a square of occluded cells
202+
(i.e., their values are between `free_space_max` and `occupied_min`)
203+
of size `min_size`.
204+
If an occlusion is found on the crosswalk or in the areas where pedestrians can enter the crosswalk,
205+
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`).
206+
207+
If the pedestrian light is red, then the occlusion is ignored and ego does not slow down.
208+
209+
<figure markdown>
210+
![stuck_vehicle_attention_range](docs/with_occlusion.svg){width=600}
211+
</figure>
212+
213+
| Parameter | Unit | Type | Description |
214+
| ------------------ | ----- | ------ | ------------------------------------------------------------------------------------ | ----------------------------------------------- |
215+
| `enable` | [-] | bool | if true, ego will slow down around crosswalks that are occluded |
216+
| slow_down_velocity | [m/s] | double | slow down velocity |
217+
| time_buffer | [s] | double | consecutive time with/without an occlusion to add/remove the slow down |
218+
| min_size | 0.5 | [m] | double | minimum size of an occlusion (square side size) |
219+
| detection_range | [m] | double | distance away from the crosswalk-ego path intersection that is checked for occlusion |
220+
| free_space_max | [-] | double | maximum value of a free space cell in the occupancy grid |
221+
| occupied_min | [-] | double | minimum value of an occupied cell in the occupancy grid |
222+
196223
### Others
197224

198225
In the `common` namespace, the following parameters are defined.

‎planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml

+1-1
Original file line numberDiff line numberDiff line change
@@ -78,7 +78,7 @@
7878
min_size: 0.5 # [m] minimum size of an occlusion (square side size)
7979
free_space_max: 43 # [-] maximum value of a free space cell in the occupancy grid
8080
occupied_min: 58 # [-] minimum value of an occupied cell in the occupancy grid
81-
ignore_with_traffic_light: true # [-] if true, occlusions at crosswalks with traffic lights are ignored
81+
ignore_with_red_traffic_light: true # [-] if true, occlusions at crosswalks with traffic lights are ignored
8282
ignore_behind_predicted_objects: true # [-] if true, occlusions behind predicted objects are ignored
8383
ignore_velocity_thresholds:
8484
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

+86
Loading

‎planning/behavior_velocity_crosswalk_module/src/manager.cpp

+2-2
Original file line numberDiff line numberDiff line change
@@ -135,8 +135,8 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node)
135135
cp.occlusion_min_size = getOrDeclareParameter<double>(node, ns + ".occlusion.min_size");
136136
cp.occlusion_free_space_max = getOrDeclareParameter<int>(node, ns + ".occlusion.free_space_max");
137137
cp.occlusion_occupied_min = getOrDeclareParameter<int>(node, ns + ".occlusion.occupied_min");
138-
cp.occlusion_ignore_with_traffic_light =
139-
getOrDeclareParameter<bool>(node, ns + ".occlusion.ignore_with_traffic_light");
138+
cp.occlusion_ignore_with_red_traffic_light =
139+
getOrDeclareParameter<bool>(node, ns + ".occlusion.ignore_with_red_traffic_light");
140140
cp.occlusion_ignore_behind_predicted_objects =
141141
getOrDeclareParameter<bool>(node, ns + ".occlusion.ignore_behind_predicted_objects");
142142

‎planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp

-10
Original file line numberDiff line numberDiff line change
@@ -51,16 +51,6 @@ lanelet::BasicPoint2d interpolate_point(
5151
return segment.second + extra_distance * direction_vector;
5252
}
5353

54-
bool is_crosswalk_ignored(
55-
const lanelet::ConstLanelet & crosswalk_lanelet, const bool ignore_with_traffic_light)
56-
{
57-
const auto traffic_lights_reg_elems =
58-
crosswalk_lanelet.regulatoryElementsAs<const lanelet::TrafficLight>();
59-
const bool has_traffic_light = !traffic_lights_reg_elems.empty();
60-
const bool has_skip_attribute = crosswalk_lanelet.hasAttribute("skip_occluded_slowdown");
61-
return (ignore_with_traffic_light && has_traffic_light) || has_skip_attribute;
62-
}
63-
6454
std::vector<lanelet::BasicPolygon2d> calculate_detection_areas(
6555
const lanelet::ConstLanelet & crosswalk_lanelet, const lanelet::BasicPoint2d & crosswalk_origin,
6656
const double detection_range)

‎planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp

-7
Original file line numberDiff line numberDiff line change
@@ -49,13 +49,6 @@ bool is_occluded(
4949
lanelet::BasicPoint2d interpolate_point(
5050
const lanelet::BasicSegment2d & segment, const double extra_distance);
5151

52-
/// @brief check if the crosswalk should be ignored
53-
/// @param crosswalk_lanelet lanelet of the crosswalk
54-
/// @param ignore_with_traffic_light if true, ignore the crosswalk if it has a traffic light
55-
/// @return true if the crosswalk should be ignored
56-
bool is_crosswalk_ignored(
57-
const lanelet::ConstLanelet & crosswalk_lanelet, const bool ignore_with_traffic_light);
58-
5952
/// @brief check if the crosswalk is occluded
6053
/// @param crosswalk_lanelet lanelet of the crosswalk
6154
/// @param occupancy_grid occupancy grid with the occlusion information

‎planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp

+4-3
Original file line numberDiff line numberDiff line change
@@ -240,9 +240,10 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto
240240
const auto cmp_with_time_buffer = [&](const auto & t, const auto cmp_fn) {
241241
return t && cmp_fn((now - *t).seconds(), planner_param_.occlusion_time_buffer);
242242
};
243-
if (
244-
planner_param_.occlusion_enable && !path_intersects.empty() &&
245-
!is_crosswalk_ignored(crosswalk_, planner_param_.occlusion_ignore_with_traffic_light)) {
243+
const auto is_crosswalk_ignored =
244+
(planner_param_.occlusion_ignore_with_red_traffic_light && isRedSignalForPedestrians()) ||
245+
crosswalk_.hasAttribute("skip_occluded_slowdown");
246+
if (planner_param_.occlusion_enable && !path_intersects.empty() && !is_crosswalk_ignored) {
246247
const auto dist_ego_to_crosswalk =
247248
calcSignedArcLength(path->points, ego_pos, path_intersects.front());
248249
const auto detection_range =

‎planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -162,7 +162,7 @@ class CrosswalkModule : public SceneModuleInterface
162162
double occlusion_min_size;
163163
int occlusion_free_space_max;
164164
int occlusion_occupied_min;
165-
bool occlusion_ignore_with_traffic_light;
165+
bool occlusion_ignore_with_red_traffic_light;
166166
bool occlusion_ignore_behind_predicted_objects;
167167
std::vector<double> occlusion_ignore_velocity_thresholds;
168168
double occlusion_extra_objects_size;

0 commit comments

Comments
 (0)
Please sign in to comment.