fix

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
autowarefoundation · Sep 4, 2023 · afb9001 · afb9001
1 parent 55e15f0
commit afb9001
Showing 1 changed file with 4 additions and 2 deletions.
diff --git a/planning/behavior_path_planner/src/utils/goal_planner/util.cpp b/planning/behavior_path_planner/src/utils/goal_planner/util.cpp
@@ -192,9 +192,11 @@ bool checkOriginalGoalIsInShoulder(const std::shared_ptr<RouteHandler> & route_h
   const auto shoulder_lanes = route_handler->getShoulderLanelets();
 
   lanelet::ConstLanelet closest_shoulder_lane{};
-  lanelet::utils::query::getClosestLanelet(shoulder_lanes, goal_pose, &closest_shoulder_lane);
+  if (lanelet::utils::query::getClosestLanelet(shoulder_lanes, goal_pose, &closest_shoulder_lane)) {
+    return lanelet::utils::isInLanelet(goal_pose, closest_shoulder_lane, 0.1);
+  }
 
-  return lanelet::utils::isInLanelet(goal_pose, closest_shoulder_lane, 0.1);
+  return false;
 }
 
 }  // namespace goal_planner_utils