feat(intersection): aggressively peek into attention area if traffic …

…light does not exist (autowarefoundation#5192)

Signed-off-by: Mamoru Sobue <[email protected]>

planning/behavior_velocity_intersection_module/README.md

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

planning/behavior_velocity_intersection_module/config/intersection.param.yaml

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