From 0ad254113ba5c146f11e08b551e9dd9dd4f756ab Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Sun, 3 Sep 2023 17:02:10 +0900 Subject: [PATCH] ractor(goal_planner): refactor isAllowedGoalModification Signed-off-by: kosuke55 --- .../scene_module/goal_planner/manager.hpp | 2 -- .../utils/goal_planner/util.hpp | 6 ++--- .../goal_planner/goal_planner_module.cpp | 24 +++++++------------ .../src/scene_module/goal_planner/manager.cpp | 8 ++----- .../src/utils/goal_planner/util.cpp | 19 ++++++--------- 5 files changed, 20 insertions(+), 39 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 47ad47aecb9b3..4a7eecfc68caf 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 @@ -48,8 +48,6 @@ class GoalPlannerModuleManager : public SceneModuleManagerInterface private: std::shared_ptr parameters_; - - bool left_side_parking_; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp index 3dde96e813aad..29a4a8efa719f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp @@ -51,10 +51,8 @@ PredictedObjects filterObjectsByLateralDistance( const Pose & ego_pose, const double vehicle_width, const PredictedObjects & objects, const double distance_thresh, const bool filter_inside); -bool isAllowedGoalModification( - const std::shared_ptr & route_handler, const bool left_side_parking); -bool checkOriginalGoalIsInShoulder( - const std::shared_ptr & route_handler, const bool left_side_parking); +bool isAllowedGoalModification(const std::shared_ptr & route_handler); +bool checkOriginalGoalIsInShoulder(const std::shared_ptr & route_handler); // debug MarkerArray createPullOverAreaMarkerArray( 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 c7da1a75f3df2..9e7b101b8c4ab 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 @@ -309,10 +309,9 @@ bool GoalPlannerModule::isExecutionRequested() const // 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; + const double request_length = goal_planner_utils::isAllowedGoalModification(route_handler) + ? calcModuleRequestLength() + : parameters_->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; @@ -321,7 +320,7 @@ bool GoalPlannerModule::isExecutionRequested() const // 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_)) { + if (!goal_planner_utils::isAllowedGoalModification(route_handler)) { return goal_is_in_current_lanes; } @@ -409,8 +408,7 @@ ModuleStatus GoalPlannerModule::updateState() { // finish module only when the goal is fixed if ( - !goal_planner_utils::isAllowedGoalModification( - planner_data_->route_handler, left_side_parking_) && + !goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler) && hasFinishedGoalPlanner()) { return ModuleStatus::SUCCESS; } @@ -508,7 +506,7 @@ void GoalPlannerModule::generateGoalCandidates() // calculate goal candidates const Pose goal_pose = route_handler->getGoalPose(); refined_goal_pose_ = calcRefinedGoal(goal_pose); - if (goal_planner_utils::isAllowedGoalModification(route_handler, left_side_parking_)) { + if (goal_planner_utils::isAllowedGoalModification(route_handler)) { goal_searcher_->setPlannerData(planner_data_); goal_candidates_ = goal_searcher_->search(refined_goal_pose_); } else { @@ -528,8 +526,7 @@ BehaviorModuleOutput GoalPlannerModule::plan() path_reference_ = getPreviousModuleOutput().reference_path; - if (goal_planner_utils::isAllowedGoalModification( - planner_data_->route_handler, left_side_parking_)) { + if (goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { return planWithGoalModification(); } else { fixed_goal_planner_->setPreviousModuleOutput(getPreviousModuleOutput()); @@ -853,8 +850,7 @@ BehaviorModuleOutput GoalPlannerModule::planWaitingApproval() path_reference_ = getPreviousModuleOutput().reference_path; - if (goal_planner_utils::isAllowedGoalModification( - planner_data_->route_handler, left_side_parking_)) { + if (goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { return planWaitingApprovalWithGoalModification(); } else { fixed_goal_planner_->setPreviousModuleOutput(getPreviousModuleOutput()); @@ -1457,9 +1453,7 @@ void GoalPlannerModule::setDebugData() } tier4_autoware_utils::appendMarkerArray(added, &debug_marker_); }; - - if (goal_planner_utils::isAllowedGoalModification( - planner_data_->route_handler, left_side_parking_)) { + if (goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { // Visualize pull over areas const auto color = status_.has_decided_path ? createMarkerColor(1.0, 1.0, 0.0, 0.999) // yellow : createMarkerColor(0.0, 1.0, 0.0, 0.999); // green 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 bcee6aeaf0c36..a891586877e67 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 @@ -241,8 +241,6 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( } parameters_ = std::make_shared(p); - - left_side_parking_ = parameters_->parking_policy == ParkingPolicy::LEFT_SIDE; } void GoalPlannerModuleManager::updateModuleParams( @@ -279,8 +277,7 @@ bool GoalPlannerModuleManager::isSimultaneousExecutableAsApprovedModule() const // 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_)) { + if (!goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { return true; } @@ -295,8 +292,7 @@ bool GoalPlannerModuleManager::isSimultaneousExecutableAsCandidateModule() const // 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_)) { + if (!goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { return true; } diff --git a/planning/behavior_path_planner/src/utils/goal_planner/util.cpp b/planning/behavior_path_planner/src/utils/goal_planner/util.cpp index 19b6bd27ea07c..01fc4b182e211 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/util.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/util.cpp @@ -181,25 +181,20 @@ MarkerArray createGoalCandidatesMarkerArray( return marker_array; } -bool isAllowedGoalModification( - const std::shared_ptr & route_handler, const bool left_side_parking) +bool isAllowedGoalModification(const std::shared_ptr & route_handler) { - return route_handler->isAllowedGoalModification() || - checkOriginalGoalIsInShoulder(route_handler, left_side_parking); + return route_handler->isAllowedGoalModification() || checkOriginalGoalIsInShoulder(route_handler); } -bool checkOriginalGoalIsInShoulder( - const std::shared_ptr & route_handler, const bool left_side_parking) +bool checkOriginalGoalIsInShoulder(const std::shared_ptr & route_handler) { const Pose & goal_pose = route_handler->getGoalPose(); + const auto shoulder_lanes = route_handler->getShoulderLanelets(); - const lanelet::ConstLanelets pull_over_lanes = - goal_planner_utils::getPullOverLanes(*(route_handler), left_side_parking); - lanelet::ConstLanelet target_lane{}; - lanelet::utils::query::getClosestLanelet(pull_over_lanes, goal_pose, &target_lane); + lanelet::ConstLanelet closest_shoulder_lane{}; + lanelet::utils::query::getClosestLanelet(shoulder_lanes, goal_pose, &closest_shoulder_lane); - return route_handler->isShoulderLanelet(target_lane) && - lanelet::utils::isInLanelet(goal_pose, target_lane, 0.1); + return lanelet::utils::isInLanelet(goal_pose, closest_shoulder_lane, 0.1); } } // namespace goal_planner_utils