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 79c422ac283d1..e4a482def7b3d 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 @@ -14,6 +14,7 @@ #include "autoware/behavior_path_lane_change_module/utils/utils.hpp" +#include "autoware/behavior_path_lane_change_module/utils/calculation.hpp" #include "autoware/behavior_path_lane_change_module/utils/data_structs.hpp" #include "autoware/behavior_path_lane_change_module/utils/path.hpp" #include "autoware/behavior_path_planner_common/marker_utils/utils.hpp" @@ -988,30 +989,41 @@ bool passed_parked_objects( } 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( - current_lane_path.points, obj_p, current_path_end); - min_dist_to_end_of_current_lane = std::min(dist, min_dist_to_end_of_current_lane); - if (is_goal_in_route) { + const auto dist_to_path_end = [&](const auto & src_point) { + if (common_data_ptr->lanes_ptr->current_lane_in_goal_section) { const auto goal_pose = route_handler.getGoalPose(); - const double dist_to_goal = autoware::motion_utils::calcSignedArcLength( - current_lane_path.points, obj_p, goal_pose.position); - min_dist_to_end_of_current_lane = std::min(min_dist_to_end_of_current_lane, dist_to_goal); + return motion_utils::calcSignedArcLength( + current_lane_path.points, src_point, goal_pose.position); } - } + return motion_utils::calcSignedArcLength(current_lane_path.points, src_point, current_path_end); + }; + + const auto min_dist_to_end_of_current_lane = std::invoke([&]() { + auto min_dist_to_end_of_current_lane = std::numeric_limits::max(); + for (const auto & point : leading_obj_poly.outer()) { + const auto obj_p = universe_utils::createPoint(point.x(), point.y(), 0.0); + const auto dist = dist_to_path_end(obj_p); + min_dist_to_end_of_current_lane = std::min(dist, min_dist_to_end_of_current_lane); + } + return min_dist_to_end_of_current_lane; + }); // If there are still enough length after the target object, we delay the lane change - 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 false; + if (min_dist_to_end_of_current_lane <= minimum_lane_change_length) { + return true; } - return true; + const auto current_pose = common_data_ptr->get_ego_pose(); + const auto dist_ego_to_obj = motion_utils::calcSignedArcLength( + current_lane_path.points, current_pose.position, leading_obj.initial_pose.pose.position); + + if (dist_ego_to_obj < lane_change_path.info.length.lane_changing) { + return true; + } + + debug.second.unsafe_reason = "delay lane change"; + utils::path_safety_checker::updateCollisionCheckDebugMap(object_debug, debug, false); + return false; } std::optional getLeadingStaticObjectIdx(