fix(behavior_path_planner): support the goal behind ego in pull out (a…

…utowarefoundation#2759) (#259)

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

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

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

planning/behavior_path_planner/include/behavior_path_planner/scene_module/utils/geometric_parallel_parking.hpp

@@ -119,7 +119,8 @@ class GeometricParallelParking
     const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes,
     const bool is_forward, const double end_pose_offset, const double velocity);
   PathWithLaneId generateStraightPath(const Pose & start_pose);
-  void setVelocityToArcPaths(std::vector<PathWithLaneId> & arc_paths, const double velocity);
+  void setVelocityToArcPaths(
+    std::vector<PathWithLaneId> & arc_paths, const double velocity, const bool set_stop_end);
 
   // debug
   Pose Cr_;

planning/behavior_path_planner/src/scene_module/pull_out/geometric_pull_out.cpp

@@ -62,12 +62,11 @@ boost::optional<PullOutPath> GeometricPullOut::plan(Pose start_pose, Pose goal_p
   if (parameters_.divide_pull_out_path) {
     output.partial_paths = planner_.getPaths();
   } else {
-    const auto partial_paths = planner_.getPaths();
-    auto combined_path = combineReferencePath(partial_paths.at(0), partial_paths.at(1));
-    // set same velocity to all points not to stop
-    for (auto & point : combined_path.points) {
-      point.point.longitudinal_velocity_mps = parameters_.geometric_pull_out_velocity;
-    }
+    auto partial_paths = planner_.getPaths();
+    // remove stop velocity of first arc path
+    partial_paths.front().points.back().point.longitudinal_velocity_mps =
+      parameters_.geometric_pull_out_velocity;
+    const auto combined_path = combineReferencePath(partial_paths.at(0), partial_paths.at(1));
     output.partial_paths.push_back(combined_path);
   }
   output.start_pose = planner_.getArcPaths().at(0).points.back().point.pose;

planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp

@@ -135,8 +135,7 @@ bool PullOutModule::isExecutionRequested() const
   // Check if any of the footprint points are in the shoulder lane
   lanelet::Lanelet closest_shoulder_lanelet;
   if (!lanelet::utils::query::getClosestLanelet(
-        planner_data_->route_handler->getShoulderLanelets(), planner_data_->self_pose->pose,
-        &closest_shoulder_lanelet)) {
+        pull_out_lanes, planner_data_->self_odometry->pose.pose, &closest_shoulder_lanelet)) {
     return false;
   }
   if (!isOverlappedWithLane(closest_shoulder_lanelet, vehicle_footprint)) {
@@ -494,7 +493,7 @@ PathWithLaneId PullOutModule::generateStopPath() const
     p.point.longitudinal_velocity_mps = 0.0;
     lanelet::Lanelet closest_shoulder_lanelet;
     lanelet::utils::query::getClosestLanelet(
-      planner_data_->route_handler->getShoulderLanelets(), pose, &closest_shoulder_lanelet);
+      status_.pull_out_lanes, pose, &closest_shoulder_lanelet);
     p.lane_ids.push_back(closest_shoulder_lanelet.id());
     return p;
   };

planning/behavior_path_planner/src/scene_module/pull_out/shift_pull_out.cpp

@@ -140,22 +140,26 @@ std::vector<PullOutPath> ShiftPullOut::calcPullOutPaths(
 
   for (double lateral_jerk = minimum_lateral_jerk; lateral_jerk <= maximum_lateral_jerk;
        lateral_jerk += jerk_resolution) {
-    // lateral distance from road center to start pose
-    const double shift_length = getArcCoordinates(road_lanes, start_pose).distance;
+    // 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);
+    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);
+      });
+    // if goal is behind start pose,
+    const bool goal_is_behind = arc_position_goal.length < s_start;
+    const double s_end = goal_is_behind ? road_lanes_length : arc_position_goal.length;
+    PathWithLaneId road_lane_reference_path = util::resamplePathWithSpline(
+      route_handler.getCenterLinePath(road_lanes, s_start, s_end), 1.0);
 
-    PathWithLaneId road_lane_reference_path{};
-    {
-      const auto arc_position = getArcCoordinates(road_lanes, start_pose);
-      const double s_start = std::max(arc_position.length - backward_path_length, 0.0);
-      const auto arc_position_goal = getArcCoordinates(road_lanes, goal_pose);
-      double s_end = arc_position_goal.length;
-      road_lane_reference_path = util::resamplePathWithSpline(
-        route_handler.getCenterLinePath(road_lanes, s_start, s_end), 1.0);
-    }
     PathShifter path_shifter{};
     path_shifter.setPath(road_lane_reference_path);
 
     // calculate after/before shifted pull out distance
+    // lateral distance from road center to start pose
+    const double shift_length = getArcCoordinates(road_lanes, start_pose).distance;
     const double pull_out_distance = std::max(
       PathShifter::calcLongitudinalDistFromJerk(
         abs(shift_length), lateral_jerk, shift_pull_out_velocity),
@@ -205,18 +209,17 @@ std::vector<PullOutPath> ShiftPullOut::calcPullOutPaths(
     // set velocity
     const size_t pull_out_end_idx =
       findNearestIndex(shifted_path.path.points, shift_end_pose.position);
-    const size_t goal_idx = findNearestIndex(shifted_path.path.points, goal_pose.position);
     for (size_t i = 0; i < shifted_path.path.points.size(); ++i) {
       auto & point = shifted_path.path.points.at(i);
       if (i < pull_out_end_idx) {
         point.point.longitudinal_velocity_mps = std::min(
           point.point.longitudinal_velocity_mps, static_cast<float>(shift_pull_out_velocity));
-        continue;
-      } else if (i >= goal_idx) {
-        point.point.longitudinal_velocity_mps = 0.0;
-        continue;
       }
     }
+    // if the end point is the goal, set the velocity to 0
+    if (!goal_is_behind) {
+      shifted_path.path.points.back().point.longitudinal_velocity_mps = 0.0;
+    }
 
     // add shifted path to candidates
     PullOutPath candidate_path;

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

@@ -99,11 +99,11 @@ PathWithLaneId GeometricParallelParking::getArcPath() const
 bool GeometricParallelParking::isParking() const { return current_path_idx_ > 0; }
 
 void GeometricParallelParking::setVelocityToArcPaths(
-  std::vector<PathWithLaneId> & arc_paths, const double velocity)
+  std::vector<PathWithLaneId> & arc_paths, const double velocity, const bool set_stop_end)
 {
   for (auto & path : arc_paths) {
     for (size_t i = 0; i < path.points.size(); i++) {
-      if (i == path.points.size() - 1) {
+      if (i == path.points.size() - 1 && set_stop_end) {
         // stop point at the end of the path
         path.points.at(i).point.longitudinal_velocity_mps = 0.0;
       } else {
@@ -130,7 +130,8 @@ std::vector<PathWithLaneId> GeometricParallelParking::generatePullOverPaths(
   arc_paths_ = arc_paths;
 
   // set parking velocity and stop velocity at the end of the path
-  setVelocityToArcPaths(arc_paths, velocity);
+  constexpr bool set_stop_end = true;
+  setVelocityToArcPaths(arc_paths, velocity, set_stop_end);
 
   // straight path from current to parking start
   const auto straight_path = generateStraightPath(start_pose);
@@ -266,24 +267,36 @@ bool GeometricParallelParking::planPullOut(
       }
     }
 
-    arc_paths_ = arc_paths;
-
     // get road center line path from departing end to goal, and combine after the second arc path
-    PathWithLaneId road_center_line_path;
-    {
-      const double s_start = getArcCoordinates(road_lanes, *end_pose).length + 1.0;  // need buffer?
-      const double s_end = getArcCoordinates(road_lanes, goal_pose).length;
-      road_center_line_path =
-        planner_data_->route_handler->getCenterLinePath(road_lanes, s_start, s_end, true);
-    }
+    const double s_start = getArcCoordinates(road_lanes, *end_pose).length + 1.0;  // need buffer?
+    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;
+    PathWithLaneId road_center_line_path =
+      planner_data_->route_handler->getCenterLinePath(road_lanes, s_start, s_end, true);
+
+    // set departing velocity to arc paths and 0 velocity to end point
+    constexpr bool set_stop_end = false;
+    setVelocityToArcPaths(arc_paths, parameters_.departing_velocity, set_stop_end);
+    arc_paths.back().points.front().point.longitudinal_velocity_mps = 0.0;
+
+    // combine the road center line path with the second arc path
     auto paths = arc_paths;
     paths.back().points.insert(
       paths.back().points.end(), road_center_line_path.points.begin(),
       road_center_line_path.points.end());
     removeOverlappingPoints(paths.back());
 
-    // set departing velocity and stop velocity at the end of the path
-    setVelocityToArcPaths(paths, parameters_.departing_velocity);
+    // if the end point is the goal, set the velocity to 0
+    if (!goal_is_behind) {
+      paths.back().points.back().point.longitudinal_velocity_mps = 0.0;
+    }
+
+    arc_paths_ = arc_paths;
     paths_ = paths;
 
     return true;