refactor(start planner): refactor for clarity, readability and maybe performance (#6655)

* refactor plane method

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

* delete unused method

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

* skip unnecessary loop

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

---------

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

Author: danielsanchezaran

planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp

@@ -297,7 +297,6 @@ class StartPlannerModule : public SceneModuleInterface
     const std::vector<PoseWithVelocityStamped> & ego_predicted_path) const;
   bool isSafePath() const;
   void setDrivableAreaInfo(BehaviorModuleOutput & output) const;
-  lanelet::ConstLanelets createDepartureCheckLanes() const;
 
   // check if the goal is located behind the ego in the same route segment.
   bool isGoalBehindOfEgoInSameRouteSegment() const;

planning/behavior_path_start_planner_module/src/start_planner_module.cpp

@@ -508,47 +508,45 @@ BehaviorModuleOutput StartPlannerModule::plan()
     return output;
   }
 
-  PathWithLaneId path;
+  const auto path = std::invoke([&]() {
+    if (!status_.driving_forward && !status_.backward_driving_complete) {
+      return status_.backward_path;
+    }
 
-  // Check if backward motion is finished
-  if (status_.driving_forward || status_.backward_driving_complete) {
     // Increment path index if the current path is finished
     if (hasFinishedCurrentPath()) {
       RCLCPP_INFO(getLogger(), "Increment path index");
       incrementPathIndex();
     }
 
-    if (!status_.is_safe_dynamic_objects && !isWaitingApproval() && !status_.stop_pose) {
+    if (isWaitingApproval()) return getCurrentPath();
+
+    if (status_.stop_pose) {
+      // Delete stop point if conditions are met
+      if (status_.is_safe_dynamic_objects && isStopped()) {
+        status_.stop_pose = std::nullopt;
+      }
+      stop_pose_ = status_.stop_pose;
+      return *status_.prev_stop_path_after_approval;
+    }
+
+    if (!status_.is_safe_dynamic_objects) {
       auto current_path = getCurrentPath();
       const auto stop_path =
         behavior_path_planner::utils::parking_departure::generateFeasibleStopPath(
           current_path, planner_data_, stop_pose_, parameters_->maximum_deceleration_for_stop,
           parameters_->maximum_jerk_for_stop);
 
+      if (!stop_path.has_value()) return current_path;
       // Insert stop point in the path if needed
-      if (stop_path) {
-        RCLCPP_ERROR_THROTTLE(
-          getLogger(), *clock_, 5000, "Insert stop point in the path because of dynamic objects");
-        path = *stop_path;
-        status_.prev_stop_path_after_approval = std::make_shared<PathWithLaneId>(path);
-        status_.stop_pose = stop_pose_;
-      } else {
-        path = current_path;
-      }
-    } else if (!isWaitingApproval() && status_.stop_pose) {
-      // Delete stop point if conditions are met
-      if (status_.is_safe_dynamic_objects && isStopped()) {
-        status_.stop_pose = std::nullopt;
-        path = getCurrentPath();
-      }
-      path = *status_.prev_stop_path_after_approval;
-      stop_pose_ = status_.stop_pose;
-    } else {
-      path = getCurrentPath();
+      RCLCPP_ERROR_THROTTLE(
+        getLogger(), *clock_, 5000, "Insert stop point in the path because of dynamic objects");
+      status_.prev_stop_path_after_approval = std::make_shared<PathWithLaneId>(stop_path.value());
+      status_.stop_pose = stop_pose_;
+      return stop_path.value();
     }
-  } else {
-    path = status_.backward_path;
-  }
+    return getCurrentPath();
+  });
 
   output.path = path;
   output.reference_path = getPreviousModuleOutput().reference_path;
@@ -768,8 +766,9 @@ bool StartPlannerModule::findPullOutPath(
   const Pose & refined_start_pose, const Pose & goal_pose, const double collision_check_margin)
 {
   // if start_pose_candidate is far from refined_start_pose, backward driving is necessary
+  constexpr double epsilon = 0.01;
   const bool backward_is_unnecessary =
-    tier4_autoware_utils::calcDistance2d(start_pose_candidate, refined_start_pose) < 0.01;
+    tier4_autoware_utils::calcDistance2d(start_pose_candidate, refined_start_pose) < epsilon;
 
   planner->setCollisionCheckMargin(collision_check_margin);
   planner->setPlannerData(planner_data_);
@@ -856,26 +855,24 @@ lanelet::ConstLanelets StartPlannerModule::getPathRoadLanes(const PathWithLaneId
   const auto & lanelet_layer = route_handler->getLaneletMapPtr()->laneletLayer;
 
   std::vector<lanelet::Id> lane_ids;
+  lanelet::ConstLanelets path_lanes;
+
   for (const auto & p : path.points) {
     for (const auto & id : p.lane_ids) {
       if (id == lanelet::InvalId) {
         continue;
       }
-      if (route_handler->isShoulderLanelet(lanelet_layer.get(id))) {
+      const auto lanelet = lanelet_layer.get(id);
+      if (route_handler->isShoulderLanelet(lanelet)) {
         continue;
       }
       if (std::find(lane_ids.begin(), lane_ids.end(), id) == lane_ids.end()) {
         lane_ids.push_back(id);
+        path_lanes.push_back(lanelet);
       }
     }
   }
 
-  lanelet::ConstLanelets path_lanes;
-  path_lanes.reserve(lane_ids.size());
-  for (const auto & id : lane_ids) {
-    path_lanes.push_back(lanelet_layer.get(id));
-  }
-
   return path_lanes;
 }
 
@@ -954,11 +951,12 @@ void StartPlannerModule::updatePullOutStatus()
 
   if (hasFinishedBackwardDriving()) {
     updateStatusAfterBackwardDriving();
-  } else {
-    status_.backward_path = start_planner_utils::getBackwardPath(
-      *route_handler, pull_out_lanes, current_pose, status_.pull_out_start_pose,
-      parameters_->backward_velocity);
+    return;
   }
+  status_.backward_path = start_planner_utils::getBackwardPath(
+    *route_handler, pull_out_lanes, current_pose, status_.pull_out_start_pose,
+    parameters_->backward_velocity);
+  return;
 }
 
 void StartPlannerModule::updateStatusAfterBackwardDriving()
@@ -1047,7 +1045,7 @@ std::vector<Pose> StartPlannerModule::searchPullOutStartPoseCandidates(
     if (distance_from_lane_end < parameters_->ignore_distance_from_lane_end) {
       RCLCPP_WARN_THROTTLE(
         getLogger(), *clock_, 5000,
-        "the ego is too close to the lane end, so needs backward driving");
+        "the ego vehicle is too close to the lane end, so backwards driving is necessary");
       continue;
     }
 
@@ -1408,30 +1406,6 @@ void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) cons
   }
 }
 
-lanelet::ConstLanelets StartPlannerModule::createDepartureCheckLanes() const
-{
-  const double backward_path_length =
-    planner_data_->parameters.backward_path_length + parameters_->max_back_distance;
-  const auto pull_out_lanes =
-    start_planner_utils::getPullOutLanes(planner_data_, backward_path_length);
-  if (pull_out_lanes.empty()) {
-    return lanelet::ConstLanelets{};
-  }
-  const auto road_lanes = utils::getExtendedCurrentLanes(
-    planner_data_, backward_path_length, std::numeric_limits<double>::max(),
-    /*forward_only_in_route*/ true);
-  // extract shoulder lanes from pull out lanes
-  lanelet::ConstLanelets shoulder_lanes;
-  std::copy_if(
-    pull_out_lanes.begin(), pull_out_lanes.end(), std::back_inserter(shoulder_lanes),
-    [this](const auto & pull_out_lane) {
-      return planner_data_->route_handler->isShoulderLanelet(pull_out_lane);
-    });
-  const auto departure_check_lanes = utils::transformToLanelets(
-    utils::generateDrivableLanesWithShoulderLanes(road_lanes, shoulder_lanes));
-  return departure_check_lanes;
-}
-
 void StartPlannerModule::setDebugData()
 {
   using lanelet::visualization::laneletsAsTriangleMarkerArray;