diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp index 284ea9409e924..639e5ce023bfe 100644 --- a/planning/behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_lane_change_module/src/scene.cpp @@ -1375,7 +1375,7 @@ bool NormalLaneChange::getLaneChangePaths( std::vector filtered_objects = filterObjectsInTargetLane(target_objects, target_lanes); if ( - !is_stuck && utils::lane_change::passParkedObject( + !is_stuck && !utils::lane_change::passParkedObject( route_handler, *candidate_path, filtered_objects, lane_change_buffer, is_goal_in_route, *lane_change_parameters_, object_debug_)) { debug_print( @@ -1417,6 +1417,24 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const CollisionCheckDebugMap debug_data; const bool is_stuck = isVehicleStuck(current_lanes); + + const auto & route_handler = *getRouteHandler(); + const auto & lc_param = *lane_change_parameters_; + + const auto min_lc_length = utils::lane_change::calcMinimumLaneChangeLength( + *lane_change_parameters_, + route_handler.getLateralIntervalsToPreferredLane(current_lanes.back())); + const auto is_goal_in_route = route_handler.isInGoalRouteSection(current_lanes.back()); + + const auto has_passed_parked_objects = utils::lane_change::passParkedObject( + route_handler, path, target_objects.target_lane, min_lc_length, is_goal_in_route, lc_param, + 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, is_stuck, debug_data); { diff --git a/planning/behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_lane_change_module/src/utils/utils.cpp index 0d5b20db3c0c8..e796f3af69db5 100644 --- a/planning/behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_lane_change_module/src/utils/utils.cpp @@ -991,14 +991,14 @@ bool passParkedObject( route_handler.getCenterLinePath(current_lanes, 0.0, std::numeric_limits::max()); if (objects.empty() || path.points.empty() || current_lane_path.points.empty()) { - return false; + 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); @@ -1006,7 +1006,7 @@ bool passParkedObject( const auto leading_obj_poly = tier4_autoware_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; @@ -1028,10 +1028,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(