Merge pull request autowarefoundation#922 from tier4/feat/latest-inte…

…rsection-cherry-pick-v0.11.0

feat(intersection): latest intersection cherry pick v0.11.0

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,45 +54,115 @@ 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
 
-If there is any object in a certain distance (`stuck_vehicle_ignore_dist` and `stuck_vehicle_detect_dist`) before and after the exit of the intersection lane and the object velocity is less than a threshold (=`stuck_vehicle_vel_thr`), the object is regarded as a stuck vehicle. If stuck vehicles exist, this module inserts a stopline a certain distance (=`stop_line_margin`) before the overlapped region with other lanes. The stuck vehicle detection area is generated based on the vehicle path, so the stuck vehicle stopline is not inserted if the upstream module generated avoidance path
+If there is any object on the path in inside the intersection and at the exit of the intersection (up to `stuck_vehicle_detect_dist`) lane and its velocity is less than a threshold (`stuck_vehicle.stuck_vehicle_vel_thr`), the object is regarded as a stuck vehicle. If stuck vehicles exist, this module inserts a stopline a certain distance (=`stop_line_margin`) before the overlapped region with other lanes. The stuck vehicle detection area is generated based on the vehicle path, so the stuck vehicle stopline is not inserted if the upstream module generated avoidance path
 
 ![stuck_vehicle_detection](./docs/stuck-vehicle.drawio.svg)
 
 #### 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
+
+If the flag `occlusion.enable` is true this module checks if there is sufficient field of view (FOV) on the attention area up to `occlusion.occlusion_attention_area_length`. If FOV is not clear enough the ego vehicle will once stop at the _default stop line_ for `occlusion.before_creep_stop_time`, and then slowly creep toward _occlusion peeking stop line_ at the speed of `occlusion.occlusion_creep_velocity` if `occlusion.enable_creeping` is true. During the creeping if collision is detected this module inserts a stop line instantly, and if the FOV gets sufficiently clear the _intersection occlusion_ wall will disappear. If occlusion is cleared and no collision is detected the ego vehicle will pass the intersection.
+
+The occlusion is detected as the common area of occlusion attention area(which is partially the same as the normal attention area) and the unknown cells of the occupancy grid map. The occupancy grid map is denoised using morphology with the window size of `occlusion.denoise_kernel`. The occlusion attention area lanes are discretized to line strings and they are used to generate a grid whose each cell represents the distance from the ego path along the lane as shown below.
+
+![occlusion_detection](./docs/occlusion_grid.drawio.svg)
+
+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
 
@@ -105,9 +175,7 @@ To avoid a rapid braking, if deceleration and jerk more than a threshold (`behav
 | `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_ignore_dist`           | double | [m] length behind 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                                                                |
 | `collision_detection.min_predicted_path_confidence` | double | [-] minimum confidence value of predicted path to use for collision detection                  |
@@ -119,7 +187,13 @@ To avoid a rapid braking, if deceleration and jerk more than a threshold (`behav
 | `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

@@ -19,20 +19,28 @@
         # enable_front_car_decel_prediction: false # By default this feature is disabled
         # assumed_front_car_decel: 1.0 # [m/ss] the expected deceleration of front car when front car as well as ego are turning
         timeout_private_area: 3.0 # [s] cancel stuck vehicle stop in private area
+        enable_private_area_stuck_disregard: false #In the intersections which labeled as "private area", the obstacle vehicles judged as "stuck" are neglected if this param is set as true.
 
       collision_detection:
         state_transit_margin_time: 1.0
         min_predicted_path_confidence: 0.05
         minimum_ego_predicted_velocity: 1.388 # [m/s]
-        normal:
-          collision_start_margin_time: 4.0 # [s] this + state_transit_margin_time should be higher to account for collision with fast/accelerating object
-          collision_end_margin_time: 6.0 # [s] this + state_transit_margin_time should be higher to account for collision with slow/decelerating object
-        relaxed:
+        fully_prioritized:
           collision_start_margin_time: 2.0
           collision_end_margin_time: 0.0
+        partially_prioritized:
+          collision_start_margin_time: 2.0
+          collision_end_margin_time: 2.0
+        not_prioritized:
+          collision_start_margin_time: 4.0 # [s] this + state_transit_margin_time should be higher to account for collision with fast/accelerating object
+          collision_end_margin_time: 6.0 # [s] this + state_transit_margin_time should be higher to account for collision with slow/decelerating object
         keep_detection_vel_thr: 0.833 # == 3.0km/h. keep detection if ego is ego.vel < keep_detection_vel_thr
         use_upstream_velocity: true # flag to use the planned velocity profile from the upstream module
         minimum_upstream_velocity: 0.01 # [m/s] minimum velocity to avoid null division for the stop line from the upstream velocity
+        yield_on_green_traffic_light:
+          distance_to_assigned_lanelet_start: 5.0
+          duration: 2.0
+          range: 30.0 # [m]
 
       occlusion:
         enable: false
@@ -50,6 +58,12 @@
         possible_object_bbox: [1.5, 2.5] # [m x m]
         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]
+        attention_lane_crop_curvature_threshold: 0.25
+        attention_lane_curvature_calculation_ds: 0.5
 
       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