refactor(behavior_path_planner): refactor pull_over (autowarefoundati…

…on#1842)

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
Signed-off-by: 1222-takeshi <m.takeshi1995@gmail.com>

planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp

@@ -189,9 +189,7 @@ class PullOverModule : public SceneModuleInterface
 
   PathWithLaneId getReferencePath() const;
   PathWithLaneId generateStopPath() const;
-  lanelet::ConstLanelets getPullOverLanes() const;
-  std::pair<bool, bool> getSafePath(ShiftParkingPath & safe_path) const;
-  Pose getRefinedGoal() const;
+  Pose calcRefinedGoal() const;
   Pose getParkingStartPose() const;
   ParallelParkingParameters getGeometricPullOverParameters() const;
   bool isLongEnoughToParkingStart(
@@ -201,7 +199,7 @@ class PullOverModule : public SceneModuleInterface
   bool isArcPath() const;
   double calcMinimumShiftPathDistance() const;
   double calcDistanceToPathChange() const;
-  bool planShiftPath();
+  bool planShiftPath(const Pose goal_pose);
   bool isStopped();
   bool hasFinishedCurrentPath();
   bool hasFinishedPullOver();

planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/util.hpp

@@ -43,37 +43,25 @@ using geometry_msgs::msg::Twist;
 
 // TODO(sugahara) move to util
 PathWithLaneId combineReferencePath(const PathWithLaneId path1, const PathWithLaneId path2);
-bool isPathInLanelets(
-  const PathWithLaneId & path, const lanelet::ConstLanelets & original_lanelets,
-  const lanelet::ConstLanelets & target_lanelets);
-std::vector<ShiftParkingPath> getShiftParkingPaths(
+
+std::vector<ShiftParkingPath> generateShiftParkingPaths(
   const RouteHandler & route_handler, const lanelet::ConstLanelets & original_lanelets,
   const lanelet::ConstLanelets & target_lanelets, const Pose & pose, const Pose & goal_pose,
-  const Twist & twist, const BehaviorPathPlannerParameters & common_parameter,
+  const BehaviorPathPlannerParameters & common_parameter,
   const behavior_path_planner::PullOverParameters & parameter);
 
 std::vector<ShiftParkingPath> selectValidPaths(
   const std::vector<ShiftParkingPath> & paths, const lanelet::ConstLanelets & current_lanes,
-  const lanelet::ConstLanelets & target_lanes,
-  const lanelet::routing::RoutingGraphContainer & overall_graphs, const Pose & current_pose,
-  const bool isInGoalRouteSection, const Pose & goal_pose,
+  const lanelet::ConstLanelets & target_lanes, const Pose & current_pose,
+  const bool is_in_goal_route_section, const Pose & goal_pose,
   const lane_departure_checker::LaneDepartureChecker & lane_departure_checker);
 bool selectSafePath(
   const std::vector<ShiftParkingPath> & paths,
   const OccupancyGridBasedCollisionDetector & occupancy_grid_map, ShiftParkingPath & selected_path);
-bool isPullOverPathSafe(
-  const PathWithLaneId & path, const lanelet::ConstLanelets & current_lanes,
-  const lanelet::ConstLanelets & target_lanes,
-  const PredictedObjects::ConstSharedPtr dynamic_objects, const Pose & current_pose,
-  const Twist & current_twist, const double vehicle_width,
-  const behavior_path_planner::PullOverParameters & ros_parameters, const bool use_buffer = true,
-  const double acceleration = 0.0);
 bool hasEnoughDistance(
   const ShiftParkingPath & path, const lanelet::ConstLanelets & current_lanes,
-  const lanelet::ConstLanelets & target_lanes, const Pose & current_pose,
-  const bool isInGoalRouteSection, const Pose & goal_pose,
-  const lanelet::routing::RoutingGraphContainer & overall_graphs);
-bool isObjectFront(const Pose & ego_pose, const Pose & obj_pose);
+  const Pose & current_pose, const bool is_in_goal_route_section, const Pose & goal_pose);
+lanelet::ConstLanelets getPullOverLanes(const RouteHandler & route_handler);
 }  // namespace pull_over_utils
 }  // namespace behavior_path_planner
 

planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp

@@ -96,8 +96,6 @@ void getProjectedDistancePointFromPolygons(
   Pose & point_on_object);
 // data conversions
 
-Path convertToPathFromPathWithLaneId(const PathWithLaneId & path_with_lane_id);
-
 std::vector<Point> convertToPointArray(const PathWithLaneId & path);
 
 std::vector<Point> convertToGeometryPointArray(const PathWithLaneId & path);

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -725,20 +725,12 @@ PathWithLaneId BehaviorPathPlannerNode::modifyPathForSmoothGoalConnection(
   const PathWithLaneId & path) const
 {
   const auto goal = planner_data_->route_handler->getGoalPose();
-  const auto is_approved = planner_data_->approval.is_approved.data;
-  auto goal_lane_id = planner_data_->route_handler->getGoalLaneId();
+  const auto goal_lane_id = planner_data_->route_handler->getGoalLaneId();
 
   Pose refined_goal{};
   {
     lanelet::ConstLanelet goal_lanelet;
-    lanelet::ConstLanelet pull_over_lane;
-    geometry_msgs::msg::Pose pull_over_goal;
-    if (
-      is_approved && planner_data_->route_handler->getPullOverTarget(
-                       planner_data_->route_handler->getShoulderLanelets(), &pull_over_lane)) {
-      refined_goal = planner_data_->route_handler->getPullOverGoalPose();
-      goal_lane_id = pull_over_lane.id();
-    } else if (planner_data_->route_handler->getGoalLanelet(&goal_lanelet)) {
+    if (planner_data_->route_handler->getGoalLanelet(&goal_lanelet)) {
       refined_goal = util::refineGoal(goal, goal_lanelet);
     } else {
       refined_goal = goal;