diff --git a/planning/autoware_obstacle_cruise_planner/README.md b/planning/autoware_obstacle_cruise_planner/README.md index 1cb8128110e7c..1cc09ece578c4 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. @@ -242,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/behavior_determination_flowchart.drawio.svg b/planning/autoware_obstacle_cruise_planner/media/behavior_determination_flowchart.drawio.svg new file mode 100644 index 0000000000000..e37e2250ed3f8 --- /dev/null +++ b/planning/autoware_obstacle_cruise_planner/media/behavior_determination_flowchart.drawio.svg @@ -0,0 +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
+
+
+
+ + + + + + + + + + + + + + + + + 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..c6b29be2b1096 --- /dev/null +++ b/planning/autoware_obstacle_cruise_planner/media/cruise_planning_block_diagram.drawio.svg @@ -0,0 +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 +
+
+
+ + + + + + + + + +