diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp index 17eaceb055fc1..703d882794ee4 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp @@ -178,10 +178,10 @@ bool isParkedObject( const ExtendedPredictedObject & object, const double buffer_to_bound, const double ratio_threshold); -bool passParkedObject( +bool passed_parked_objects( const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path, const std::vector & objects, const double minimum_lane_change_length, - const bool is_goal_in_route, CollisionCheckDebugMap & object_debug); + CollisionCheckDebugMap & object_debug); std::optional getLeadingStaticObjectIdx( const RouteHandler & route_handler, const LaneChangePath & lane_change_path, diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index 659e5b78bf577..7a4b72631629d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -1628,10 +1628,9 @@ bool NormalLaneChange::getLaneChangePaths( candidate_paths->push_back(*candidate_path); if ( - !is_stuck && - utils::lane_change::passParkedObject( - common_data_ptr_, *candidate_path, filtered_objects.target_lane, lane_change_buffer, - is_goal_in_route, lane_change_debug_.collision_check_objects)) { + !is_stuck && !utils::lane_change::passed_parked_objects( + common_data_ptr_, *candidate_path, filtered_objects.target_lane, + lane_change_buffer, lane_change_debug_.collision_check_objects)) { debug_print_lat( "Reject: parking vehicle exists in the target lane, and the ego is not in stuck. Skip " "lane change."); @@ -1815,6 +1814,19 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const const auto target_objects = getTargetObjects(filtered_objects, current_lanes); CollisionCheckDebugMap debug_data; + + const auto min_lc_length = utils::lane_change::calcMinimumLaneChangeLength( + *lane_change_parameters_, + common_data_ptr_->route_handler_ptr->getLateralIntervalsToPreferredLane(current_lanes.back())); + + const auto has_passed_parked_objects = utils::lane_change::passed_parked_objects( + common_data_ptr_, path, filtered_objects.target_lane, min_lc_length, debug_data); + + if (!has_passed_parked_objects) { + RCLCPP_DEBUG(logger_, "Lane change has been delayed."); + return {false, false}; + } + const auto safety_status = isLaneChangePathSafe( path, target_objects, lane_change_parameters_->rss_params_for_abort, debug_data); { diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp index 20610380a0a5b..6ea280817f39b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp @@ -968,10 +968,10 @@ bool isParkedObject( return clamped_ratio > ratio_threshold; } -bool passParkedObject( +bool passed_parked_objects( const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path, const std::vector & objects, const double minimum_lane_change_length, - const bool is_goal_in_route, CollisionCheckDebugMap & object_debug) + CollisionCheckDebugMap & object_debug) { const auto route_handler = *common_data_ptr->route_handler_ptr; const auto lane_change_parameters = *common_data_ptr->lc_param_ptr; @@ -979,20 +979,19 @@ bool passParkedObject( lane_change_parameters.object_check_min_road_shoulder_width; const auto & object_shiftable_ratio_threshold = lane_change_parameters.object_shiftable_ratio_threshold; - const auto & path = lane_change_path.path; const auto & current_lanes = common_data_ptr->lanes_ptr->current; const auto current_lane_path = route_handler.getCenterLinePath(current_lanes, 0.0, std::numeric_limits::max()); - if (objects.empty() || path.points.empty() || current_lane_path.points.empty()) { - return false; + if (objects.empty() || lane_change_path.path.points.empty() || current_lane_path.points.empty()) { + return true; } const auto leading_obj_idx = getLeadingStaticObjectIdx( route_handler, lane_change_path, objects, object_check_min_road_shoulder_width, object_shiftable_ratio_threshold); if (!leading_obj_idx) { - return false; + return true; } const auto & leading_obj = objects.at(*leading_obj_idx); @@ -1000,11 +999,13 @@ bool passParkedObject( const auto leading_obj_poly = autoware::universe_utils::toPolygon2d(leading_obj.initial_pose.pose, leading_obj.shape); if (leading_obj_poly.outer().empty()) { - return false; + return true; } const auto & current_path_end = current_lane_path.points.back().point.pose.position; double min_dist_to_end_of_current_lane = std::numeric_limits::max(); + const auto & target_lanes = common_data_ptr->lanes_ptr->target; + const auto is_goal_in_route = route_handler.isInGoalRouteSection(target_lanes.back()); for (const auto & point : leading_obj_poly.outer()) { const auto obj_p = autoware::universe_utils::createPoint(point.x(), point.y(), 0.0); const double dist = autoware::motion_utils::calcSignedArcLength( @@ -1022,10 +1023,10 @@ bool passParkedObject( if (min_dist_to_end_of_current_lane > minimum_lane_change_length) { debug.second.unsafe_reason = "delay lane change"; utils::path_safety_checker::updateCollisionCheckDebugMap(object_debug, debug, false); - return true; + return false; } - return false; + return true; } std::optional getLeadingStaticObjectIdx(