diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/obstacle_stop_planner.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/obstacle_stop_planner.param.yaml
index 82a77203343f7..55a21d6cd7b20 100644
--- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/obstacle_stop_planner.param.yaml
+++ b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/obstacle_stop_planner.param.yaml
@@ -1,25 +1,49 @@
/**:
ros__parameters:
+ hunting_threshold: 0.5 # even if the obstacle disappears, the stop judgment continues for hunting_threshold [s]
+ lowpass_gain: 0.9 # gain parameter for low pass filter [-]
+ max_velocity: 20.0 # max velocity [m/s]
+ enable_slow_down: False # whether to use slow down planner [-]
+
stop_planner:
- stop_margin: 5.0 # stop margin distance from obstacle on the path [m]
- min_behavior_stop_margin: 5.0 # stop margin distance when any other stop point is inserted in stop margin [m]
- step_length: 1.0 # step length for pointcloud search range [m]
- extend_distance: 0.0 # extend trajectory to consider after goal obstacle in the extend_distance
- expand_stop_range: 0.0 # margin of vehicle footprint [m]
+ # params for stop position
+ stop_position:
+ max_longitudinal_margin: 5.0 # stop margin distance from obstacle on the path [m]
+ min_longitudinal_margin: 5.0 # stop margin distance when any other stop point is inserted in stop margin [m]
+ hold_stop_margin_distance: 0.0 # the ego keeps stopping if the ego is in this margin [m]
+
+ # params for detection area
+ detection_area:
+ lateral_margin: 0.0 # margin of vehicle footprint [m]
+ step_length: 1.0 # step length for pointcloud search range [m]
+ extend_distance: 0.0 # extend trajectory to consider after goal obstacle in the extend_distance [m]
+
slow_down_planner:
- # slow down planner parameters
- forward_margin: 5.0 # margin distance from slow down point to vehicle front [m]
- backward_margin: 0.0 # margin distance from slow down point to vehicle rear [m]
- expand_slow_down_range: 1.0 # offset from vehicle side edge for expanding the search area of the surrounding point cloud [m]
- max_slow_down_vel: 1.38 # max slow down velocity [m/s]
- min_slow_down_vel: 0.28 # min slow down velocity [m/s]
-
- # slow down constraint parameters
- consider_constraints: False # set "True", if no decel plan found under jerk/dec constrains, relax target slow down vel
- forward_margin_min: 1.0 # min margin for relaxing slow down margin [m/s]
- forward_margin_span: -0.1 # fineness param for relaxing slow down margin [m/s]
- jerk_min_slow_down: -0.6 # min slow down jerk constraint [m/sss]
- jerk_span: -0.01 # fineness param for planning deceleration jerk [m/sss]
- jerk_start: -0.1 # init jerk used for deceleration planning [m/sss]
- slow_down_vel: 1.38 # target slow down velocity [m/s]
+ # params for slow down section
+ slow_down_section:
+ longitudinal_forward_margin: 5.0 # margin distance from slow down point to vehicle front [m]
+ longitudinal_backward_margin: 0.0 # margin distance from slow down point to vehicle rear [m]
+ longitudinal_margin_span: -0.1 # fineness param for relaxing slow down margin (use this param if consider_constraints is True) [m/s]
+ min_longitudinal_forward_margin: 1.0 # min margin for relaxing slow down margin (use this param if consider_constraints is True) [m/s]
+
+ # params for detection area
+ detection_area:
+ lateral_margin: 1.0 # offset from vehicle side edge for expanding the search area of the surrounding point cloud [m]
+
+ # params for velocity
+ target_velocity:
+ max_slow_down_velocity: 1.38 # max slow down velocity (use this param if consider_constraints is False)[m/s]
+ min_slow_down_velocity: 0.28 # min slow down velocity (use this param if consider_constraints is False)[m/s]
+ slow_down_velocity: 1.38 # target slow down velocity (use this param if consider_constraints is True)[m/s]
+
+ # params for deceleration constraints (use this param if consider_constraints is True)
+ constraints:
+ jerk_min_slow_down: -0.6 # min slow down jerk constraint [m/sss]
+ jerk_span: -0.01 # fineness param for planning deceleration jerk [m/sss]
+ jerk_start: -0.1 # init jerk used for deceleration planning [m/sss]
+
+ # others
+ consider_constraints: False # set "True", if no decel plan found under jerk/dec constrains, relax target slow down vel [-]
+ velocity_threshold_decel_complete: 0.2 # use for judge whether the ego velocity converges the target slow down velocity [m/s]
+ acceleration_threshold_decel_complete: 0.1 # use for judge whether the ego velocity converges the target slow down velocity [m/ss]
diff --git a/planning/obstacle_stop_planner/README.md b/planning/obstacle_stop_planner/README.md
index be62447684982..3b0a38d194a78 100644
--- a/planning/obstacle_stop_planner/README.md
+++ b/planning/obstacle_stop_planner/README.md
@@ -11,22 +11,7 @@
- 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
+### Input topics
| Name | Type | Description |
| --------------------------- | ----------------------------------------------- | ------------------- |
@@ -37,40 +22,87 @@ When the deceleration section is inserted, the start point of the section is ins
| `~/input/dynamic_objects` | autoware_auto_perception_msgs::PredictedObjects | dynamic objects |
| `~/input/expand_stop_range` | tier4_planning_msgs::msg::ExpandStopRange | expand stop range |
-## Output topics
+### Output topics
| Name | Type | Description |
| ---------------------- | --------------------------------------- | -------------------------------------- |
| `~output/trajectory` | autoware_auto_planning_msgs::Trajectory | trajectory to be followed |
| `~output/stop_reasons` | tier4_planning_msgs::StopReasonArray | reasons that cause the vehicle to stop |
-## Modules
-
### 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
+
+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 `baselink`(center of rear-wheel axis) and front of the car. The detection area is generated along the processed trajectory as following figure. (This module cut off the input trajectory behind the ego position and decimates the trajectory points for reducing computational costs.)
+
+
+ {width=1000}
+ parameters for obstacle stop planner
+
+
+
+ {width=1000}
+ target 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}
+ minimum longitudinal margin
+
+
+The module searches the obstacle pointcloud within detection area. When the pointcloud is found, `Adaptive Cruise Controller` modules starts to work. only when `Adaptive Cruise Controller` modules does not insert target velocity, the stop point is inserted to the trajectory. The stop point means the point with 0 velocity.
+
+### Restart prevention
+
+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, 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}
+ parameters
+
+
+
+ {width=1000}
+ outside the hold_stop_margin_distance
+
+
+
+ {width=1000}
+ inside the hold_stop_margin_distance
+
-### Obstacle Stop Planner
+### Parameters
-#### Role
+#### Stop position
-`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.
+| Parameter | Type | Description |
+| --------------------------- | ------ | ---------------------------------------------------------------------------------------------------------------------------------------------- |
+| `max_longitudinal_margin` | double | margin between obstacle and the ego's front [m] |
+| `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] |
+| `hold_stop_margin_distance` | double | parameter for restart prevention (See above section) [m] |
-| 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] |
+#### Obstacle detection area
-#### Flowchart
+| Parameter | Type | Description |
+| ----------------- | ------ | ----------------------------------------------------------------------------------- |
+| `lateral_margin` | double | lateral margin from the vehicle footprint for collision obstacle detection area [m] |
+| `step_length` | double | step length for pointcloud search range [m] |
+| `extend_distance` | double | extend trajectory to consider after goal obstacle in the extend_distance [m] |
+
+### Flowchart
```plantuml
@startuml
@@ -103,52 +135,56 @@ stop
@enduml
```
-First, this module cut off the trajectory behind the car and decimates the points of trajectory for reducing computational costs.
+## Slow Down Planner
-Then, a detection area is generated by the decimated trajectory as following figure. The detection area means the area through which the vehicle-body passes.
+### Role
-
+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.
-The module searches the obstacle pointcloud within detection area. When the pointcloud is found, `Adaptive Cruise Controller` modules starts to work. only when `Adaptive Cruise Controller` modules does not insert target velocity, the stop point is inserted to the trajectory. The stop point means the point with 0 velocity.
+$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}$ : `min_slow_down_velocity` [m/s]
+- $v_{max}$ : `max_slow_down_velocity` [m/s]
+- $l_{ld}$ : lateral deviation between the obstacle and the ego footprint [m]
+- $l_{margin}$ : `lateral_margin` [m]
+- $l_{vw}$ : width of the ego footprint [m]
-#### Restart prevention
-
-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.
+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
+ {width=1000}
+ parameters for slow down planner
- {width=1000}
- outside the hold_stop_margin_distance
+ {width=1000}
+ target for slow down planner
-
- {width=1000}
- inside the hold_stop_margin_distance
-
+### Parameters
+
+#### Slow down section
-### Slow Down Planner
+| Parameter | Type | Description |
+| ------------------------------ | ------ | ----------------------------------------------- |
+| `longitudinal_forward_margin` | double | margin between obstacle and the ego's front [m] |
+| `longitudinal_backward_margin` | double | margin between obstacle and the ego's rear [m] |
-#### Role
+#### Obstacle detection area
-`Slow Down Planner` module inserts a deceleration point in trajectory when there is a point cloud near the trajectory.
+| Parameter | Type | Description |
+| ---------------- | ------ | ----------------------------------------------------------------------------------- |
+| `lateral_margin` | double | lateral margin from the vehicle footprint for slow down obstacle detection area [m] |
-| 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] |
+#### Slow down target velocity
-#### Flowchart
+| Parameter | Type | Description |
+| ------------------------ | ------ | ---------------------------- |
+| `max_slow_down_velocity` | double | max slow down velocity [m/s] |
+| `min_slow_down_velocity` | double | min slow down velocity [m/s] |
+
+### Flowchart
```plantuml
@startuml
@@ -176,35 +212,9 @@ stop
@enduml
```
-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`.
-
-
-
-The module searches the obstacle pointcloud within detection area. When the pointcloud is found, the deceleration point is inserted to the trajectory.
-
-
-
-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.
-
-
-
-
-
-### Adaptive Cruise Controller
+## Adaptive Cruise Controller
-#### Role
+### Role
`Adaptive Cruise Controller` module embeds maximum velocity in trajectory when there is a dynamic point cloud on the trajectory. The value of maximum velocity depends on the own velocity, the velocity of the point cloud ( = velocity of the front car), and the distance to the point cloud (= distance to the front car).
@@ -241,7 +251,7 @@ The above method means that the smaller the lateral deviation of the pointcloud,
| `adaptive_cruise_control.use_rough_velocity_estimation:` | bool | Use rough estimated velocity if the velocity estimation is failed |
| `adaptive_cruise_control.rough_velocity_rate` | double | In the rough velocity estimation, the velocity of front car is estimated as self current velocity \* this value |
-#### Flowchart
+### Flowchart
```plantuml
@startuml
diff --git a/planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml b/planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml
index 6fe30b216118e..ed4e632ebd6b3 100644
--- a/planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml
+++ b/planning/obstacle_stop_planner/config/obstacle_stop_planner.param.yaml
@@ -1,28 +1,49 @@
/**:
ros__parameters:
- hunting_threshold: 0.5 # even if the obstacle disappears, the stop judgment continues for hunting_threshold [s]
+ hunting_threshold: 0.5 # even if the obstacle disappears, the stop judgment continues for hunting_threshold [s]
+ lowpass_gain: 0.9 # gain parameter for low pass filter [-]
+ max_velocity: 20.0 # max velocity [m/s]
+ enable_slow_down: False # whether to use slow down planner [-]
stop_planner:
- stop_margin: 5.0 # stop margin distance from obstacle on the path [m]
- min_behavior_stop_margin: 2.0 # stop margin distance when any other stop point is inserted in stop margin [m]
- step_length: 1.0 # step length for pointcloud search range [m]
- extend_distance: 0.0 # extend trajectory to consider after goal obstacle in the extend_distance
- expand_stop_range: 0.0 # margin of vehicle footprint [m]
- hold_stop_margin_distance: 0.0 # the ego keeps stopping if the ego is in this margin [m]
+ # params for stop position
+ stop_position:
+ max_longitudinal_margin: 5.0 # stop margin distance from obstacle on the path [m]
+ min_longitudinal_margin: 2.0 # stop margin distance when any other stop point is inserted in stop margin [m]
+ hold_stop_margin_distance: 0.0 # the ego keeps stopping if the ego is in this margin [m]
+
+ # params for detection area
+ detection_area:
+ lateral_margin: 0.0 # margin of vehicle footprint [m]
+ step_length: 1.0 # step length for pointcloud search range [m]
+ extend_distance: 0.0 # extend trajectory to consider after goal obstacle in the extend_distance [m]
+
slow_down_planner:
- # slow down planner parameters
- forward_margin: 5.0 # margin distance from slow down point to vehicle front [m]
- backward_margin: 5.0 # margin distance from slow down point to vehicle rear [m]
- expand_slow_down_range: 1.0 # offset from vehicle side edge for expanding the search area of the surrounding point cloud [m]
- max_slow_down_vel: 1.38 # max slow down velocity [m/s]
- min_slow_down_vel: 0.28 # min slow down velocity [m/s]
-
- # slow down constraint parameters
- consider_constraints: False # set "True", if no decel plan found under jerk/dec constrains, relax target slow down vel
- forward_margin_min: 1.0 # min margin for relaxing slow down margin [m/s]
- forward_margin_span: -0.1 # fineness param for relaxing slow down margin [m/s]
- jerk_min_slow_down: -0.3 # min slow down jerk constraint [m/sss]
- jerk_span: -0.01 # fineness param for planning deceleration jerk [m/sss]
- jerk_start: -0.1 # init jerk used for deceleration planning [m/sss]
- slow_down_vel: 1.38 # target slow down velocity [m/s]
+ # params for slow down section
+ slow_down_section:
+ longitudinal_forward_margin: 5.0 # margin distance from slow down point to vehicle front [m]
+ longitudinal_backward_margin: 5.0 # margin distance from slow down point to vehicle rear [m]
+ longitudinal_margin_span: -0.1 # fineness param for relaxing slow down margin (use this param if consider_constraints is True) [m/s]
+ min_longitudinal_forward_margin: 1.0 # min margin for relaxing slow down margin (use this param if consider_constraints is True) [m/s]
+
+ # params for detection area
+ detection_area:
+ lateral_margin: 1.0 # offset from vehicle side edge for expanding the search area of the surrounding point cloud [m]
+
+ # params for velocity
+ target_velocity:
+ max_slow_down_velocity: 1.38 # max slow down velocity (use this param if consider_constraints is False)[m/s]
+ min_slow_down_velocity: 0.28 # min slow down velocity (use this param if consider_constraints is False)[m/s]
+ slow_down_velocity: 1.38 # target slow down velocity (use this param if consider_constraints is True)[m/s]
+
+ # params for deceleration constraints (use this param if consider_constraints is True)
+ constraints:
+ jerk_min_slow_down: -0.3 # min slow down jerk constraint [m/sss]
+ jerk_span: -0.01 # fineness param for planning deceleration jerk [m/sss]
+ jerk_start: -0.1 # init jerk used for deceleration planning [m/sss]
+
+ # others
+ consider_constraints: False # set "True", if no decel plan found under jerk/dec constrains, relax target slow down vel [-]
+ velocity_threshold_decel_complete: 0.2 # use for judge whether the ego velocity converges the target slow down velocity [m/s]
+ acceleration_threshold_decel_complete: 0.1 # use for judge whether the ego velocity converges the target slow down velocity [m/ss]
diff --git a/planning/obstacle_stop_planner/docs/collision_parameters.svg b/planning/obstacle_stop_planner/docs/collision_parameters.svg
new file mode 100644
index 0000000000000..a1f3c346bd305
--- /dev/null
+++ b/planning/obstacle_stop_planner/docs/collision_parameters.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/point_cloud.drawio.svg b/planning/obstacle_stop_planner/docs/point_cloud.drawio.svg
deleted file mode 100644
index b7f2e2c5bcf45..0000000000000
--- a/planning/obstacle_stop_planner/docs/point_cloud.drawio.svg
+++ /dev/null
@@ -1,174 +0,0 @@
-
\ No newline at end of file
diff --git a/planning/obstacle_stop_planner/docs/point_cloud_decel.drawio.svg b/planning/obstacle_stop_planner/docs/point_cloud_decel.drawio.svg
deleted file mode 100644
index e0bd3001607b6..0000000000000
--- a/planning/obstacle_stop_planner/docs/point_cloud_decel.drawio.svg
+++ /dev/null
@@ -1,175 +0,0 @@
-
\ 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..dcfcd62fc3a92
--- /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/slow_down_target.svg b/planning/obstacle_stop_planner/docs/slow_down_target.svg
new file mode 100644
index 0000000000000..203d703936aa6
--- /dev/null
+++ b/planning/obstacle_stop_planner/docs/slow_down_target.svg
@@ -0,0 +1,3 @@
+
+
+
\ No newline at end of file
diff --git a/planning/obstacle_stop_planner/docs/stop_target.svg b/planning/obstacle_stop_planner/docs/stop_target.svg
new file mode 100644
index 0000000000000..838321bfd8196
--- /dev/null
+++ b/planning/obstacle_stop_planner/docs/stop_target.svg
@@ -0,0 +1,3 @@
+
+
+
\ No newline at end of file
diff --git a/planning/obstacle_stop_planner/docs/vehicle_shape.drawio.svg b/planning/obstacle_stop_planner/docs/vehicle_shape.drawio.svg
deleted file mode 100644
index 677e43bed4448..0000000000000
--- a/planning/obstacle_stop_planner/docs/vehicle_shape.drawio.svg
+++ /dev/null
@@ -1,225 +0,0 @@
-
\ No newline at end of file
diff --git a/planning/obstacle_stop_planner/docs/vehicle_shape_decel.drawio.svg b/planning/obstacle_stop_planner/docs/vehicle_shape_decel.drawio.svg
deleted file mode 100644
index 9847751a317e4..0000000000000
--- a/planning/obstacle_stop_planner/docs/vehicle_shape_decel.drawio.svg
+++ /dev/null
@@ -1,376 +0,0 @@
-
\ 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
diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp
index 7d63be90db608..1294e6dccd5a1 100644
--- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp
+++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp
@@ -17,6 +17,7 @@
#include "obstacle_stop_planner/adaptive_cruise_control.hpp"
#include "obstacle_stop_planner/debug_marker.hpp"
+#include "obstacle_stop_planner/planner_data.hpp"
#include
#include
@@ -76,97 +77,11 @@ using tier4_planning_msgs::msg::VelocityLimit;
using tier4_planning_msgs::msg::VelocityLimitClearCommand;
using vehicle_info_util::VehicleInfo;
-struct StopPoint
-{
- TrajectoryPoint point{};
- size_t index;
-};
-
-struct SlowDownSection
-{
- TrajectoryPoint start_point{};
- TrajectoryPoint end_point{};
- size_t slow_down_start_idx;
- size_t slow_down_end_idx;
- double velocity;
-};
-
class ObstacleStopPlannerNode : public rclcpp::Node
{
public:
explicit ObstacleStopPlannerNode(const rclcpp::NodeOptions & node_options);
- struct NodeParam
- {
- bool enable_slow_down; // set True, slow down for obstacle beside the path
- double max_velocity; // max velocity [m/s]
- double lowpass_gain; // smoothing calculated current acceleration [-]
- double hunting_threshold; // keep slow down or stop state if obstacle vanished [s]
- double ego_nearest_dist_threshold; // dist threshold for ego's nearest index
- double ego_nearest_yaw_threshold; // yaw threshold for ego's nearest index
- };
-
- struct StopParam
- {
- double stop_margin; // stop margin distance from obstacle on the path [m]
- double min_behavior_stop_margin; // margin distance, any other stop point is inserted [m]
- double expand_stop_range; // margin of vehicle footprint [m]
- double extend_distance; // trajectory extend_distance [m]
- double step_length; // step length for pointcloud search range [m]
- double stop_search_radius; // search radius for obstacle point cloud [m]
- double hold_stop_margin_distance; // keep stopping if the ego is in this margin [m]
- };
-
- struct SlowDownParam
- {
- double normal_min_jerk; // min jerk limit for mild stop [m/sss]
- double normal_min_acc; // min deceleration limit for mild stop [m/ss]
- double limit_min_jerk; // min jerk limit [m/sss]
- double limit_min_acc; // min deceleration limit [m/ss]
- double forward_margin; // slow down margin(vehicle front -> obstacle) [m]
- double backward_margin; // slow down margin(obstacle vehicle rear) [m]
- double expand_slow_down_range; // lateral range of detection area [m]
- double max_slow_down_vel; // maximum speed in slow down section [m/s]
- double min_slow_down_vel; // minimum velocity in slow down section [m/s]
- bool consider_constraints; // set "True", decel point is planned under jerk/dec constraints
- double slow_down_vel; // target slow down velocity [m/s]
- double forward_margin_min; // min margin for relaxing slow down margin [m/s]
- double forward_margin_span; // fineness param for relaxing slow down margin [m/s]
- double slow_down_min_jerk; // min slow down jerk constraint [m/sss]
- double jerk_start; // init jerk used for deceleration planning [m/sss]
- double jerk_span; // fineness param for planning deceleration jerk [m/sss]
- double vel_threshold_reset_velocity_limit_; // velocity threshold,
- // check complete deceleration [m/s]
- double dec_threshold_reset_velocity_limit_; // acceleration threshold,
- // check complete deceleration [m/ss]
- double slow_down_search_radius; // search radius for slow down obstacle point cloud [m]
- };
-
- struct PlannerData
- {
- diagnostic_msgs::msg::DiagnosticStatus stop_reason_diag{};
-
- geometry_msgs::msg::Pose current_pose{};
-
- pcl::PointXYZ nearest_collision_point;
- pcl::PointXYZ nearest_slow_down_point;
- pcl::PointXYZ lateral_nearest_slow_down_point;
- rclcpp::Time nearest_collision_point_time{};
- double lateral_deviation{0.0};
-
- size_t trajectory_trim_index{};
- size_t decimate_trajectory_collision_index{};
- size_t decimate_trajectory_slow_down_index{};
- std::map decimate_trajectory_index_map{}; // key: decimate index
- // value: original index
-
- bool found_collision_points{false};
- bool found_slow_down_points{false};
- bool stop_require{false};
- bool slow_down_require{false};
- bool enable_adaptive_cruise{false};
- };
-
private:
/*
* ROS
diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_data.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_data.hpp
new file mode 100644
index 0000000000000..11fcdcdf56828
--- /dev/null
+++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_data.hpp
@@ -0,0 +1,247 @@
+// Copyright 2022 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef OBSTACLE_STOP_PLANNER__PLANNER_DATA_HPP_
+#define OBSTACLE_STOP_PLANNER__PLANNER_DATA_HPP_
+
+#include
+
+#include
+
+#include
- 
-
-