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 fc60fde4ac740..c57b5b8a37e66 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 @@ -156,15 +156,6 @@ bool is_delay_lane_change( const std::vector & target_objects, CollisionCheckDebugMap & object_debug); -bool passed_parked_objects( - const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path, - const std::vector & objects, CollisionCheckDebugMap & object_debug); - -std::optional getLeadingStaticObjectIdx( - const RouteHandler & route_handler, const LaneChangePath & lane_change_path, - const std::vector & objects, - const double object_check_min_road_shoulder_width, const double object_shiftable_ratio_threshold); - lanelet::BasicPolygon2d create_polygon( const lanelet::ConstLanelets & lanes, const double start_dist, const double end_dist); 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 f4f087b031c79..df09e35108c5b 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 @@ -800,132 +800,6 @@ bool is_delay_lane_change( return false; } -bool passed_parked_objects( - const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path, - const std::vector & objects, CollisionCheckDebugMap & object_debug) -{ - const auto route_handler = *common_data_ptr->route_handler_ptr; - const auto lane_change_parameters = *common_data_ptr->lc_param_ptr; - const auto & object_check_min_road_shoulder_width = - lane_change_parameters.delay.min_road_shoulder_width; - const auto & object_shiftable_ratio_threshold = - lane_change_parameters.delay.th_parked_vehicle_shift_ratio; - const auto & current_lane_path = common_data_ptr->current_lanes_path; - - 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 true; - } - - const auto & leading_obj = objects.at(*leading_obj_idx); - auto debug = utils::path_safety_checker::createObjectDebug(leading_obj); - const auto leading_obj_poly = - autoware::universe_utils::toPolygon2d(leading_obj.initial_pose, leading_obj.shape); - if (leading_obj_poly.outer().empty()) { - return true; - } - - const auto & current_path_end = current_lane_path.points.back().point.pose.position; - 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(); - 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 <= common_data_ptr->transient_data.current_dist_buffer.min) { - 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.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( - const RouteHandler & route_handler, const LaneChangePath & lane_change_path, - const std::vector & objects, - const double object_check_min_road_shoulder_width, const double object_shiftable_ratio_threshold) -{ - const auto & path = lane_change_path.path; - - if (path.points.empty() || objects.empty()) { - return {}; - } - - const auto & lane_change_start = lane_change_path.info.lane_changing_start; - const auto & path_end = path.points.back(); - - double dist_lc_start_to_leading_obj = 0.0; - std::optional leading_obj_idx = std::nullopt; - for (size_t obj_idx = 0; obj_idx < objects.size(); ++obj_idx) { - const auto & obj = objects.at(obj_idx); - const auto & obj_pose = obj.initial_pose; - - // ignore non-static object - // TODO(shimizu): parametrize threshold - const double obj_vel_norm = std::hypot(obj.initial_twist.linear.x, obj.initial_twist.linear.y); - if (obj_vel_norm > 1.0) { - continue; - } - - // ignore non-parked object - if (!isParkedObject( - path, route_handler, obj, object_check_min_road_shoulder_width, - object_shiftable_ratio_threshold)) { - continue; - } - - const double dist_back_to_obj = autoware::motion_utils::calcSignedArcLength( - path.points, path_end.point.pose.position, obj_pose.position); - if (dist_back_to_obj > 0.0) { - // object is not on the lane change path - continue; - } - - const double dist_lc_start_to_obj = autoware::motion_utils::calcSignedArcLength( - path.points, lane_change_start.position, obj_pose.position); - if (dist_lc_start_to_obj < 0.0) { - // object is on the lane changing path or behind it. It will be detected in safety check - continue; - } - - if (dist_lc_start_to_obj > dist_lc_start_to_leading_obj) { - dist_lc_start_to_leading_obj = dist_lc_start_to_obj; - leading_obj_idx = obj_idx; - } - } - - return leading_obj_idx; -} - lanelet::BasicPolygon2d create_polygon( const lanelet::ConstLanelets & lanes, const double start_dist, const double end_dist) {