From b8ed4663f1cc779fa953d05171c1ec0bd100e5d3 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Mon, 4 Sep 2023 12:50:33 +0900 Subject: [PATCH 1/2] feat(behavior_path_planner): add always executable module option (#4785) * feat(behavior_path_planner): add always executable module option Signed-off-by: kosuke55 * fix conditions Signed-off-by: kosuke55 * rename itr Signed-off-by: kosuke55 * Update planning/behavior_path_planner/src/planner_manager.cpp Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> * Update planning/behavior_path_planner/src/planner_manager.cpp Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> * Update planning/behavior_path_planner/src/planner_manager.cpp Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> * fix candidate break Signed-off-by: kosuke55 * Revert "fix candidate break" This reverts commit 0071bbdd8aa4d510a67f6d9bc32e23716f7aa416. * remove continue Signed-off-by: kosuke55 * check manager_ptr->isAlwaysExecutableModule first Signed-off-by: kosuke55 * common similar process Signed-off-by: kosuke55 * fix getRequestModules Signed-off-by: kosuke55 --------- Signed-off-by: kosuke55 Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> --- .../scene_module/goal_planner/manager.hpp | 2 + .../scene_module_manager_interface.hpp | 19 +++ .../src/planner_manager.cpp | 132 ++++++++++++++---- .../src/scene_module/goal_planner/manager.cpp | 26 +++- 4 files changed, 146 insertions(+), 33 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/manager.hpp index d8c53c69ec8b1..c964582064e8a 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/manager.hpp @@ -41,6 +41,8 @@ class GoalPlannerModuleManager : public SceneModuleManagerInterface void updateModuleParams(const std::vector & parameters) override; + bool isAlwaysExecutableModule() const override; + bool isSimultaneousExecutableAsApprovedModule() const override; bool isSimultaneousExecutableAsCandidateModule() const override; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp index ed57fb4ddee68..2f83977608112 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp @@ -201,13 +201,32 @@ class SceneModuleManagerInterface bool canLaunchNewModule() const { return registered_modules_.size() < max_module_num_; } + /** + * Determine if the module is always executable, regardless of the state of other modules. + * + * When this returns true: + * - The module can be executed even if other modules are not marked as 'simultaneously + * executable'. + * - Conversely, the presence of this module will not prevent other modules + * from executing, even if they are not marked as 'simultaneously executable'. + */ + virtual bool isAlwaysExecutableModule() const { return false; } + virtual bool isSimultaneousExecutableAsApprovedModule() const { + if (isAlwaysExecutableModule()) { + return true; + } + return enable_simultaneous_execution_as_approved_module_; } virtual bool isSimultaneousExecutableAsCandidateModule() const { + if (isAlwaysExecutableModule()) { + return true; + } + return enable_simultaneous_execution_as_candidate_module_; } diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp index 05e8c0e58356f..4d2d97c90aa41 100644 --- a/planning/behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/src/planner_manager.cpp @@ -180,22 +180,6 @@ std::vector PlannerManager::getRequestModules( { std::vector request_modules{}; - /** - * check whether it is possible to push back more modules to approved modules. - */ - { - const auto find_block_module = [this](const auto & m) { - return !getManager(m)->isSimultaneousExecutableAsApprovedModule(); - }; - - const auto itr = - std::find_if(approved_module_ptrs_.begin(), approved_module_ptrs_.end(), find_block_module); - - if (itr != approved_module_ptrs_.end()) { - return {}; - } - } - const auto toc = [this](const auto & name) { processing_time_.at(name) += stop_watch_.toc(name, true); }; @@ -204,14 +188,62 @@ std::vector PlannerManager::getRequestModules( stop_watch_.tic(manager_ptr->getModuleName()); /** - * don't launch candidate module if approved modules already exist. + * determine the execution capability of modules based on existing approved modules. */ - if (!approved_module_ptrs_.empty()) { - if (!manager_ptr->isSimultaneousExecutableAsApprovedModule()) { - toc(manager_ptr->getModuleName()); + // Condition 1: always executable module can be added regardless of the existence of other + // modules, so skip checking the existence of other modules. + // in other cases, need to check the existence of other modules and which module can be added. + const bool has_non_always_executable_module = std::any_of( + approved_module_ptrs_.begin(), approved_module_ptrs_.end(), + [this](const auto & m) { return !getManager(m)->isAlwaysExecutableModule(); }); + if (!manager_ptr->isAlwaysExecutableModule() && has_non_always_executable_module) { + // pairs of find_block_module and is_executable + std::vector, std::function>> + conditions; + + // Condition 2: do not add modules that are neither always nor simultaneous executable + // if there exists at least one approved module that is simultaneous but not always + // executable. (only modules that are either always executable or simultaneous executable can + // be added) + conditions.push_back( + {[&](const SceneModulePtr & m) { + return !getManager(m)->isAlwaysExecutableModule() && + getManager(m)->isSimultaneousExecutableAsApprovedModule(); + }, + [&]() { return manager_ptr->isSimultaneousExecutableAsApprovedModule(); }}); + + // Condition 3: do not add modules that are not always executable if there exists + // at least one approved module that is neither always nor simultaneous executable. + // (only modules that are always executable can be added) + conditions.push_back( + {[&](const SceneModulePtr & m) { + return !getManager(m)->isAlwaysExecutableModule() && + !getManager(m)->isSimultaneousExecutableAsApprovedModule(); + }, + [&]() { return false; }}); + + bool skip_module = false; + for (const auto & condition : conditions) { + const auto & find_block_module = condition.first; + const auto & is_executable = condition.second; + + const auto itr = std::find_if( + approved_module_ptrs_.begin(), approved_module_ptrs_.end(), find_block_module); + + if (itr != approved_module_ptrs_.end() && !is_executable()) { + toc(manager_ptr->getModuleName()); + skip_module = true; + continue; + } + } + if (skip_module) { continue; } } + // else{ + // Condition 4: if none of the above conditions are met, any module can be added. + // (when the approved modules are either empty or consist only of always executable modules.) + // } /** * launch new candidate module. @@ -334,20 +366,58 @@ std::pair PlannerManager::runRequestModule * remove non-executable modules. */ for (const auto & module_ptr : sorted_request_modules) { - if (!getManager(module_ptr)->isSimultaneousExecutableAsCandidateModule()) { - if (executable_modules.empty()) { - executable_modules.push_back(module_ptr); - break; - } + // Condition 1: always executable module can be added regardless of the existence of other + // modules. + if (getManager(module_ptr)->isAlwaysExecutableModule()) { + executable_modules.push_back(module_ptr); + continue; } - const auto itr = - std::find_if(executable_modules.begin(), executable_modules.end(), [this](const auto & m) { - return !getManager(m)->isSimultaneousExecutableAsCandidateModule(); - }); - - if (itr == executable_modules.end()) { + // Condition 4: If the executable modules are either empty or consist only of always executable + // modules, any module can be added. + const bool has_non_always_executable_module = std::any_of( + executable_modules.begin(), executable_modules.end(), + [this](const auto & m) { return !getManager(m)->isAlwaysExecutableModule(); }); + if (!has_non_always_executable_module) { executable_modules.push_back(module_ptr); + continue; + } + + // pairs of find_block_module and is_executable + std::vector, std::function>> + conditions; + + // Condition 3: Only modules that are always executable can be added + // if there exists at least one executable module that is neither always nor simultaneous + // executable. + conditions.push_back( + {[this](const SceneModulePtr & m) { + return !getManager(m)->isAlwaysExecutableModule() && + !getManager(m)->isSimultaneousExecutableAsCandidateModule(); + }, + [&]() { return false; }}); + + // Condition 2: Only modules that are either always executable or simultaneous executable can be + // added if there exists at least one executable module that is simultaneous but not always + // executable. + conditions.push_back( + {[this](const SceneModulePtr & m) { + return !getManager(m)->isAlwaysExecutableModule() && + getManager(m)->isSimultaneousExecutableAsCandidateModule(); + }, + [&]() { return getManager(module_ptr)->isSimultaneousExecutableAsCandidateModule(); }}); + + for (const auto & condition : conditions) { + const auto & find_block_module = condition.first; + const auto & is_executable = condition.second; + + const auto itr = + std::find_if(executable_modules.begin(), executable_modules.end(), find_block_module); + + if (itr != executable_modules.end() && is_executable()) { + executable_modules.push_back(module_ptr); + break; + } } } diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp index cac6f4f940809..d6fbaaf5fbf7a 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp @@ -48,10 +48,26 @@ void GoalPlannerModuleManager::updateModuleParams( }); } -// enable SimultaneousExecutable whenever goal modification is not allowed -// because only minor path refinements are made for fixed goals +bool GoalPlannerModuleManager::isAlwaysExecutableModule() const +{ + // enable AlwaysExecutable whenever goal modification is not allowed + // because only minor path refinements are made for fixed goals + if (!goal_planner_utils::isAllowedGoalModification( + planner_data_->route_handler, left_side_parking_)) { + 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 (!goal_planner_utils::isAllowedGoalModification( planner_data_->route_handler, left_side_parking_)) { return true; @@ -64,6 +80,12 @@ bool GoalPlannerModuleManager::isSimultaneousExecutableAsApprovedModule() const // because only minor path refinements are made for fixed goals 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 (!goal_planner_utils::isAllowedGoalModification( planner_data_->route_handler, left_side_parking_)) { return true; From b69c5d9278c431139725966e328fff8816faf274 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Wed, 6 Sep 2023 02:21:37 +0900 Subject: [PATCH 2/2] feat(goal_planner): do not use minimum_request_length for fixed goal planner (#4831) Signed-off-by: kosuke55 --- .../goal_planner/goal_planner.param.yaml | 2 +- ...havior_path_planner_goal_planner_design.md | 38 ++++++++++--------- .../goal_planner/goal_planner_parameters.hpp | 2 +- .../src/behavior_path_planner_node.cpp | 2 +- .../goal_planner/goal_planner_module.cpp | 20 +++++----- 5 files changed, 34 insertions(+), 30 deletions(-) diff --git a/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml b/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml index 3c5846edf3ee9..6d0936121db6b 100644 --- a/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml +++ b/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml @@ -2,7 +2,6 @@ ros__parameters: goal_planner: # general params - minimum_request_length: 100.0 th_arrived_distance: 1.0 th_stopped_velocity: 0.01 th_stopped_time: 2.0 # It must be greater than the state_machine's. @@ -35,6 +34,7 @@ # pull over pull_over: + minimum_request_length: 100.0 pull_over_velocity: 3.0 pull_over_minimum_velocity: 1.38 decide_path_distance: 10.0 diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md index 008087ee32b4b..fbda16dd466ec 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md @@ -86,8 +86,8 @@ Either one is activated when all conditions are met. ### fixed_goal_planner -- The distance between the goal and ego-vehicle is shorter than `minimum_request_length`. - Route is set with `allow_goal_modification=false` by default. +- ego-vehicle is in the same lane as the goal. @@ -95,7 +95,7 @@ Either one is activated when all conditions are met. #### pull over on road lane -- The distance between the goal and ego-vehicle is shorter than `minimum_request_length`. +- The distance between the goal and ego-vehicle is shorter than `pull_over_minimum_request_length`. - Route is set with `allow_goal_modification=true` . - We can set this option with [SetRoute](https://github.com/autowarefoundation/autoware_adapi_msgs/blob/main/autoware_adapi_v1_msgs/routing/srv/SetRoute.srv#L2) api service. - We support `2D Rough Goal Pose` with the key bind `r` in RViz, but in the future there will be a panel of tools to manipulate various Route API from RViz. @@ -105,7 +105,7 @@ Either one is activated when all conditions are met. #### pull over on shoulder lane -- The distance between the goal and ego-vehicle is shorter than `minimum_request_length`. +- The distance between the goal and ego-vehicle is shorter than `pull_over_minimum_request_length`. - Goal is set in the `road_shoulder`. @@ -118,17 +118,11 @@ Either one is activated when all conditions are met. ## General parameters for goal_planner -| Name | Unit | Type | Description | Default value | -| :------------------------- | :----- | :----- | :-------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| minimum_request_length | [m] | double | when the ego-vehicle approaches the goal by this distance or a safe distance to stop, the module is activated. | 100.0 | -| th_arrived_distance | [m] | double | distance threshold for arrival of path termination | 1.0 | -| th_stopped_velocity | [m/s] | double | velocity threshold for arrival of path termination | 0.01 | -| th_stopped_time | [s] | double | time threshold for arrival of path termination | 2.0 | -| pull_over_velocity | [m/s] | double | decelerate to this speed by the goal search area | 3.0 | -| pull_over_minimum_velocity | [m/s] | double | speed of pull_over after stopping once. this prevents excessive acceleration. | 1.38 | -| margin_from_boundary | [m] | double | distance margin from edge of the shoulder lane | 0.5 | -| decide_path_distance | [m] | double | decide path if it approaches this distance relative to the parking position. after that, no path planning and goal search are performed | 10.0 | -| maximum_deceleration | [m/s2] | double | maximum deceleration. it prevents sudden deceleration when a parking path cannot be found suddenly | 1.0 | +| Name | Unit | Type | Description | Default value | +| :------------------ | :---- | :----- | :------------------------------------------------- | :------------ | +| th_arrived_distance | [m] | double | distance threshold for arrival of path termination | 1.0 | +| th_stopped_velocity | [m/s] | double | velocity threshold for arrival of path termination | 0.01 | +| th_stopped_time | [s] | double | time threshold for arrival of path termination | 2.0 | ## **collision check** @@ -173,12 +167,22 @@ searched for in certain range of the shoulder lane. | longitudinal_margin | [m] | double | margin between ego-vehicle at the goal position and obstacles | 3.0 | | max_lateral_offset | [m] | double | maximum offset of goal search in the lateral direction | 0.5 | | lateral_offset_interval | [m] | double | distance interval of goal search in the lateral direction | 0.25 | -| ignore_distance_from_lane_start | [m] | double | distance from start of pull over lanes for ignoring goal candidates | 15.0 | +| ignore_distance_from_lane_start | [m] | double | distance from start of pull over lanes for ignoring goal candidates | 0.0 | +| ignore_distance_from_lane_start | [m] | double | distance from start of pull over lanes for ignoring goal candidates | 0.0 | +| margin_from_boundary | [m] | double | distance margin from edge of the shoulder lane | 0.5 | -## **Path Generation** +## **Pull Over** There are three path generation methods. -The path is generated with a certain margin (default: `0.5 m`) from left boundary of shoulder lane. +The path is generated with a certain margin (default: `0.5 m`) from the boundary of shoulder lane. + +| Name | Unit | Type | Description | Default value | +| :------------------------------- | :----- | :----- | :-------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| pull_over_minimum_request_length | [m] | double | when the ego-vehicle approaches the goal by this distance or a safe distance to stop, pull over is activated. | 100.0 | +| pull_over_velocity | [m/s] | double | decelerate to this speed by the goal search area | 3.0 | +| pull_over_minimum_velocity | [m/s] | double | speed of pull_over after stopping once. this prevents excessive acceleration. | 1.38 | +| decide_path_distance | [m] | double | decide path if it approaches this distance relative to the parking position. after that, no path planning and goal search are performed | 10.0 | +| maximum_deceleration | [m/s2] | double | maximum deceleration. it prevents sudden deceleration when a parking path cannot be found suddenly | 1.0 | ### **shift parking** diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp index f4a3a2bc5a88b..349bdb7d4ea5d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp @@ -40,7 +40,6 @@ enum class ParkingPolicy { struct GoalPlannerParameters { // general params - double minimum_request_length; double th_arrived_distance; double th_stopped_velocity; double th_stopped_time; @@ -70,6 +69,7 @@ struct GoalPlannerParameters double object_recognition_collision_check_margin; // pull over general params + double pull_over_minimum_request_length; double pull_over_velocity; double pull_over_minimum_velocity; double decide_path_distance; diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 84393848267b9..d11abfe9826c7 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -857,7 +857,6 @@ GoalPlannerParameters BehaviorPathPlannerNode::getGoalPlannerParam() // general params { std::string ns = "goal_planner."; - p.minimum_request_length = declare_parameter(ns + "minimum_request_length"); p.th_stopped_velocity = declare_parameter(ns + "th_stopped_velocity"); p.th_arrived_distance = declare_parameter(ns + "th_arrived_distance"); p.th_stopped_time = declare_parameter(ns + "th_stopped_time"); @@ -915,6 +914,7 @@ GoalPlannerParameters BehaviorPathPlannerNode::getGoalPlannerParam() std::string ns = "goal_planner.pull_over."; p.pull_over_velocity = declare_parameter(ns + "pull_over_velocity"); p.pull_over_minimum_velocity = declare_parameter(ns + "pull_over_minimum_velocity"); + p.pull_over_minimum_request_length = declare_parameter(ns + "minimum_request_length"); p.decide_path_distance = declare_parameter(ns + "decide_path_distance"); p.maximum_deceleration = declare_parameter(ns + "maximum_deceleration"); p.maximum_jerk = declare_parameter(ns + "maximum_jerk"); diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index ad75daef96f58..f39481d146090 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -314,25 +314,25 @@ bool GoalPlannerModule::isExecutionRequested() const return false; } + // if goal modification is not allowed + // 1) goal_pose is in current_lanes, plan path to the original fixed goal + // 2) goal_pose is NOT in current_lanes, do not execute goal_planner + if (!goal_planner_utils::isAllowedGoalModification(route_handler, left_side_parking_)) { + return goal_is_in_current_lanes; + } + // if goal arc coordinates can be calculated, check if goal is in request_length const double self_to_goal_arc_length = utils::getSignedDistance(current_pose, goal_pose, current_lanes); const double request_length = goal_planner_utils::isAllowedGoalModification(route_handler, left_side_parking_) ? calcModuleRequestLength() - : parameters_->minimum_request_length; + : parameters_->pull_over_minimum_request_length; if (self_to_goal_arc_length < 0.0 || self_to_goal_arc_length > request_length) { // if current position is far from goal or behind goal, do not execute goal_planner return false; } - // if goal modification is not allowed - // 1) goal_pose is in current_lanes, plan path to the original fixed goal - // 2) goal_pose is NOT in current_lanes, do not execute goal_planner - if (!goal_planner_utils::isAllowedGoalModification(route_handler, left_side_parking_)) { - return goal_is_in_current_lanes; - } - // if (A) or (B) is met execute pull over // (A) target lane is `road` and same to the current lanes // (B) target lane is `road_shoulder` and neighboring to the current lanes @@ -356,13 +356,13 @@ double GoalPlannerModule::calcModuleRequestLength() const { const auto min_stop_distance = calcFeasibleDecelDistance(0.0); if (!min_stop_distance) { - return parameters_->minimum_request_length; + return parameters_->pull_over_minimum_request_length; } const double minimum_request_length = *min_stop_distance + parameters_->backward_goal_search_length + approximate_pull_over_distance_; - return std::max(minimum_request_length, parameters_->minimum_request_length); + return std::max(minimum_request_length, parameters_->pull_over_minimum_request_length); } Pose GoalPlannerModule::calcRefinedGoal(const Pose & goal_pose) const