fix(start_planner): fix path when goal is out of current lanes (autow…

…arefoundation#4636)

* fix(start_planner): fix path when goal is not preffered lanes

Signed-off-by: kosuke55 <[email protected]>

* add calcEndArcLength

Signed-off-by: kosuke55 <[email protected]>

* Update planning/behavior_path_planner/src/utils/start_planner/util.cpp

Co-authored-by: Kyoichi Sugahara <[email protected]>

* Update planning/behavior_path_planner/src/utils/start_planner/util.cpp

* Update planning/behavior_path_planner/src/utils/start_planner/util.cpp

* Update planning/behavior_path_planner/src/utils/start_planner/util.cpp

* fix build

Signed-off-by: kosuke55 <[email protected]>

---------

Signed-off-by: kosuke55 <[email protected]>
Co-authored-by: Kyoichi Sugahara <[email protected]>

planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp

@@ -28,6 +28,7 @@
 #include <lanelet2_core/primitives/Primitive.h>
 
 #include <memory>
+#include <utility>
 #include <vector>
 
 namespace behavior_path_planner::start_planner_utils
@@ -47,6 +48,13 @@ lanelet::ConstLanelets getPullOutLanes(
   const std::shared_ptr<const PlannerData> & planner_data, const double backward_length);
 Pose getBackedPose(
   const Pose & current_pose, const double & yaw_shoulder_lane, const double & back_distance);
+/**
+ * @brief calculate end arc length to generate reference path considering the goal position
+ * @return a pair of s_end and terminal_is_goal
+ */
+std::pair<double, bool> calcEndArcLength(
+  const double s_start, const double forward_path_length, const lanelet::ConstLanelets & road_lanes,
+  const Pose & goal_pose);
 }  // namespace behavior_path_planner::start_planner_utils
 
 #endif  // BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__UTIL_HPP_

planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp

@@ -15,6 +15,7 @@
 #include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp"
 
 #include "behavior_path_planner/utils/path_utils.hpp"
+#include "behavior_path_planner/utils/start_planner/util.hpp"
 #include "behavior_path_planner/utils/utils.hpp"
 #include "tier4_autoware_utils/geometry/geometry.hpp"
 
@@ -272,13 +273,10 @@ bool GeometricParallelParking::planPullOut(
 
     // get road center line path from pull_out end to goal, and combine after the second arc path
     const double s_start = getArcCoordinates(road_lanes, *end_pose).length;
-    const double s_goal = getArcCoordinates(road_lanes, goal_pose).length;
-    const double road_lanes_length = std::accumulate(
-      road_lanes.begin(), road_lanes.end(), 0.0, [](const double sum, const auto & lane) {
-        return sum + lanelet::utils::getLaneletLength2d(lane);
-      });
-    const bool goal_is_behind = s_goal < s_start;
-    const double s_end = goal_is_behind ? road_lanes_length : s_goal;
+    const auto path_end_info = start_planner_utils::calcEndArcLength(
+      s_start, planner_data_->parameters.forward_path_length, road_lanes, goal_pose);
+    const double s_end = path_end_info.first;
+    const bool path_terminal_is_goal = path_end_info.second;
     PathWithLaneId road_center_line_path =
       planner_data_->route_handler->getCenterLinePath(road_lanes, s_start, s_end, true);
 
@@ -305,7 +303,7 @@ bool GeometricParallelParking::planPullOut(
     paths.back().points = motion_utils::removeOverlapPoints(paths.back().points);
 
     // if the end point is the goal, set the velocity to 0
-    if (!goal_is_behind) {
+    if (path_terminal_is_goal) {
       paths.back().points.back().point.longitudinal_velocity_mps = 0.0;
     }
 

planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp

@@ -150,13 +150,10 @@ std::vector<PullOutPath> ShiftPullOut::calcPullOutPaths(
   // generate road lane reference path
   const auto arc_position_start = getArcCoordinates(road_lanes, start_pose);
   const double s_start = std::max(arc_position_start.length - backward_path_length, 0.0);
-  const auto arc_position_goal = getArcCoordinates(road_lanes, goal_pose);
-
-  // if goal is behind start pose, use path with forward_path_length
-  const bool goal_is_behind = arc_position_goal.length < s_start;
-  const double s_forward_length = s_start + forward_path_length;
-  const double s_end =
-    goal_is_behind ? s_forward_length : std::min(arc_position_goal.length, s_forward_length);
+  const auto path_end_info =
+    start_planner_utils::calcEndArcLength(s_start, forward_path_length, road_lanes, goal_pose);
+  const double s_end = path_end_info.first;
+  const bool path_terminal_is_goal = path_end_info.second;
 
   constexpr double RESAMPLE_INTERVAL = 1.0;
   PathWithLaneId road_lane_reference_path = utils::resamplePathWithSpline(
@@ -276,7 +273,7 @@ std::vector<PullOutPath> ShiftPullOut::calcPullOutPaths(
       }
     }
     // if the end point is the goal, set the velocity to 0
-    if (!goal_is_behind) {
+    if (path_terminal_is_goal) {
       shifted_path.path.points.back().point.longitudinal_velocity_mps = 0.0;
     }
 

planning/behavior_path_planner/src/utils/start_planner/util.cpp

@@ -118,4 +118,26 @@ lanelet::ConstLanelets getPullOutLanes(
     /*forward_length*/ std::numeric_limits<double>::max(),
     /*forward_only_in_route*/ true);
 }
+
+std::pair<double, bool> calcEndArcLength(
+  const double s_start, const double forward_path_length, const lanelet::ConstLanelets & road_lanes,
+  const Pose & goal_pose)
+{
+  const double s_forward_length = s_start + forward_path_length;
+  // use forward length if the goal pose is not in the lanelets
+  if (!utils::isInLanelets(goal_pose, road_lanes)) {
+    return {s_forward_length, false};
+  }
+
+  const double s_goal = lanelet::utils::getArcCoordinates(road_lanes, goal_pose).length;
+
+  // If the goal is behind the start or beyond the forward length, use forward length.
+  if (s_goal < s_start || s_goal >= s_forward_length) {
+    return {s_forward_length, false};
+  }
+
+  // path end is goal
+  return {s_goal, true};
+}
+
 }  // namespace behavior_path_planner::start_planner_utils