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 index d3a120b43a073..32406a85248ac 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -217,6 +217,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; @@ -636,7 +653,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 NormalLaneChange::sampleLongitudinalAccValues( @@ -1221,6 +1239,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 " @@ -1240,21 +1272,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,