fix(lane_change): stop point activated too early

Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>

planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp

@@ -220,7 +220,19 @@ void NormalLaneChange::insertStopPoint(
   const double stop_point_buffer = getCommonParam().backward_length_buffer_for_end_of_lane;
   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 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_);
+  const auto is_valid_start_point = 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;
@@ -640,7 +652,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(
@@ -1224,6 +1237,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 "
@@ -1238,27 +1265,14 @@ bool NormalLaneChange::getLaneChangePaths(
           lane_changing_length, forward_path_length, resample_interval, is_goal_in_route,
           next_lane_change_buffer);
 
+        lane_change_info.shift_line = utils::lane_change::getLaneChangingShiftLine(
+          prepare_segment, target_segment, target_lane_reference_path, shift_length);
+
         if (target_lane_reference_path.points.empty()) {
           debug_print("Reject: target_lane_reference_path is empty!!");
           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,
           sorted_lane_ids);