fix(goal_planner): fix checkOriginalGoalIsInShoulder (autowarefoundat…

…ion#5836) Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> Signed-off-by: karishma <karishma@interpl.ai>
Interplai · Dec 19, 2023 · db2bbb5 · db2bbb5
1 parent eb71fec
commit db2bbb5
Show file tree

Hide file tree

Showing 3 changed files with 1 addition and 20 deletions.
diff --git a/...ath_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp b/...ath_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, 2> & pose, const std::array<double, 2> 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
@@ -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
@@ -203,7 +203,7 @@ bool isAllowedGoalModification(const std::shared_ptr<RouteHandler> & route_handl
 
 bool checkOriginalGoalIsInShoulder(const std::shared_ptr<RouteHandler> & 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{};