diff --git a/planning/obstacle_stop_planner/README.md b/planning/obstacle_stop_planner/README.md
index bbe5ab3d85401..2f2f6226b76c6 100644
--- a/planning/obstacle_stop_planner/README.md
+++ b/planning/obstacle_stop_planner/README.md
@@ -11,21 +11,6 @@
- Adaptive Cruise Controller (ACC)
- embeds target velocity in trajectory when there is a dynamic point cloud on the trajectory.
-In order to stop with a `stop margin` from the obstacle exists, the stop point (`v=0`) is inserted at a distance of `baselink to front` + `stop margin` from the obstacle. The `baselink to front` means the distance between `base_link`(center of rear-wheel axis) and front of the car.
-
-If a stop point has already been inserted by other nodes between the obstacle and a position which is `stop margin` meters away from the obstacle, the stop point is inserted at a distance of `baselink to front` + `min behavior stop margin` from the obstacle.
-
-
-
-
-
-
-When the deceleration section is inserted, the start point of the section is inserted in front of the target point cloud by the distance of `baselink to front` + `slow down forward margin`. the end point of the section is inserted behind the target point cloud by the distance of `slow down backward margin` + `baselink to rear`. The `baselink to rear` means the distance between `base_link` and rear of the car. The velocities of points in the deceleration section are modified to the deceleration velocity. `slow down backward margin` and `slow down forward margin` are determined by the parameters described below.
-
-
-
-
-
## Input topics
| Name | Type | Description |
@@ -48,27 +33,41 @@ When the deceleration section is inserted, the start point of the section is ins
### Common Parameter
-| Parameter | Type | Description |
-| ------------------- | ------ | ---------------------------------------------------------------------------------------- |
-| `enable_slow_down` | bool | enable slow down planner [-] |
-| `max_velocity` | double | max velocity [m/s] |
-| `hunting_threshold` | double | # even if the obstacle disappears, the stop judgment continues for hunting_threshold [s] |
-| `lowpass_gain` | double | low pass gain for calculating acceleration [-] |
+| Parameter | Type | Description |
+| ------------------- | ------ | -------------------------------------------------------------------------------------- |
+| `enable_slow_down` | bool | enable slow down planner [-] |
+| `max_velocity` | double | max velocity [m/s] |
+| `hunting_threshold` | double | even if the obstacle disappears, the stop judgment continues for hunting_threshold [s] |
+| `lowpass_gain` | double | low pass gain for calculating acceleration [-] |
### Obstacle Stop Planner
#### Role
-`Obstacle Stop Planner` module inserts a stop point in trajectory when there is a static point cloud on the trajectory. This module does not work when `Adaptive Cruise Controller` works.
+`obstacle stop planner` module inserts the stop point in the trajectory and modifies that's velocity if there is any obstacle point cloud that has risk to collide with the ego on the trajectory.
+
+This module inserts the stop point before the obstacle with margin. In nominal case, the margin is the sum of `baselink_to_front` and `max_longitudinal_margin`. The `baselink_to_front` means the distance between `base_link`(center of rear-wheel axis) and front of the car.
+
+
+ {width=1000}
+ parameters for obstacle stop planner
+
+
+If another stop point has already been inserted by other modules within `max_longitudinal_margin`, the margin is the sum of `baselink_to_front` and `min_longitudinal_margin`. This feature exists to avoid stopping unnaturally position. (For example, the ego stops unnaturally far away from stop line of crosswalk that pedestrians cross to without this feature.)
+
+
+ {width=1000}
+ minimium longitudinal margin
+
-| Parameter | Type | Description |
-| ---------------------------------------- | ------ | ----------------------------------------------------------------------------- |
-| `stop_planner.stop_margin` | double | stop margin distance from obstacle on the path [m] |
-| `stop_planner.min_behavior_stop_margin` | double | stop margin distance when any other stop point is inserted in stop margin [m] |
-| `stop_planner.step_length` | double | step length for pointcloud search range [m] |
-| `stop_planner.extend_distance` | double | extend trajectory to consider after goal obstacle in the extend_distance [m] |
-| `stop_planner.expand_stop_range` | double | margin of vehicle footprint [m] |
-| `stop_planner.hold_stop_margin_distance` | double | parameter for restart prevention (See following section) [m] |
+| Parameter | Type | Description |
+| ---------------------------------------- | ------ | ---------------------------------------------------------------------------------------------------------------------------------------------- |
+| `stop_planner.max_longitudinal_margin` | double | stop margin distance from obstacle on the path [m] |
+| `stop_planner.min_longitudinal_margin` | double | if any obstacle exists within `max_longitudinal_margin`, this module set margin as the value of _stop margin_ to `min_longitudinal_margin` [m] |
+| `stop_planner.lateral_margin` | double | lateral margin from the vehicle footprint for collision obstacle detection area [m] |
+| `stop_planner.step_length` | double | step length for pointcloud search range [m] |
+| `stop_planner.extend_distance` | double | extend trajectory to consider after goal obstacle in the extend_distance [m] |
+| `stop_planner.hold_stop_margin_distance` | double | parameter for restart prevention (See following section) [m] |
#### Flowchart
@@ -117,7 +116,7 @@ The module searches the obstacle pointcloud within detection area. When the poin
If it needs X meters (e.g. 0.5 meters) to stop once the vehicle starts moving due to the poor vehicle control performance, the vehicle goes over the stopping position that should be strictly observed when the vehicle starts to moving in order to approach the near stop point (e.g. 0.3 meters away).
-This module has parameter `hold_stop_margin_distance` in order to prevent from these redundant restart. If the vehicle is stopped within `hold_stop_margin_distance` meters from stop point of the module (\_front_to_stop_line < hold_stop_margin_distance), the module judges that the vehicle has already stopped for the module's stop point and plans to keep stopping current position even if the vehicle is stopped due to other factors.
+This module has parameter `hold_stop_margin_distance` in order to prevent from these redundant restart. If the vehicle is stopped within `hold_stop_margin_distance` meters from stop point of the module (\front_to_stop_line < hold_stop_margin_distance), the module judges that the vehicle has already stopped for the module's stop point and plans to keep stopping current position even if the vehicle is stopped due to other factors.
{width=1000}
@@ -138,15 +137,33 @@ This module has parameter `hold_stop_margin_distance` in order to prevent from t
#### Role
-`Slow Down Planner` module inserts a deceleration point in trajectory when there is a point cloud near the trajectory.
+`slow down planner` module inserts the deceleration point in trajectory when there is a point cloud near the trajectory.
+
+This module inserts the slow down section before the obstacle with forward margin and backward margin. The forward margin is the sum of `baselink_to_front` and `longitudinal_forward_margin`, and the backward margin is the sum of `baselink_to_front` and `longitudinal_backward_margin`. The ego keeps slow down velocity in slow down section. The velocity is calculated the following equation.
+
+$v_{target} = v_{min} + \frac{l_{ld} - l_{vw}/2}{l_{margin}} (v_{max} - v_{min} )$
+
+- $v_{target}$ : slow down target velocity [m/s]
+- $v_{min}$ : `slow_down_planner.min_slow_down_velocity` [m/s]
+- $v_{max}$ : `slow_down_planner.max_slow_down_velocity` [m/s]
+- $l_{ld}$ : lateral deviation between the obstacle and the ego footprint [m]
+- $l_{margin}$ : `slow_down_planner.lateral_margin` [m]
+- $l_{vw}$ : width of the ego footprint [m]
+
+The above equation means that the smaller the lateral deviation of the pointcloud, the lower the velocity of the slow down section.
+
+
+ {width=1000}
+ parameters for slow down planner
+
-| Parameter | Type | Description |
-| --------------------------------------------- | ------ | ---------------------------------------------------------------------------------------------- |
-| `slow_down_planner.slow_down_forward_margin` | double | margin distance from slow down point to vehicle front [m] |
-| `slow_down_planner.slow_down_backward_margin` | double | margin distance from slow down point to vehicle rear [m] |
-| `slow_down_planner.expand_slow_down_range` | double | offset from vehicle side edge for expanding the search area of the surrounding point cloud [m] |
-| `slow_down_planner.max_slow_down_vel` | double | max slow down velocity [m/s] |
-| `slow_down_planner.min_slow_down_vel` | double | min slow down velocity [m/s] |
+| Parameter | Type | Description |
+| ------------------------------------------------ | ------ | ----------------------------------------------------------------------------------- |
+| `slow_down_planner.longitudinal_forward_margin` | double | margin distance from slow down point to vehicle front [m] |
+| `slow_down_planner.longitudinal_backward_margin` | double | margin distance from slow down point to vehicle rear [m] |
+| `slow_down_planner.lateral_margin` | double | lateral margin from the vehicle footprint for slow down obstacle detection area [m] |
+| `slow_down_planner.max_slow_down_velocity` | double | max slow down velocity [m/s] |
+| `slow_down_planner.min_slow_down_velocity` | double | min slow down velocity [m/s] |
#### Flowchart
@@ -178,7 +195,7 @@ stop
First, this module cut off the trajectory behind the car and decimates the points of trajectory for reducing computational costs. ( This is the same process as that of `Obstacle Stop planner` module. )
-Then, a detection area is generated by the decimated trajectory as following figure. The detection area in this module is the extended area of the detection area used in `Obstacle Stop Planner` module. The distance to be extended depends on the above parameter `expand_slow_down_range`.
+Then, a detection area is generated by the decimated trajectory as following figure. The detection area in this module is the extended area of the detection area used in `Obstacle Stop Planner` module. The distance to be extended depends on the above parameter `lateral_margin`.
@@ -186,18 +203,6 @@ The module searches the obstacle pointcloud within detection area. When the poin
-The deceleration point means the point with low velocity; the value of the velocity $v_{target}$ is determined as follows.
-
-$v_{target} = v_{min} + \frac{l_{ld} - l_{vw}/2}{l_{er}} (v_{max} - v_{min} )$
-
-- $v_{min}$ is minimum target value of `Slow Down Planner` module. The value of $v_{min}$ depends on the parameter `min_slow_down_vel`.
-- $v_{max}$ is maximum target value of `Slow Down Planner` module. The value of $v_{max}$ depends on the parameter `max_slow_down_vel`.
-- $l_{ld}$ is the lateral deviation of the target pointcloud.
-- $l_{vw}$ is the vehicle width.
-- $l_{er}$ is the expand range of detection area. The value of $l_{er}$ depends on the parameter `expand_slow_down_range`
-
-The above method means that the smaller the lateral deviation of the pointcloud, the lower the velocity of the deceleration point.
-
diff --git a/planning/obstacle_stop_planner/docs/collision_paramters.svg b/planning/obstacle_stop_planner/docs/collision_paramters.svg
new file mode 100644
index 0000000000000..a1f3c346bd305
--- /dev/null
+++ b/planning/obstacle_stop_planner/docs/collision_paramters.svg
@@ -0,0 +1,3 @@
+
+
+
\ No newline at end of file
diff --git a/planning/obstacle_stop_planner/docs/insert_decel_velocity.drawio.svg b/planning/obstacle_stop_planner/docs/insert_decel_velocity.drawio.svg
deleted file mode 100644
index a49853974a2f9..0000000000000
--- a/planning/obstacle_stop_planner/docs/insert_decel_velocity.drawio.svg
+++ /dev/null
@@ -1,4 +0,0 @@
-
-
-
-
\ No newline at end of file
diff --git a/planning/obstacle_stop_planner/docs/insert_velocity.drawio.svg b/planning/obstacle_stop_planner/docs/insert_velocity.drawio.svg
deleted file mode 100644
index bba4ad8a79115..0000000000000
--- a/planning/obstacle_stop_planner/docs/insert_velocity.drawio.svg
+++ /dev/null
@@ -1,208 +0,0 @@
-
\ No newline at end of file
diff --git a/planning/obstacle_stop_planner/docs/insert_velocity1.drawio.svg b/planning/obstacle_stop_planner/docs/insert_velocity1.drawio.svg
deleted file mode 100644
index dd1054e17fb14..0000000000000
--- a/planning/obstacle_stop_planner/docs/insert_velocity1.drawio.svg
+++ /dev/null
@@ -1,4 +0,0 @@
-
-
-
-
\ No newline at end of file
diff --git a/planning/obstacle_stop_planner/docs/insert_velocity2.drawio.svg b/planning/obstacle_stop_planner/docs/insert_velocity2.drawio.svg
deleted file mode 100644
index 941668f909b60..0000000000000
--- a/planning/obstacle_stop_planner/docs/insert_velocity2.drawio.svg
+++ /dev/null
@@ -1,4 +0,0 @@
-
-
-
-
\ No newline at end of file
diff --git a/planning/obstacle_stop_planner/docs/min_longitudinal_margin.svg b/planning/obstacle_stop_planner/docs/min_longitudinal_margin.svg
new file mode 100644
index 0000000000000..fb42e7f03a641
--- /dev/null
+++ b/planning/obstacle_stop_planner/docs/min_longitudinal_margin.svg
@@ -0,0 +1,3 @@
+
+
+
\ No newline at end of file
diff --git a/planning/obstacle_stop_planner/docs/slow_down_parameters.svg b/planning/obstacle_stop_planner/docs/slow_down_parameters.svg
new file mode 100644
index 0000000000000..e16cc0dfa326c
--- /dev/null
+++ b/planning/obstacle_stop_planner/docs/slow_down_parameters.svg
@@ -0,0 +1,3 @@
+
+
+
\ No newline at end of file
diff --git a/planning/obstacle_stop_planner/docs/velocity_limitation.drawio.svg b/planning/obstacle_stop_planner/docs/velocity_limitation.drawio.svg
deleted file mode 100644
index e63c63d1ee1da..0000000000000
--- a/planning/obstacle_stop_planner/docs/velocity_limitation.drawio.svg
+++ /dev/null
@@ -1,270 +0,0 @@
-
\ No newline at end of file
- 
-
-