Skip to content

Commit

Permalink
feat(behavior_path _planner): divide planner manager modules into dep…
Browse files Browse the repository at this point in the history 
…endent slots (#8117)
  • Loading branch information
@soblin
soblin authored Aug 1, 2024
1 parent 7504092 commit 7135f72
Show file tree
Hide file tree
Showing 35 changed files with 2,097 additions and 1,768 deletions.
2 changes: 0 additions & 2 deletions ...dance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/manager.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,8 +47,6 @@ class DynamicObstacleAvoidanceModuleManager : public SceneModuleManagerInterface

void updateModuleParams(const std::vector<rclcpp::Parameter> & parameters) override;

bool isAlwaysExecutableModule() const override;

private:
std::shared_ptr<DynamicAvoidanceParameters> parameters_;
};
Expand Down
4 changes: 0 additions & 4 deletions ...ior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -287,10 +287,6 @@ void DynamicObstacleAvoidanceModuleManager::updateModuleParams(
});
}

bool DynamicObstacleAvoidanceModuleManager::isAlwaysExecutableModule() const
{
return true;
}
} // namespace autoware::behavior_path_planner

#include <pluginlib/class_list_macros.hpp>
Expand Down
2 changes: 0 additions & 2 deletions ...r_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/manager.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,8 +43,6 @@ class GoalPlannerModuleManager : public SceneModuleManagerInterface

void updateModuleParams(const std::vector<rclcpp::Parameter> & parameters) override;

bool isAlwaysExecutableModule() const override;

bool isSimultaneousExecutableAsApprovedModule() const override;

bool isSimultaneousExecutableAsCandidateModule() const override;
Expand Down
19 changes: 0 additions & 19 deletions planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -827,23 +827,8 @@ void GoalPlannerModuleManager::updateModuleParams(
});
}

bool GoalPlannerModuleManager::isAlwaysExecutableModule() const
{
// enable AlwaysExecutable whenever goal modification is not allowed
// because only minor path refinements are made for fixed goals
if (!utils::isAllowedGoalModification(planner_data_->route_handler)) {
return true;
}

return false;
}

bool GoalPlannerModuleManager::isSimultaneousExecutableAsApprovedModule() const
{
if (isAlwaysExecutableModule()) {
return true;
}

// enable SimultaneousExecutable whenever goal modification is not allowed
// because only minor path refinements are made for fixed goals
if (!utils::isAllowedGoalModification(planner_data_->route_handler)) {
Expand All @@ -855,10 +840,6 @@ bool GoalPlannerModuleManager::isSimultaneousExecutableAsApprovedModule() const

bool GoalPlannerModuleManager::isSimultaneousExecutableAsCandidateModule() const
{
if (isAlwaysExecutableModule()) {
return true;
}

// enable SimultaneousExecutable whenever goal modification is not allowed
// because only minor path refinements are made for fixed goals
if (!utils::isAllowedGoalModification(planner_data_->route_handler)) {
Expand Down
44 changes: 22 additions & 22 deletions ...havior_path_planner/autoware_behavior_path_planner/config/scene_module_manager.param.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,80 +3,80 @@
# NOTE: The smaller the priority number is, the higher the module priority is.
/**:
ros__parameters:
# NOTE: modules which are not set true in the preset is ignored in the slot configuration
slots:
# NOTE: array of array is not supported
- slot1
- slot2
- slot3
- slot4
slot1:
- "start_planner"
slot2:
- "side_shift"
- "avoidance_by_lane_change"
- "static_obstacle_avoidance"
- "lane_change_left"
- "lane_change_right"
- "external_request_lane_change_left"
- "external_request_lane_change_right"
slot3:
- "goal_planner"
slot4:
- "dynamic_obstacle_avoidance"

external_request_lane_change_left:
enable_rtc: false
enable_simultaneous_execution_as_approved_module: false
enable_simultaneous_execution_as_candidate_module: true
keep_last: false
priority: 6

external_request_lane_change_right:
enable_rtc: false
enable_simultaneous_execution_as_approved_module: false
enable_simultaneous_execution_as_candidate_module: true
keep_last: false
priority: 6

lane_change_left:
enable_rtc: false
enable_simultaneous_execution_as_approved_module: true
enable_simultaneous_execution_as_candidate_module: true
keep_last: false
priority: 5

lane_change_right:
enable_rtc: false
enable_simultaneous_execution_as_approved_module: true
enable_simultaneous_execution_as_candidate_module: true
keep_last: false
priority: 5

start_planner:
enable_rtc: false
enable_simultaneous_execution_as_approved_module: false
enable_simultaneous_execution_as_candidate_module: false
keep_last: false
priority: 0

side_shift:
enable_rtc: false
enable_simultaneous_execution_as_approved_module: false
enable_simultaneous_execution_as_candidate_module: false
keep_last: false
priority: 2

goal_planner:
enable_rtc: false
enable_simultaneous_execution_as_approved_module: false
enable_simultaneous_execution_as_candidate_module: false
keep_last: true
priority: 1

static_obstacle_avoidance:
enable_rtc: false
enable_simultaneous_execution_as_approved_module: true
enable_simultaneous_execution_as_candidate_module: false
keep_last: false
priority: 4

avoidance_by_lane_change:
enable_rtc: false
enable_simultaneous_execution_as_approved_module: false
enable_simultaneous_execution_as_candidate_module: false
keep_last: false
priority: 3

dynamic_obstacle_avoidance:
enable_rtc: false
enable_simultaneous_execution_as_approved_module: true
enable_simultaneous_execution_as_candidate_module: true
keep_last: false
priority: 7

sampling_planner:
enable_module: true
enable_rtc: false
enable_simultaneous_execution_as_approved_module: false
enable_simultaneous_execution_as_candidate_module: false
keep_last: true
priority: 16
102 changes: 75 additions & 27 deletions ...ner/autoware_behavior_path_planner/docs/behavior_path_planner_manager_design.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,6 +46,31 @@ Additionally, the manager generates root reference path, and if any other module

![manager_overview](../image/manager/manager_overview.svg)

### Slot

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

```cpp
struct SlotOutput
{
BehaviorModuleOutput valid_output;

// if candidate module is running, valid_output contains the planning by candidate module. In
// that case, downstream slots will just run aproved modules and do not try to launch new
// modules
bool is_upstream_candidate_exclusive{false};

// if this slot failed, downstream slots need to refresh approved/candidate modules and just
// forward valid_output of this slot output
bool is_upstream_failed_approved{false};

// if the approved module in this slot returned to isWaitingApproval, downstream slots need to
// refresh candidate once
bool is_upstream_waiting_approved{false};
};

```

### Sub-managers

The sub-manager's main task is
Expand Down Expand Up @@ -136,13 +161,13 @@ There are 6 steps in one process:

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.

![process_step1](../image/manager/process_step1.svg)
![process_step1](../image/manager/process_step1.drawio.svg)

### Step2

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.

![process_step2](../image/manager/process_step2.svg)
![process_step2](../image/manager/process_step2.drawio.svg)

### Step3

Expand All @@ -152,19 +177,19 @@ Check request module existence.

The manager decides which module to execute as candidate modules from the modules that requested to execute path modification.

![process_step4](../image/manager/process_step4.svg)
![process_step4](../image/manager/process_step4.drawio.svg)

### Step5

Decides the priority order of execution among candidate modules. And, run all candidate modules. Each modules outputs reference path and RTC cooperate status.

![process_step5](../image/manager/process_step5.svg)
![process_step5](../image/manager/process_step5.drawio.svg)

### Step6

Move approved module to approved modules stack from candidate modules stack.

![process_step6](../image/manager/process_step6.svg)
![process_step6](../image/manager/process_step6.drawio.svg)

---

Expand Down Expand Up @@ -377,20 +402,20 @@ If a module that doesn't support simultaneous execution exists in approved modul

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.

![request_step2](../image/manager/request_step2.svg)
![request_step2](../image/manager/request_step2.drawio.svg)

Other examples:

| Process | Description |
| :------------------------------------------------------- | :---------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
| ![request_step2-2](../image/manager/request_step2-2.svg) | 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`. |
| ![request_step2-3](../image/manager/request_step2-3.svg) | If approved module's setting of `enable_simultaneous_execution_as_approved_module` is **DISABLE**, then all request modules are discarded. |
| Process | Description |
| :-------------------------------------------------------------- | :---------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
| ![request_step2-2](../image/manager/request_step2-2.drawio.svg) | 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`. |
| ![request_step2-3](../image/manager/request_step2-3.drawio.svg) | If approved module's setting of `enable_simultaneous_execution_as_approved_module` is **DISABLE**, then all request modules are discarded. |

### Step3

Sort `request_modules` by priority.

![request_step3](../image/manager/request_step3.svg)
![request_step3](../image/manager/request_step3.drawio.svg)

### Step4

Expand Down Expand Up @@ -444,26 +469,26 @@ Executable or not:

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.

![request_step4](../image/manager/request_step4.svg)
![request_step4](../image/manager/request_step4.drawio.svg)

Other examples:

| Process | Description |
| :------------------------------------------------------- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
| ![request_step4-2](../image/manager/request_step4-2.svg) | If a module with a higher priority exists, lower priority modules whose setting of `enable_simultaneous_execution_as_candidate_module` is **DISABLE** are discarded. |
| ![request_step4-3](../image/manager/request_step4-3.svg) | If all modules' setting of `enable_simultaneous_execution_as_candidate_module` is **ENABLE**, then all modules proceed to the next step. |
| Process | Description |
| :-------------------------------------------------------------- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
| ![request_step4-2](../image/manager/request_step4-2.drawio.svg) | If a module with a higher priority exists, lower priority modules whose setting of `enable_simultaneous_execution_as_candidate_module` is **DISABLE** are discarded. |
| ![request_step4-3](../image/manager/request_step4-3.drawio.svg) | If all modules' setting of `enable_simultaneous_execution_as_candidate_module` is **ENABLE**, then all modules proceed to the next step. |

### Step5

Run all candidate modules.

![request_step5](../image/manager/request_step5.svg)
![request_step5](../image/manager/request_step5.drawio.svg)

## How to decide which module's output to use?

Sometimes, multiple candidate modules are running simultaneously.

![multiple_candidates](../image/manager/multiple_candidates.svg)
![multiple_candidates](../image/manager/multiple_candidates.drawio.svg)

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.

Expand All @@ -482,11 +507,11 @@ In this case, the manager selects a candidate modules which output path is used
The smaller the number is, the higher the priority is.

<figure markdown>
![module_select](../image/manager/module_select.svg){width=1000}
![module_select](../image/manager/module_select.drawio.svg){width=1000}
<figcaption>module priority</figcaption>
</figure>

![output_module](../image/manager/output_module.svg)
![output_module](../image/manager/output_module.drawio.svg)

Additionally, the manager moves the highest priority module to approved modules stack if it is already approved.

Expand All @@ -502,32 +527,55 @@ This is because module C is planning output path with the output of module B as

As a result, the module A's output is used as approved modules stack.

![waiting_approve](../image/manager/waiting_approve.svg)
![waiting_approve](../image/manager/waiting_approve.drawio.svg)

If this case happened in the slot, `is_upstream_waiting_approved` is set to true.

### Failure modules

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.

As a result, the module A's output is used as approved modules stack.

![failure_modules](../image/manager/failure_modules.svg)
![failure_modules](../image/manager/failure_modules.drawio.svg)

If this case happened in the slot, `is_upstream_failed_approved` is set to true.

### Succeeded modules

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.

![success_modules_remove](../image/manager/success_modules_remove.svg)
![success_modules_remove](../image/manager/success_modules_remove.drawio.svg)

![success_modules_skip](../image/manager/success_modules_skip.svg)
![success_modules_skip](../image/manager/success_modules_skip.drawio.svg)

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.

![lane_change_remove](../image/manager/lane_change_remove.svg)
![lane_change_remove](../image/manager/lane_change_remove.drawio.svg)

![lane_change_skip](../image/manager/lane_change_skip.svg)
![lane_change_skip](../image/manager/lane_change_skip.drawio.svg)

When the manager removes succeeded modules, the last added module's output is used as approved modules stack.

## Slot output propagation

As the initial solution, following _SlotOutput_ is passed to the first slot.

```cpp
SlotOutput result_output = SlotOutput{
getReferencePath(data),
false,
false,
false,
};
```
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.
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.
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.
## Reference path generation
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.
Expand All @@ -545,7 +593,7 @@ The **current route lanelet** can be reset to the closest lanelet within the rou
![current_route_lanelet](../image/manager/current_route_lanelet.svg)
The manager needs to know the ego behavior and then generate a root reference path from the lanes that Ego should follow.
The manager needs to know the ego behavior and then generate a root reference path from the lanes tghat Ego should follow.
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.
Expand Down
Loading

0 comments on commit 7135f72

Please sign in to comment.