You signed in with another tab or window. Reload to refresh your session.You signed out in another tab or window. Reload to refresh your session.You switched accounts on another tab or window. Reload to refresh your session.Dismiss alert
Copy file name to clipboardExpand all lines: planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/manager.hpp
-2
Original file line number
Diff line number
Diff line change
@@ -47,8 +47,6 @@ class DynamicObstacleAvoidanceModuleManager : public SceneModuleManagerInterface
Copy file name to clipboardExpand all lines: planning/behavior_path_planner/autoware_behavior_path_planner/docs/behavior_path_planner_manager_design.md
+75-29
Original file line number
Diff line number
Diff line change
@@ -46,6 +46,31 @@ Additionally, the manager generates root reference path, and if any other module
The manager owns several containers of sub-managers, namely _slots_, that holds/runs several sub-managers and send the output to the next slot. Given the initial reference path, each slot processes the input path and the ouptut path is processed by the next slot. The final slot output is utilized as the output of the manager. The slot passes following information
52
+
53
+
```cpp
54
+
structSlotOutput
55
+
{
56
+
BehaviorModuleOutput valid_output;
57
+
58
+
// if candidate module is running, valid_output contains the planning by candidate module. In
59
+
// that case, downstream slots will just run aproved modules and do not try to launch new
60
+
// modules
61
+
bool is_upstream_candidate_exclusive{false};
62
+
63
+
// if this slot failed, downstream slots need to refresh approved/candidate modules and just
64
+
// forward valid_output of this slot output
65
+
bool is_upstream_failed_approved{false};
66
+
67
+
// if the approved module in this slot returned to isWaitingApproval, downstream slots need to
@@ -101,7 +125,6 @@ Code is [here](https://github.com/autowarefoundation/autoware.universe/blob/b173
101
125
|`enable_simultaneous_execution_as_candidate_module`| bool | if true, the manager allows its scene modules to run with other scene modules as **candidate module**. |
102
126
|`enable_simultaneous_execution_as_approved_module`| bool | if true, the manager allows its scene modules to run with other scene modules as **approved module**. |
103
127
|`priority`| uint8_t | the manager decides execution priority based on this parameter. The smaller the number is, the higher the priority is. |
104
-
|`max_module_size`| uint8_t | the sub-manager can run some modules simultaneously. this parameter set the maximum number of the launched modules. |
105
128
106
129
### Scene modules
107
130
@@ -138,13 +161,13 @@ There are 6 steps in one process:
138
161
139
162
At first, the manager set latest planner data, and run all approved modules and get output path. At this time, the manager checks module status and removes expired modules from approved modules stack.
Input approved modules output and necessary data to all registered modules, and the modules judge the necessity of path modification based on it. The manager checks which module makes execution request.
Decides the priority order of execution among candidate modules. And, run all candidate modules. Each modules outputs reference path and RTC cooperate status.
@@ -379,20 +402,20 @@ If a module that doesn't support simultaneous execution exists in approved modul
379
402
380
403
For example, if approved module's setting of `enable_simultaneous_execution_as_approved_module` is **ENABLE**, then only modules whose the setting is **ENABLE** proceed to the next step.
|| If approved modules stack is empty, then all request modules proceed to the next step, regardless of the setting of `enable_simultaneous_execution_as_approved_module`. |
389
-
|| If approved module's setting of `enable_simultaneous_execution_as_approved_module` is **DISABLE**, then all request modules are discarded. |
|| If approved modules stack is empty, then all request modules proceed to the next step, regardless of the setting of `enable_simultaneous_execution_as_approved_module`. |
412
+
|| If approved module's setting of `enable_simultaneous_execution_as_approved_module` is **DISABLE**, then all request modules are discarded. |
For example, if the highest priority module's setting of `enable_simultaneous_execution_as_candidate_module` is **DISABLE**, then all modules after the second priority are discarded.
|| If a module with a higher priority exists, lower priority modules whose setting of `enable_simultaneous_execution_as_candidate_module` is **DISABLE** are discarded. |
456
-
|| If all modules' setting of `enable_simultaneous_execution_as_candidate_module` is **ENABLE**, then all modules proceed to the next step. |
|| If a module with a higher priority exists, lower priority modules whose setting of `enable_simultaneous_execution_as_candidate_module` is **DISABLE** are discarded. |
479
+
|| If all modules' setting of `enable_simultaneous_execution_as_candidate_module` is **ENABLE**, then all modules proceed to the next step. |
In this case, the manager selects a candidate modules which output path is used as `behavior_path_planner` output by approval condition in the following rules.
471
494
@@ -484,11 +507,11 @@ In this case, the manager selects a candidate modules which output path is used
484
507
The smaller the number is, the higher the priority is.
If this case happened in the slot, `is_upstream_waiting_approved` is set to true.
508
533
509
534
### Failure modules
510
535
511
536
The failure modules return the status `ModuleStatus::FAILURE`. The manager removes the module from approved modules stack as well as waiting approval modules, but the failure module is not moved to candidate modules stack.
512
537
513
538
As a result, the module A's output is used as approved modules stack.
If this case happened in the slot, `is_upstream_failed_approved` is set to true.
516
543
517
544
### Succeeded modules
518
545
519
546
The succeeded modules return the status `ModuleStatus::SUCCESS`. The manager removes those modules based on **Last In First Out** policy. In other words, if a module added later to approved modules stack is still running (is in `ModuleStatus::RUNNING`), the manager doesn't remove the succeeded module. The reason for this is the same as in removal for waiting approval modules, and is to prevent sudden changes of the running module's output.
As an exception, if **Lane Change** module returns status `ModuleStatus::SUCCESS`, the manager doesn't remove any modules until all modules is in status `ModuleStatus::SUCCESS`. This is because when the manager removes the **Lane Change** (normal LC, external LC, avoidance by LC) module as succeeded module, the manager updates the information of the lane Ego is currently driving in, so root reference path (= module A's input path) changes significantly at that moment.
When the manager removes succeeded modules, the last added module's output is used as approved modules stack.
532
559
560
+
## Slot output propagation
561
+
562
+
As the initial solution, following _SlotOutput_ is passed to the first slot.
563
+
564
+
```cpp
565
+
SlotOutput result_output = SlotOutput{
566
+
getReferencePath(data),
567
+
false,
568
+
false,
569
+
false,
570
+
};
571
+
```
572
+
573
+
If a slot turned out to be `is_upstream_failed_approved`, then all the subsequent slots are refreshed and have all of their approved_modules and candidate_modules cleared. The valid_output is just transferred to the end without any modification.
574
+
575
+
If a slot turned out to be `is_upstream_waiting_approved`, then all the subsequent slots clear their candidate_modules once and apply their approved_modules to obtain the slot output.
576
+
577
+
If a slot turned out to be `is_upstream_candidate_exclusive`, it means that a not `simultaneously_executable_as_candidate` module is running in that slot. Then all the subsequent modules just apply their approved_modules without trying to launch new candidate_modules.
578
+
533
579
## Reference path generation
534
580
535
581
The reference path is generated from the centerline of the **lanelet sequence** obtained from the **current route lanelet**, and it is not only used as an input to the first added module of approved modules stack, but also used as the output of `behavior_path_planner` if none of the modules are running.
@@ -547,7 +593,7 @@ The **current route lanelet** can be reset to the closest lanelet within the rou
The manager needs to know the ego behavior and then generate a root reference path from the lanes that Ego should follow.
596
+
The manager needs to know the ego behavior and then generate a root reference path from the lanes tghat Ego should follow.
551
597
552
598
For example, during autonomous driving, even if Ego moves into the next lane in order to avoid a parked vehicle, the target lanes that Ego should follow will **NOT** change because Ego will return to the original lane after the avoidance maneuver. Therefore, the manager does **NOT** reset the **current route lanelet**, even if the avoidance maneuver is finished.
0 commit comments