Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(intersection): aggressively peek into attention area if traffic light does not exist #5192

Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
93 changes: 79 additions & 14 deletions planning/behavior_velocity_intersection_module/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -24,7 +24,7 @@ This module is activated when the path contains the lanes with `turn_direction`

### Attention area

The `Attention Area` in the intersection are defined as the set of lanes that are conflicting with ego vehicle's path and their preceding lanes up to `attention_area_length` meters. `RightOfWay` tag is used to rule out the lanes that each lane has priority given the traffic light relation and `turn_direction` priority.
The `attention area` in the intersection are defined as the set of lanes that are conflicting with ego vehicle's path and their preceding lanes up to `common.attention_area_length` meters. `RightOfWay` tag is used to rule out the lanes that each lane has priority given the traffic light relation and `turn_direction` priority(`yield lane`).

`Intersection Area`, which is supposed to be defined on the HDMap, is an area converting the entire intersection.

Expand Down Expand Up @@ -54,10 +54,10 @@ For [stuck vehicle detection](#stuck-vehicle-detection) and [collision detection

Objects that satisfy all of the following conditions are considered as target objects (possible collision objects):

- The center of mass of the object is **within a certain distance** from the attention lane (threshold = `attention_area_margin`) .
- The center of mass of the object is **within a certain distance** from the attention lane (threshold = `common.attention_area_margin`) .
- (Optional condition) The center of gravity is in the **intersection area**.
- To deal with objects that is in the area not covered by the lanelets in the intersection.
- The posture of object is **the same direction as the attention lane** (threshold = `attention_area_angle_threshold`).
- The posture of object is **the same direction as the attention lane** (threshold = `common.attention_area_angle_threshold`).
- Not being **in the adjacent lanes of the ego vehicle**.

#### Stuck Vehicle Detection
Expand All @@ -68,31 +68,31 @@ If there is any object on the path in inside the intersection and at the exit of

#### Collision detection

The following process is performed for the targets objects to determine whether the ego vehicle can pass the intersection safely. If it is judged that the ego vehicle cannot pass through the intersection with enough _margin_, it will insert a stopline on the _path_.
The following process is performed for the targets objects to determine whether the ego vehicle can pass the intersection safely. If it is judged that the ego vehicle cannot pass through the intersection with enough margin, this module inserts a stopline on the path.

1. calculate the time interval that the ego vehicle is in the intersection. This time is set as $t_s$ ~ $t_e$
2. extract the predicted path of the target object whose confidence is greater than `min_predicted_path_confidence`.
2. extract the predicted path of the target object whose confidence is greater than `collision_detection.min_predicted_path_confidence`.
3. detect collision between the extracted predicted path and ego's predicted path in the following process.
1. obtain the passing area of the ego vehicle $A_{ego}$ in $t_s$ ~ $t_e$.
2. calculate the passing area of the target object $A_{target}$ at $t_s$ - `collision_start_margin_time` ~ $t_e$ + `collision_end_margin_time` for each predicted path (\*1).
2. calculate the passing area of the target object $A_{target}$ at $t_s$ - `collision_detection.collision_start_margin_time` ~ $t_e$ + `collision_detection.collision_end_margin_time` for each predicted path (\*1).
3. check if $A_{ego}$ and $A_{target}$ polygons are overlapped (has collision).
4. when a collision is detected, the module inserts a stopline.
5. If ego is over the `pass_judge_line`, collision checking is not processed to avoid sudden braking and/or unnecessary stop in the middle of the intersection.

The parameters `collision_start_margin_time` and `collision_end_margin_time` can be interpreted as follows:
The parameters `collision_detection.collision_start_margin_time` and `collision_detection.collision_end_margin_time` can be interpreted as follows:

- If the ego vehicle was to pass through the intersection earlier than the target object, collision would be detected if the time difference between the two was less than `collision_start_margin_time`.
- If the ego vehicle was to pass through the intersection later than the target object, collision would be detected if the time difference between the two was less than `collision_end_margin_time`.
- If the ego vehicle was to pass through the intersection earlier than the target object, collision would be detected if the time difference between the two was less than `collision_detection.collision_start_margin_time`.
- If the ego vehicle was to pass through the intersection later than the target object, collision would be detected if the time difference between the two was less than `collision_detection.collision_end_margin_time`.

If collision is detected, the state transits to "STOP" immediately. On the other hand, the state does not transit to "GO" unless _safe_ judgement continues for a certain period to prevent the chattering of decisions.
If collision is detected, the state transits to "STOP" immediately. On the other hand, the state does not transit to "GO" unless safe judgement continues for a certain period `collision_detection.state_transit_margin` to prevent the chattering of decisions.

#### Stop Line Automatic Generation

If a stopline is associated with the intersection lane, that line is used as the stopline for collision detection. Otherwise the path is interpolated at a certain intervals (=`path_interpolation_ds`), and the point which is `stop_line_margin` meters behind the attention lane is defined as the position of the stop line for the vehicle front.
If a stopline is associated with the intersection lane on the map, that line is used as the stopline for collision detection. Otherwise the path is interpolated at a certain intervals (=`common.path_interpolation_ds`), and the point which is `stop_line_margin` meters behind the attention area is defined as the position of the stop line for the vehicle front.

#### Pass Judge Line

To avoid a rapid braking, if deceleration and jerk more than a threshold (`behavior_velocity_planner.max_accel` and `behavior_velocity_planner.max_jerk`) are needed to stop just in front of the attention area, this module does not insert stopline after passing the default stopline position.
To avoid sudden braking, if deceleration and jerk more than a threshold (`behavior_velocity_planner.max_accel` and `behavior_velocity_planner.max_jerk`) is required to stop just in front of the attention area, this module does not insert stopline after passing the default stopline position.

### Occlusion detection

Expand All @@ -104,6 +104,66 @@ The occlusion is detected as the common area of occlusion attention area(which i

If the nearest occlusion cell value is below the threshold, occlusion is detected. It is expected that the occlusion gets cleared as the vehicle approaches the occlusion peeking stop line.

In there are no traffic lights associated with the lane, the ego vehicle will make a brief stop at the _default stop line_ and the position where the vehicle heading touches the attention area for the first time(which is denoted as _first attention stop line_). After stopping at the _first attention area stop line_ this module inserts `occlusion.absence_traffic_light.creep_velocity` velocity between ego and the position which is `occlusion.absence_traffic_light.maximum_peeking_distance` ahead of _first attention area stop line_ while occlusion is not cleared. If collision is detected, ego will instantly stop. Once the occlusion is cleared or ego passed `occlusion.absence_traffic_light.maximum_peeking_distance` this module does not detect collision and occlusion because ego vehicle is already inside the intersection.

![occlusion_detection](./docs/occlusion-without-tl.drawio.svg)

### Data Structure

#### `IntersectionLanelets`

```plantuml
@startuml
entity IntersectionLanelets {
* conflicting lanes/area
--
* first conflicting area
The conflicting lane area which the path intersects first
--
* attention lanes/area
--
* first attention lane area
The attention lane area which the path intersects first
--
* occlusion attention lanes/area
Part of attention lanes/area for occlusion detection
--
* is_priortized: bool
If ego vehicle has priority in current traffic light context
}
@enduml
```

#### `IntersectionStopLines`

Each stop lines are generated from interpolated path points to obtain precise positions.

```plantuml
@startuml
entity IntersectionStopLines {
* closest_idx: size_t
closest path point index for ego vehicle
--
* stuck_stop_line: size_t
stop line index on stuck vehicle detection
--
* default_stop_line: size_t
If defined on the map, its index on the path. Otherwise generated before first_attention_stop_line
--
* first_attention_stop_line
The index of the first path point which is inside the attention area
--
* occlusion_peeking_stop_line
The index of the path point for the peeking limit position
--
* pass_judge_line
The index of the path point which is before first_attention_stop_line/occlusion_peeking_stop_line by braking distance
}
@enduml
```

![data structure](./docs/data-structure.drawio.svg)

### Module Parameters

| Parameter | Type | Description |
Expand All @@ -115,7 +175,6 @@ If the nearest occlusion cell value is below the threshold, occlusion is detecte
| `common.intersection_velocity` | double | [m/s] velocity profile for pass judge calculation |
| `common.intersection_max_accel` | double | [m/s^2] acceleration profile for pass judge calculation |
| `common.stop_overshoot_margin` | double | [m] margin for the overshoot from stopline |
| `common.path_interpolation_ds` | double | [m] path interpolation interval |
| `stuck_vehicle.stuck_vehicle_detect_dist` | double | [m] length toward from the exit of intersection for stuck vehicle detection |
| `stuck_vehicle.stuck_vehicle_vel_thr` | double | [m/s] velocity threshold for stuck vehicle detection |
| `collision_detection.state_transit_margin_time` | double | [m] time margin to change state |
Expand All @@ -128,7 +187,13 @@ If the nearest occlusion cell value is below the threshold, occlusion is detecte
| `occlusion.peeking_offset` | double | [m] the offset of the front of the vehicle into the attention area for peeking to occlusion |
| `occlusion.min_vehicle_brake_for_rss` | double | [m/s] assumed minimum brake of the vehicle running from behind the occlusion |
| `occlusion.max_vehicle_velocity_for_rss` | double | [m/s] assumed maximum velocity of the vehicle running from behind the occlusion |
| `occlusion.denoise_kernel` | double | [m] the window size of morphology process for clearing noisy occlusion |

#### For developers only

| Parameter | Type | Description |
| ------------------------------ | ------ | ---------------------------------------------------------------------- |
| `common.path_interpolation_ds` | double | [m] path interpolation interval |
| `occlusion.denoise_kernel` | double | [m] the window size of morphology process for clearing noisy occlusion |

### How to turn parameters

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -54,6 +54,9 @@
ignore_parked_vehicle_speed_threshold: 0.8333 # == 3.0km/h
stop_release_margin_time: 1.5 # [s]
temporal_stop_before_attention_area: false
absence_traffic_light:
creep_velocity: 1.388 # [m/s]
maximum_peeking_distance: 6.0 # [m]

enable_rtc:
intersection: true # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval
Expand Down
Loading
Loading