todo: add state diagram

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

planning/behavior_velocity_intersection_module/README.md

@@ -83,7 +83,7 @@ The following process is performed for the targets objects to determine whether
 1. predict the time $t$ when the object intersects with ego path from the predicted path time step. Only the predicted whose confidence is greater than `collision_detection.min_predicted_path_confidence` is used.
 2. detect collision between the predicted path and ego's predicted path in the following process
    1. calculate the collision interval of [$t$ - `collision_detection.collision_start_margin_time`, $t$ + `collision_detection.collision_end_margin_time`]
-   2. obtain the passing area of ego vehicle during the collision interval from the array of (time, distance) obtained by smoothed velocity profile
+   2. calculate the passing area of ego vehicle during the collision interval from the array of (time, distance) obtained by smoothed velocity profile
    3. check if ego passing area and object predicted path interval collides
 3. if collision is detected, the module inserts a stopline
 4. if ego is over the [pass_judge_line](#pass-judge-line), collision checking is skipped to avoid sudden braking and/or unnecessary stop in the middle of the intersection
@@ -103,19 +103,6 @@ ros2 run behavior_velocity_intersection_module ttc.py --lane_id <lane_id>
 
 ![ego ttc profile](./docs/ttc.gif)
 
-### Pass Judge Line
-
-To avoid sudden braking, if deceleration and jerk more than the threshold (`common.max_accel` and `common.max_jerk`) is required to stop at first_attention_stopline, this module does not command to stop once it passed the default_stopline position.
-
-The position of the pass judge line depends on the occlusion detection configuration and the existence of the associated traffic light of the intersection lane.
-
-- If `occlusion.enable` is false, the pass judge line before the `first_attention_stopline` by the braking distance $v_{ego}^{2} / 2a_{max}$.
-- If `occlusion.enable` is true and:
-  - if there are associated traffic lights, the pass judge line is at the `occlusion_peeking_stopline` in order to continue peeking/collision detection while occlusion is detected.
-  - if there are no associated traffic lights and:
-    - if occlusion is detected, pass judge line is at the `occlusion_wo_tl_pass_judge_line` to continue peeking.
-    - if occlusion is not detected, pass judge line is at the same place at the case where `occlusion.enable` is false.
-
 ### 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 ego vehicle first make a brief stop at the default stop line for `occlusion.temporal_stop_time_before_peeking`, and then slowly creep toward occlusion_peeking_stop_line at the speed of `occlusion.creep_during_peeking.creep_velocity` if `occlusion.creep_during_peeking.enable` is true. During the creeping if collision is detected this module inserts a stop line in front of ego vehicle immediately, and if the FOV gets sufficiently clear the intersection_occlusion wall will disappear. If occlusion is cleared and no collision is detected ego vehicle will pass the intersection.
@@ -128,7 +115,9 @@ If the nearest occlusion cell value is below the threshold, occlusion is detecte
 
 #### Occlusion source estimation with traffic light
 
-At intersection with traffic light, the whereabout of occlusion is estimated by checking if there are any objects between ego and the nearest occlusion cell. While the occlusion is estimated to be caused by some object (DYNAMICALLY occluded), intersection_wall will be inserted. If no objects are found between ego and the nearest occlusion cell (STATICALLY occluded), after ego stopped for the duration of `occlusion.static_occlusion_with_traffic_light_timeout`, occlusion is ignored to avoid stuck.
+At intersection with traffic light, the whereabout of occlusion is estimated by checking if there are any objects between ego and the nearest occlusion cell. While the occlusion is estimated to be caused by some object (DYNAMICALLY occluded), intersection_wall will be inserted. If no objects are found between ego and the nearest occlusion cell (STATICALLY occluded), after ego stopped for the duration of `occlusion.static_occlusion_with_traffic_light_timeout`, occlusion is intentionally ignored to avoid stuck.
+
+![occlusion_detection](./docs/occlusion-with-tl.drawio.svg)
 
 #### Occlusion handling without traffic light
 
@@ -164,6 +153,19 @@ If the traffic light color is RED or Arrow signal is turned on, the attention la
 
 When the traffic light color/shape is RED/Arrow, occlusion detection is skipped.
 
+### Pass Judge Line
+
+To avoid sudden braking, if deceleration and jerk more than the threshold (`common.max_accel` and `common.max_jerk`) is required to stop at first_attention_stopline, this module does not command to stop once it passed the default_stopline position.
+
+The position of the pass judge line depends on the occlusion detection configuration and the existence of the associated traffic light of the intersection lane.
+
+- If `occlusion.enable` is false, the pass judge line before the `first_attention_stopline` by the braking distance $v_{ego}^{2} / 2a_{max}$.
+- If `occlusion.enable` is true and:
+  - if there are associated traffic lights, the pass judge line is at the `occlusion_peeking_stopline` in order to continue peeking/collision detection while occlusion is detected.
+  - if there are no associated traffic lights and:
+    - if occlusion is detected, pass judge line is at the `occlusion_wo_tl_pass_judge_line` to continue peeking.
+    - if occlusion is not detected, pass judge line is at the same place at the case where `occlusion.enable` is false.
+
 ### Data Structure
 
 Each data structure is defined in `util_type.hpp`.