fix(lane_change): stop point activated too early (autowarefoundation#…

…5402) * fix(lane_change): stop point activated too early Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]> * refactor Signed-off-by: Zulfaqar Azmi <[email protected]> * fix dead node Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]> --------- Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]> Signed-off-by: Zulfaqar Azmi <[email protected]>
kyoichi-sugahara · Nov 2, 2023 · 4e00bf2 · 4e00bf2
1 parent 012de21
commit 4e00bf2
Showing 1 changed file with 33 additions and 14 deletions.
diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp
@@ -222,6 +222,23 @@ void NormalLaneChange::insertStopPoint(
   const auto target_objects = getTargetObjects(status_.current_lanes, status_.target_lanes);
   double stopping_distance = distance_to_terminal - lane_change_buffer - stop_point_buffer;
 
+  const auto is_valid_start_point = std::invoke([&]() -> bool {
+    auto lc_start_point = lanelet::utils::conversion::toLaneletPoint(
+      status_.lane_change_path.info.lane_changing_start.position);
+    const auto target_neighbor_preferred_lane_poly_2d =
+      utils::lane_change::getTargetNeighborLanesPolygon(
+        *route_handler, status_.current_lanes, type_);
+    return boost::geometry::covered_by(
+      lanelet::traits::to2D(lc_start_point), target_neighbor_preferred_lane_poly_2d);
+  });
+
+  if (!is_valid_start_point) {
+    const auto stop_point = utils::insertStopPoint(stopping_distance, path);
+    setStopPose(stop_point.point.pose);
+
+    return;
+  }
+
   // calculate minimum distance from path front to the stationary object on the ego lane.
   const auto distance_to_ego_lane_obj = [&]() -> double {
     double distance_to_obj = distance_to_terminal;
@@ -646,7 +663,8 @@ double NormalLaneChange::calcMaximumLaneChangeLength(
   const auto shift_intervals =
     getRouteHandler()->getLateralIntervalsToPreferredLane(current_terminal_lanelet);
   return utils::lane_change::calcMaximumLaneChangeLength(
-    getEgoVelocity(), getCommonParam(), shift_intervals, max_acc);
+    std::max(getCommonParam().minimum_lane_changing_velocity, getEgoVelocity()), getCommonParam(),
+    shift_intervals, max_acc);
 }
 
 std::vector<double> NormalLaneChange::sampleLongitudinalAccValues(
@@ -1250,6 +1268,20 @@ bool NormalLaneChange::getLaneChangePaths(
           boost::geometry::covered_by(lc_start_point, target_neighbor_preferred_lane_poly_2d) ||
           boost::geometry::covered_by(lc_start_point, target_lane_poly_2d);
 
+        LaneChangeInfo lane_change_info;
+        lane_change_info.longitudinal_acceleration =
+          LaneChangePhaseInfo{longitudinal_acc_on_prepare, longitudinal_acc_on_lane_changing};
+        lane_change_info.duration = LaneChangePhaseInfo{prepare_duration, lane_changing_time};
+        lane_change_info.velocity =
+          LaneChangePhaseInfo{prepare_velocity, initial_lane_changing_velocity};
+        lane_change_info.length = LaneChangePhaseInfo{prepare_length, lane_changing_length};
+        lane_change_info.current_lanes = current_lanes;
+        lane_change_info.target_lanes = target_lanes;
+        lane_change_info.lane_changing_start = prepare_segment.points.back().point.pose;
+        lane_change_info.lane_changing_end = target_segment.points.front().point.pose;
+        lane_change_info.lateral_acceleration = lateral_acc;
+        lane_change_info.terminal_lane_changing_velocity = terminal_lane_changing_velocity;
+
         if (!is_valid_start_point) {
           debug_print(
             "Reject: lane changing points are not inside of the target preferred lanes or its "
@@ -1269,21 +1301,8 @@ bool NormalLaneChange::getLaneChangePaths(
           continue;
         }
 
-        LaneChangeInfo lane_change_info;
-        lane_change_info.longitudinal_acceleration =
-          LaneChangePhaseInfo{longitudinal_acc_on_prepare, longitudinal_acc_on_lane_changing};
-        lane_change_info.duration = LaneChangePhaseInfo{prepare_duration, lane_changing_time};
-        lane_change_info.velocity =
-          LaneChangePhaseInfo{prepare_velocity, initial_lane_changing_velocity};
-        lane_change_info.length = LaneChangePhaseInfo{prepare_length, lane_changing_length};
-        lane_change_info.current_lanes = current_lanes;
-        lane_change_info.target_lanes = target_lanes;
-        lane_change_info.lane_changing_start = prepare_segment.points.back().point.pose;
-        lane_change_info.lane_changing_end = target_segment.points.front().point.pose;
         lane_change_info.shift_line = utils::lane_change::getLaneChangingShiftLine(
           prepare_segment, target_segment, target_lane_reference_path, shift_length);
-        lane_change_info.lateral_acceleration = lateral_acc;
-        lane_change_info.terminal_lane_changing_velocity = terminal_lane_changing_velocity;
 
         const auto candidate_path = utils::lane_change::constructCandidatePath(
           lane_change_info, prepare_segment, target_segment, target_lane_reference_path,