fix(lane_change): skip generating path if lane changing path is too long (autowarefoundation#8362)

rework. skip lane changing for insufficeient distance in target lane

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

Author: zulfaqar-azmi-t4

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp

@@ -191,6 +191,7 @@ struct Lanes
 {
   bool current_lane_in_goal_section{false};
   lanelet::ConstLanelets current;
+  lanelet::ConstLanelets target_neighbor;
   lanelet::ConstLanelets target;
   std::vector<lanelet::ConstLanelets> preceding_target;
 };

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp

@@ -106,10 +106,6 @@ lanelet::ConstLanelets getTargetNeighborLanes(
   const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes,
   const LaneChangeModuleType & type);
 
-lanelet::BasicPolygon2d getTargetNeighborLanesPolygon(
-  const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes,
-  const LaneChangeModuleType & type);
-
 bool isPathInLanelets(
   const PathWithLaneId & path, const lanelet::ConstLanelets & current_lanes,
   const lanelet::ConstLanelets & target_lanes);

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp

@@ -87,6 +87,9 @@ void NormalLaneChange::update_lanes(const bool is_approved)
   common_data_ptr_->lanes_ptr->target = target_lanes;
 
   const auto & route_handler_ptr = common_data_ptr_->route_handler_ptr;
+  common_data_ptr_->lanes_ptr->target_neighbor = utils::lane_change::getTargetNeighborLanes(
+    *route_handler_ptr, current_lanes, common_data_ptr_->lc_type);
+
   common_data_ptr_->lanes_ptr->current_lane_in_goal_section =
     route_handler_ptr->isInGoalRouteSection(current_lanes.back());
   common_data_ptr_->lanes_ptr->preceding_target = utils::getPrecedingLanelets(
@@ -1464,6 +1467,9 @@ bool NormalLaneChange::getLaneChangePaths(
       RCLCPP_DEBUG(logger_, "  -  sampling num for lat_acc: %lu", sample_lat_acc.size());
 
       debug_print("Prepare path satisfy constraints");
+      const auto dist_lc_start_to_end_of_lanes = calculation::calc_dist_from_pose_to_terminal_end(
+        common_data_ptr_, common_data_ptr_->lanes_ptr->target_neighbor, lane_changing_start_pose);
+
       for (const auto & lateral_acc : sample_lat_acc) {
         const auto lane_changing_time = PathShifter::calcShiftTimeFromJerk(
           shift_length, lane_change_parameters_->lane_changing_lateral_jerk, lateral_acc);
@@ -1474,11 +1480,6 @@ bool NormalLaneChange::getLaneChangePaths(
         const auto lane_changing_length = utils::lane_change::calcPhaseLength(
           initial_lane_changing_velocity, getCommonParam().max_vel,
           longitudinal_acc_on_lane_changing, lane_changing_time);
-        const auto terminal_lane_changing_velocity = std::min(
-          initial_lane_changing_velocity + longitudinal_acc_on_lane_changing * lane_changing_time,
-          getCommonParam().max_vel);
-        utils::lane_change::setPrepareVelocity(
-          prepare_segment, current_velocity, terminal_lane_changing_velocity);
 
         const auto debug_print_lat = [&](const auto & s) {
           RCLCPP_DEBUG(
@@ -1493,26 +1494,30 @@ bool NormalLaneChange::getLaneChangePaths(
           continue;
         }
 
-        if (is_goal_in_route) {
-          const double s_start =
-            lanelet::utils::getArcCoordinates(target_lanes, lane_changing_start_pose).length;
-          const double s_goal =
-            lanelet::utils::getArcCoordinates(target_lanes, route_handler.getGoalPose()).length;
-          const auto num =
+        // if multiple lane change is necessary, does the remaining distance is sufficient
+        const auto remaining_dist_in_target = std::invoke([&]() {
+          const auto finish_judge_buffer = lane_change_parameters_->lane_change_finish_judge_buffer;
+          const auto num_to_preferred_lane_from_target_lane =
             std::abs(route_handler.getNumLaneToPreferredLane(target_lanes.back(), direction));
-          const double backward_buffer =
-            num == 0 ? 0.0 : lane_change_parameters_->backward_length_buffer_for_end_of_lane;
-          const double finish_judge_buffer =
-            lane_change_parameters_->lane_change_finish_judge_buffer;
-          if (
-            s_start + lane_changing_length + finish_judge_buffer + backward_buffer +
-              next_lane_change_buffer >
-            s_goal) {
-            debug_print_lat("Reject: length of lane changing path is longer than length to goal!!");
-            continue;
-          }
+          const auto backward_len_buffer =
+            lane_change_parameters_->backward_length_buffer_for_end_of_lane;
+          const auto backward_buffer_to_target_lane =
+            num_to_preferred_lane_from_target_lane == 0 ? 0.0 : backward_len_buffer;
+          return lane_changing_length + finish_judge_buffer + backward_buffer_to_target_lane +
+                 next_lane_change_buffer;
+        });
+
+        if (remaining_dist_in_target > dist_lc_start_to_end_of_lanes) {
+          debug_print_lat("Reject: length of lane changing path is longer than length to goal!!");
+          continue;
         }
 
+        const auto terminal_lane_changing_velocity = std::min(
+          initial_lane_changing_velocity + longitudinal_acc_on_lane_changing * lane_changing_time,
+          getCommonParam().max_vel);
+        utils::lane_change::setPrepareVelocity(
+          prepare_segment, current_velocity, terminal_lane_changing_velocity);
+
         const auto target_segment = getTargetSegment(
           target_lanes, lane_changing_start_pose, target_lane_length, lane_changing_length,
           initial_lane_changing_velocity, next_lane_change_buffer);

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp

@@ -290,21 +290,6 @@ lanelet::ConstLanelets getTargetNeighborLanes(
   return neighbor_lanes;
 }
 
-lanelet::BasicPolygon2d getTargetNeighborLanesPolygon(
-  const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes,
-  const LaneChangeModuleType & type)
-{
-  const auto target_neighbor_lanelets =
-    utils::lane_change::getTargetNeighborLanes(route_handler, current_lanes, type);
-  if (target_neighbor_lanelets.empty()) {
-    return {};
-  }
-  const auto target_neighbor_preferred_lane_poly = lanelet::utils::getPolygonFromArcLength(
-    target_neighbor_lanelets, 0, std::numeric_limits<double>::max());
-
-  return lanelet::utils::to2D(target_neighbor_preferred_lane_poly).basicPolygon();
-}
-
 bool isPathInLanelets(
   const PathWithLaneId & path, const lanelet::ConstLanelets & current_lanes,
   const lanelet::ConstLanelets & target_lanes)
@@ -1227,9 +1212,8 @@ LanesPolygon create_lanes_polygon(const CommonDataPtr & common_data_ptr)
   lanes_polygon.expanded_target = utils::lane_change::createPolygon(
     expanded_target_lanes, 0.0, std::numeric_limits<double>::max());
 
-  const auto & route_handler = *common_data_ptr->route_handler_ptr;
-  lanes_polygon.target_neighbor =
-    getTargetNeighborLanesPolygon(route_handler, lanes->current, common_data_ptr->lc_type);
+  lanes_polygon.target_neighbor = *utils::lane_change::createPolygon(
+    lanes->target_neighbor, 0.0, std::numeric_limits<double>::max());
 
   lanes_polygon.preceding_target.reserve(lanes->preceding_target.size());
   for (const auto & preceding_lane : lanes->preceding_target) {