From db2bbb56ec82c6d48ac897d5055d4c98c5651053 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 12 Dec 2023 09:57:56 +0900 Subject: [PATCH] fix(goal_planner): fix checkOriginalGoalIsInShoulder (#5836) Signed-off-by: kosuke55 Signed-off-by: karishma --- .../goal_planner_module.hpp | 4 ---- .../src/goal_planner_module.cpp | 15 --------------- .../src/util.cpp | 2 +- 3 files changed, 1 insertion(+), 20 deletions(-) diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp index 7b5b9b1a68102..8100ed7c66128 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp @@ -476,10 +476,6 @@ class GoalPlannerModule : public SceneModuleInterface void onTimer(); void onFreespaceParkingTimer(); - // flag for the interface which do not support `allow_goal_modification` - // when the goal is in `road_shoulder`, always allow goal modification. - bool checkOriginalGoalIsInShoulder() const; - // steering factor void updateSteeringFactor( const std::array & pose, const std::array distance, const uint16_t type); diff --git a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp index 98f5664153bfd..6f1b1e7f41571 100644 --- a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -2036,21 +2036,6 @@ void GoalPlannerModule::printParkingPositionError() const distance_from_real_shoulder); } -bool GoalPlannerModule::checkOriginalGoalIsInShoulder() const -{ - const auto & route_handler = planner_data_->route_handler; - const Pose goal_pose = route_handler->getOriginalGoalPose(); - - const lanelet::ConstLanelets pull_over_lanes = goal_planner_utils::getPullOverLanes( - *(route_handler), left_side_parking_, parameters_->backward_goal_search_length, - parameters_->forward_goal_search_length); - lanelet::ConstLanelet target_lane{}; - lanelet::utils::query::getClosestLanelet(pull_over_lanes, goal_pose, &target_lane); - - return route_handler->isShoulderLanelet(target_lane) && - lanelet::utils::isInLanelet(goal_pose, target_lane, 0.1); -} - bool GoalPlannerModule::needPathUpdate(const double path_update_duration) const { return !isOnModifiedGoal() && hasEnoughTimePassedSincePathUpdate(path_update_duration); diff --git a/planning/behavior_path_goal_planner_module/src/util.cpp b/planning/behavior_path_goal_planner_module/src/util.cpp index a01f5680dcc13..23e9c53efae7b 100644 --- a/planning/behavior_path_goal_planner_module/src/util.cpp +++ b/planning/behavior_path_goal_planner_module/src/util.cpp @@ -203,7 +203,7 @@ bool isAllowedGoalModification(const std::shared_ptr & route_handl bool checkOriginalGoalIsInShoulder(const std::shared_ptr & route_handler) { - const Pose & goal_pose = route_handler->getGoalPose(); + const Pose & goal_pose = route_handler->getOriginalGoalPose(); const auto shoulder_lanes = route_handler->getShoulderLanelets(); lanelet::ConstLanelet closest_shoulder_lane{};