From b667634c73ba1940457f741021ab87c3e22ecd33 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Fri, 25 Oct 2024 10:27:34 +0900 Subject: [PATCH 1/3] add behavior determination flowchart Signed-off-by: mitukou1109 --- planning/autoware_obstacle_cruise_planner/README.md | 4 ++++ .../media/behavior_determination_flowchart.drawio.svg | 4 ++++ 2 files changed, 8 insertions(+) create mode 100644 planning/autoware_obstacle_cruise_planner/media/behavior_determination_flowchart.drawio.svg diff --git a/planning/autoware_obstacle_cruise_planner/README.md b/planning/autoware_obstacle_cruise_planner/README.md index 1cb8128110e7c..2ac43e64c75e5 100644 --- a/planning/autoware_obstacle_cruise_planner/README.md +++ b/planning/autoware_obstacle_cruise_planner/README.md @@ -76,6 +76,10 @@ The obstacles not in front of the ego will be ignored. ![determine_cruise_stop_slow_down](./media/determine_cruise_stop_slow_down.drawio.svg) +The behavior determination flowchart is shown below. + +![behavior_determination_flowchart](./media/behavior_determination_flowchart.drawio.svg) + #### Determine cruise vehicles The obstacles meeting the following condition are determined as obstacles for cruising. diff --git a/planning/autoware_obstacle_cruise_planner/media/behavior_determination_flowchart.drawio.svg b/planning/autoware_obstacle_cruise_planner/media/behavior_determination_flowchart.drawio.svg new file mode 100644 index 0000000000000..009e5b19b2773 --- /dev/null +++ b/planning/autoware_obstacle_cruise_planner/media/behavior_determination_flowchart.drawio.svg @@ -0,0 +1,4 @@ + + + +
Stop obstacle
Slow down
obstacle
Object
(PredictedObject / PointCloud)
Cruise obstacle
Type for
cruising?
Type for
stop?
Should
maintain stop?
Previously
stop obstacle?
Previously
slow down obstacle?
Lateral
distance > C1?
Ego driving
forward?
Moving in
same direction?
Lateral
distance < A1?
Not crossing
ego's trajectory?
Inside ego's
trajectory?
Ego moving?
Lateral
distance < B1?
Type for
outside stop?
Time to approach
ego's trajectory < B2?
Type for
outside cruising?
Type for
inside cruising?
Collision
predicted?
Type for
inside stop?
Longitudinal
velocity > A3?
Longitudinal
velocity > A4?
Previously
cruise obstacle?
Longitudinal
velocity > A5?
Collision
predicted?
Collision
predicted?
Not transient
obstacle?
Type for
slow down?
Lateral
distance < C2?
decrement
count
increment
count
count > -C3?
count > C4?
Collision
predicted?
reset
count
A1: behavior_determination.cruise.max_lat_margin
A2: behavior_determination.cruise.outside_obstacle.max_lateral_time_margin
A3: behavior_determination.obstacle_velocity_threshold_from_cruise_to_stop
A4: behavior_determination.obstacle_velocity_threshold_from_stop_to_cruise
A5: behavior_determination.cruise.outside_obstacle.obstacle_velocity_threshold

B1: behavior_determination.stop.max_lat_margin
B2: behavior_determination.stop.outside_obstacle.max_lateral_time_margin
B3: behavior_determination.stop.crossing_obstacle.collision_time_margin

C1: behavior_determination.slow_down.max_lat_margin
   + behavior_determination.slow_down.lat_hysteresis_margin
C2: behavior_determination.slow_down.max_lat_margin
   - behavior_determination.slow_down.lat_hysteresis_margin
C3: behavior_determination.slow_down.successive_num_to_exit_slow_down_condition
C4: behavior_determination.slow_down.successive_num_to_entry_slow_down_condition

D1: behavior_determination.obstacle_velocity_threshold_from_stop_to_cruise
D2: behavior_determination.stop_obstacle_hold_time_threshold
Time to approach
ego's trajectory < A2?
Should maintain stop?

Transient obstacle?

Collision predicted?
Obstacle velocity < D1 AND elapsed time since last stop decision < D2

Time to reach collision point > B3 OR collision not predicted at the time of reaching collision point

At least 1 vehicle footprint polygon at each point on trajectory intersects object footprint polygon
 \ No newline at end of file From 0c6890950d9fd56b430fd370c2502e4f65b734a4 Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Fri, 25 Oct 2024 10:27:47 +0900 Subject: [PATCH 2/3] add cruise planning block diagram Signed-off-by: mitukou1109 --- planning/autoware_obstacle_cruise_planner/README.md | 4 ++++ .../media/cruise_planning_block_diagram.drawio.svg | 4 ++++ 2 files changed, 8 insertions(+) create mode 100644 planning/autoware_obstacle_cruise_planner/media/cruise_planning_block_diagram.drawio.svg diff --git a/planning/autoware_obstacle_cruise_planner/README.md b/planning/autoware_obstacle_cruise_planner/README.md index 2ac43e64c75e5..1cc09ece578c4 100644 --- a/planning/autoware_obstacle_cruise_planner/README.md +++ b/planning/autoware_obstacle_cruise_planner/README.md @@ -246,6 +246,10 @@ $$ | `lpf(val)` | apply low-pass filter to `val` | | `pid(val)` | apply pid to `val` | +#### Block diagram + +![cruise_planning_block_diagram](./media/cruise_planning_block_diagram.drawio.svg) + ### Slow down planning | Parameter | Type | Description | diff --git a/planning/autoware_obstacle_cruise_planner/media/cruise_planning_block_diagram.drawio.svg b/planning/autoware_obstacle_cruise_planner/media/cruise_planning_block_diagram.drawio.svg new file mode 100644 index 0000000000000..e83e1a1d18d44 --- /dev/null +++ b/planning/autoware_obstacle_cruise_planner/media/cruise_planning_block_diagram.drawio.svg @@ -0,0 +1,4 @@ + + + +
Obstacle velocity
Obstacle acceleration
Ego velocity / acceleration
Actual distance to obstacle
Normalize
&
LPF
PID
&
suppress output
when accelerating
Ideal distance to obstacle
Distance error
Normalized distance error
Target acceleration
RSS
Actual distance to obstacle
+
-
LPF
Square
(preserve sign)
Squared distance error
Trajectory
 \ No newline at end of file From 3aa5e643579552c7f479085804a73fac18bb7d98 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 25 Oct 2024 01:38:18 +0000 Subject: [PATCH 3/3] style(pre-commit): autofix --- ...ehavior_determination_flowchart.drawio.svg | 1959 ++++++++++++++++- .../cruise_planning_block_diagram.drawio.svg | 987 ++++++++- 2 files changed, 2944 insertions(+), 2 deletions(-) diff --git a/planning/autoware_obstacle_cruise_planner/media/behavior_determination_flowchart.drawio.svg b/planning/autoware_obstacle_cruise_planner/media/behavior_determination_flowchart.drawio.svg index 009e5b19b2773..e37e2250ed3f8 100644 --- a/planning/autoware_obstacle_cruise_planner/media/behavior_determination_flowchart.drawio.svg +++ b/planning/autoware_obstacle_cruise_planner/media/behavior_determination_flowchart.drawio.svg @@ -1,4 +1,1961 @@ -
Stop obstacle
Slow down
obstacle
Object
(PredictedObject / PointCloud)
Cruise obstacle
Type for
cruising?
Type for
stop?
Should
maintain stop?
Previously
stop obstacle?
Previously
slow down obstacle?
Lateral
distance > C1?
Ego driving
forward?
Moving in
same direction?
Lateral
distance < A1?
Not crossing
ego's trajectory?
Inside ego's
trajectory?
Ego moving?
Lateral
distance < B1?
Type for
outside stop?
Time to approach
ego's trajectory < B2?
Type for
outside cruising?
Type for
inside cruising?
Collision
predicted?
Type for
inside stop?
Longitudinal
velocity > A3?
Longitudinal
velocity > A4?
Previously
cruise obstacle?
Longitudinal
velocity > A5?
Collision
predicted?
Collision
predicted?
Not transient
obstacle?
Type for
slow down?
Lateral
distance < C2?
decrement
count
increment
count
count > -C3?
count > C4?
Collision
predicted?
reset
count
A1: behavior_determination.cruise.max_lat_margin
A2: behavior_determination.cruise.outside_obstacle.max_lateral_time_margin
A3: behavior_determination.obstacle_velocity_threshold_from_cruise_to_stop
A4: behavior_determination.obstacle_velocity_threshold_from_stop_to_cruise
A5: behavior_determination.cruise.outside_obstacle.obstacle_velocity_threshold

B1: behavior_determination.stop.max_lat_margin
B2: behavior_determination.stop.outside_obstacle.max_lateral_time_margin
B3: behavior_determination.stop.crossing_obstacle.collision_time_margin

C1: behavior_determination.slow_down.max_lat_margin
   + behavior_determination.slow_down.lat_hysteresis_margin
C2: behavior_determination.slow_down.max_lat_margin
   - behavior_determination.slow_down.lat_hysteresis_margin
C3: behavior_determination.slow_down.successive_num_to_exit_slow_down_condition
C4: behavior_determination.slow_down.successive_num_to_entry_slow_down_condition

D1: behavior_determination.obstacle_velocity_threshold_from_stop_to_cruise
D2: behavior_determination.stop_obstacle_hold_time_threshold
Time to approach
ego's trajectory < A2?
Should maintain stop?

Transient obstacle?

Collision predicted?
Obstacle velocity < D1 AND elapsed time since last stop decision < D2

Time to reach collision point > B3 OR collision not predicted at the time of reaching collision point

At least 1 vehicle footprint polygon at each point on trajectory intersects object footprint polygon
 \ No newline at end of file + + + + + + + + + + + + + + +
+
+
+ + Stop obstacle + +
+
+
+ + + + + + + + + + + + + + +
+
+
+ + Slow down + +
+ + obstacle + +
+
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
+ + Object + +
+ (PredictedObject / PointCloud) +
+
+
+
+ + + + + + + + + + + + + + +
+
+
+ + Cruise obstacle + +
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
+ Type + for +
+ cruising? +
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Type + for +
+ stop? +
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Should +
maintain stop?
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Previously +
stop obstacle?
+
+
+
+ + + + + + + + + + + + + + +
+
+
+ Previously +
slow down obstacle?
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Lateral +
distance > C1?
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Ego + driving +
+ forward? +
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Moving in +
+ same + direction? +
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Lateral +
+ distance + < A1? +
+
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
+ Not + crossing +
ego's trajectory?
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Inside ego's +
trajectory?
+
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
+ Ego moving? +
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
+ Lateral +
+ distance + < B1? +
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Type for +
outside stop?
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Time to approach +
+ ego's + trajectory < B2? +
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Type for +
outside cruising?
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Type for +
inside cruising?
+
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
+
Collision
+
predicted?
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Type for +
inside stop?
+
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
+ Longitudinal +
velocity > A3?
+
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
+ Longitudinal +
velocity > A4?
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Previously +
cruise obstacle?
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Longitudinal +
velocity > A5?
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+
Collision
+
predicted?
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+
Collision
+
predicted?
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+
+ Not + transient +
+
obstacle?
+
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
+ Type + for +
+ slow down? +
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Lateral +
distance < C2?
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ decrement +
count
+
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
+ increment +
count
+
+
+
+ + + + + + + + + + + + + + +
+
+
count > -C3?
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
count > C4?
+
+
+ + + + + + + + + + + + + + +
+
+
+
Collision
+
predicted?
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+
reset
+
count
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + A1: + + + behavior_determination.cruise.max_lat_margin +
+ +
+ A2: + + behavior_determination.cruise.outside_obstacle.max_lateral_time_margin +
+ +
+
+ A3: + + behavior_determination.obstacle_velocity_threshold_from_cruise_to_stop +
+ +
+
+ A4: + + behavior_determination.obstacle_velocity_threshold_from_stop_to_cruise +
+ +
+
+ A5: + + behavior_determination.cruise.outside_obstacle.obstacle_velocity_threshold +
+ +
+
+ +
+ +
+
+ B1: + + behavior_determination.stop.max_lat_margin +
+ +
+
+ B2: + + behavior_determination.stop.outside_obstacle.max_lateral_time_margin +
+ +
+
+ B3: + + behavior_determination.stop.crossing_obstacle.collision_time_margin +
+ +
+
+ +
+ +
+
+ C1: + behavior_determination.slow_down.max_lat_margin +
+
+ + + behavior_determination.slow_down.lat_hysteresis_margin + +
+ +
+
+ C2: + behavior_determination.slow_down.max_lat_margin +
+
+ - + behavior_determination.slow_down.lat_hysteresis_margin + +
+ +
+
+ C3: + + behavior_determination.slow_down.successive_num_to_exit_slow_down_condition +
+ +
+
+ C4: + + behavior_determination.slow_down.successive_num_to_entry_slow_down_condition +
+ +
+
+ +
+ +
+
+ D1: + + behavior_determination.obstacle_velocity_threshold_from_stop_to_cruise +
+ +
+
+ D2: + + behavior_determination.stop_obstacle_hold_time_threshold +
+ +
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Time to approach +
+ ego's + trajectory < A2? +
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Should maintain stop? +
+
+
+ Transient obstacle? +
+
+
+
Collision predicted?
+
+
+
+ + + + + + + + + + + + + + +
+
+
Obstacle velocity < D1 AND elapsed time since last stop decision < D2
+
+
Time to reach collision point > B3 OR collision not predicted at the time of reaching collision point
+
+
+
+
At least 1 vehicle footprint polygon at each point on trajectory intersects object footprint polygon
+
+
+
+ + + + + + + + + + + + + + + + + diff --git a/planning/autoware_obstacle_cruise_planner/media/cruise_planning_block_diagram.drawio.svg b/planning/autoware_obstacle_cruise_planner/media/cruise_planning_block_diagram.drawio.svg index e83e1a1d18d44..c6b29be2b1096 100644 --- a/planning/autoware_obstacle_cruise_planner/media/cruise_planning_block_diagram.drawio.svg +++ b/planning/autoware_obstacle_cruise_planner/media/cruise_planning_block_diagram.drawio.svg @@ -1,4 +1,989 @@ -
Obstacle velocity
Obstacle acceleration
Ego velocity / acceleration
Actual distance to obstacle
Normalize
&
LPF
PID
&
suppress output
when accelerating
Ideal distance to obstacle
Distance error
Normalized distance error
Target acceleration
RSS
Actual distance to obstacle
+
-
LPF
Square
(preserve sign)
Squared distance error
Trajectory
 \ No newline at end of file + + + + + + + + + + + + + + + + +
+
+
+ Obstacle velocity +
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
+ Obstacle acceleration +
+
+
+ + + + + + + + + + + + + + +
+
+
+ Ego velocity / acceleration +
+
+
+ + + + + + + + + + + + + + +
+
+
+ Actual distance to obstacle +
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
+
Normalize
+
&
+ LPF +
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
+ PID +
&
+
suppress output
+
when accelerating
+
+
+
+ + + + + + + + + + + + + + +
+
+
+ Ideal distance to obstacle +
+
+
+ + + + + + + + + + + + + + +
+
+
+ Distance error +
+
+
+ + + + + + + + + + + + + + +
+
+
+ Normalized distance error +
+
+
+ + + + + + + + + + + + + + +
+
+
+ Target acceleration +
+
+
+ + + + + + + + + + + + + + + + +
+
+
+
RSS
+
+
+
+ + + + + + + + + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Actual distance to obstacle +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + +
+
+
+ + + + + + + + + + + + + + +
+
+
+ - +
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
+ LPF +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Square +
(preserve sign)
+
+
+
+ + + + + + + + + + + + + + +
+
+
+ Squared distance error +
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
+ Trajectory +
+
+
+ + + + + + + + + +