From 1348392ff45c2913ef5d29235a540bf150922059 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Fri, 17 Feb 2023 13:06:56 +0900 Subject: [PATCH 1/3] fix(lane_change): extend target lane range in safety check (#2776) * fix(lane_change): extend target lane range in safety check Signed-off-by: Muhammad Zulfaqar Azmi * fix both safety check in lane change module Signed-off-by: Muhammad Zulfaqar Azmi * Refactoring Signed-off-by: Muhammad Zulfaqar Azmi * also made for external request Signed-off-by: Muhammad Zulfaqar Azmi * get all lanes within 100 meters Signed-off-by: Muhammad Zulfaqar Azmi * Change logging level Signed-off-by: Muhammad Zulfaqar Azmi * Fix target lane empty Signed-off-by: Muhammad Zulfaqar Azmi * check current pose Signed-off-by: Muhammad Zulfaqar Azmi * fixed the wrong lane parameter Signed-off-by: Muhammad Zulfaqar Azmi * fix spell check Signed-off-by: Muhammad Zulfaqar Azmi * make code similar between lane change and external request Signed-off-by: Muhammad Zulfaqar Azmi * fix collision not working Signed-off-by: Muhammad Zulfaqar Azmi * Rename parameters Signed-off-by: Muhammad Zulfaqar Azmi * fix remove some argument from function parameter Signed-off-by: Muhammad Zulfaqar Azmi * style(pre-commit): autofix --------- Signed-off-by: Muhammad Zulfaqar Azmi Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../scene_module/lane_change/util.hpp | 11 +- .../external_request_lane_change_module.cpp | 88 +++++----- .../lane_change/lane_change_module.cpp | 94 +++++------ .../src/scene_module/lane_change/util.cpp | 150 ++++++++++++------ .../include/route_handler/route_handler.hpp | 13 ++ planning/route_handler/src/route_handler.cpp | 8 + 6 files changed, 225 insertions(+), 139 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp index d0b83171771ba..e9c2368e2ae05 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp @@ -81,8 +81,7 @@ LaneChangePaths selectValidPaths( const Pose & current_pose, const Pose & goal_pose, const double minimum_lane_change_length); bool selectSafePath( - const LaneChangePaths & paths, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes, + const LaneChangePaths & paths, const lanelet::ConstLanelets & backward_lanes, const PredictedObjects::ConstSharedPtr dynamic_objects, const Pose & current_pose, const Twist & current_twist, const BehaviorPathPlannerParameters & common_parameters, const behavior_path_planner::LaneChangeParameters & ros_parameters, @@ -90,8 +89,7 @@ bool selectSafePath( std::unordered_map & debug_data); bool isLaneChangePathSafe( - const LaneChangePath & lane_change_path, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes, + const LaneChangePath & lane_change_path, const lanelet::ConstLanelets & backward_lanes, const PredictedObjects::ConstSharedPtr dynamic_objects, const Pose & current_pose, const size_t current_seg_idx, const Twist & current_twist, const BehaviorPathPlannerParameters & common_parameters, @@ -148,6 +146,9 @@ bool hasEnoughDistanceToLaneChangeAfterAbort( const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, const Pose & curent_pose, const double abort_return_dist, const BehaviorPathPlannerParameters & common_param); -} // namespace behavior_path_planner::lane_change_utils +lanelet::ConstLanelets getExtendedTargetLanesForCollisionCheck( + const RouteHandler & route_handler, const lanelet::ConstLanelet & target_lanes, + const Pose & current_pose, const double backward_length); +} // namespace behavior_path_planner::lane_change_utils #endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__UTIL_HPP_ diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp index f2c8cad34b4e9..055f53304b5c8 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp @@ -371,50 +371,56 @@ std::pair ExternalRequestLaneChangeModule::getSafePath( const auto current_lanes = util::getCurrentLanes(planner_data_); - if (!lane_change_lanes.empty()) { - // find candidate paths - const auto lane_change_paths = lane_change_utils::getLaneChangePaths( - *route_handler, current_lanes, lane_change_lanes, current_pose, current_twist, - common_parameters, *parameters_); - - // get lanes used for detection - lanelet::ConstLanelets check_lanes; - if (!lane_change_paths.empty()) { - const auto & longest_path = lane_change_paths.front(); - // we want to see check_distance [m] behind vehicle so add lane changing length - const double check_distance_with_path = check_distance + longest_path.length.sum(); - check_lanes = route_handler->getCheckTargetLanesFromPath( - longest_path.path, lane_change_lanes, check_distance_with_path); - } + if (lane_change_lanes.empty()) { + return std::make_pair(false, false); + } - // select valid path - const LaneChangePaths valid_paths = lane_change_utils::selectValidPaths( - lane_change_paths, current_lanes, check_lanes, *route_handler, current_pose, - route_handler->getGoalPose(), - common_parameters.minimum_lane_change_length + - common_parameters.backward_length_buffer_for_end_of_lane + - parameters_->lane_change_finish_judge_buffer); + // find candidate paths + const auto lane_change_paths = lane_change_utils::getLaneChangePaths( + *route_handler, current_lanes, lane_change_lanes, current_pose, current_twist, + common_parameters, *parameters_); - if (valid_paths.empty()) { - return std::make_pair(false, false); - } - debug_valid_path_ = valid_paths; + if (lane_change_paths.empty()) { + return std::make_pair(false, false); + } + + // get lanes used for detection + lanelet::ConstLanelets check_lanes; + const auto & longest_path = lane_change_paths.front(); + // we want to see check_distance [m] behind vehicle so add lane changing length + const double check_distance_with_path = check_distance + longest_path.length.sum(); + check_lanes = route_handler->getCheckTargetLanesFromPath( + longest_path.path, lane_change_lanes, check_distance_with_path); + + // select valid path + const LaneChangePaths valid_paths = lane_change_utils::selectValidPaths( + lane_change_paths, current_lanes, check_lanes, *route_handler, current_pose, + route_handler->getGoalPose(), + common_parameters.minimum_lane_change_length + + common_parameters.backward_length_buffer_for_end_of_lane + + parameters_->lane_change_finish_judge_buffer); + + if (valid_paths.empty()) { + return std::make_pair(false, false); + } + debug_valid_path_ = valid_paths; - // select safe path - const bool found_safe_path = lane_change_utils::selectSafePath( - valid_paths, current_lanes, check_lanes, planner_data_->dynamic_object, current_pose, - current_twist, common_parameters, *parameters_, &safe_path, object_debug_); + // get lanes used for detection + const auto backward_lanes = lane_change_utils::getExtendedTargetLanesForCollisionCheck( + *route_handler, lane_change_lanes.front(), current_pose, check_distance); - if (parameters_->publish_debug_marker) { - setObjectDebugVisualization(); - } else { - debug_marker_.markers.clear(); - } + // select safe path + const bool found_safe_path = lane_change_utils::selectSafePath( + valid_paths, backward_lanes, planner_data_->dynamic_object, current_pose, current_twist, + common_parameters, *parameters_, &safe_path, object_debug_); - return std::make_pair(true, found_safe_path); + if (parameters_->publish_debug_marker) { + setObjectDebugVisualization(); + } else { + debug_marker_.markers.clear(); } - return std::make_pair(false, false); + return std::make_pair(true, found_safe_path); } bool ExternalRequestLaneChangeModule::isSafe() const { return status_.is_safe; } @@ -669,16 +675,14 @@ bool ExternalRequestLaneChangeModule::isApprovedPathSafe(Pose & ego_pose_before_ const auto current_pose = getEgoPose(); const auto current_twist = getEgoTwist(); const auto & dynamic_objects = planner_data_->dynamic_object; - const auto & current_lanes = status_.current_lanes; const auto & common_parameters = planner_data_->parameters; const auto & route_handler = planner_data_->route_handler; const auto path = status_.lane_change_path; constexpr double check_distance = 100.0; // get lanes used for detection - const double check_distance_with_path = check_distance + path.length.sum(); - const auto check_lanes = route_handler->getCheckTargetLanesFromPath( - path.path, status_.lane_change_lanes, check_distance_with_path); + const auto check_lanes = lane_change_utils::getExtendedTargetLanesForCollisionCheck( + *route_handler, path.target_lanelets.front(), current_pose, check_distance); std::unordered_map debug_data; @@ -686,7 +690,7 @@ bool ExternalRequestLaneChangeModule::isApprovedPathSafe(Pose & ego_pose_before_ path.path.points, current_pose, common_parameters.ego_nearest_dist_threshold, common_parameters.ego_nearest_yaw_threshold); return lane_change_utils::isLaneChangePathSafe( - path, current_lanes, check_lanes, dynamic_objects, current_pose, current_seg_idx, current_twist, + path, check_lanes, dynamic_objects, current_pose, current_seg_idx, current_twist, common_parameters, *parameters_, common_parameters.expected_front_deceleration_for_abort, common_parameters.expected_rear_deceleration_for_abort, ego_pose_before_collision, debug_data, false, status_.lane_change_path.acceleration); diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp index c66531b6796e3..ef647d7ac1ea3 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp @@ -371,50 +371,55 @@ std::pair LaneChangeModule::getSafePath( const auto current_lanes = util::getCurrentLanes(planner_data_); - if (!lane_change_lanes.empty()) { - // find candidate paths - const auto lane_change_paths = lane_change_utils::getLaneChangePaths( - *route_handler, current_lanes, lane_change_lanes, current_pose, current_twist, - common_parameters, *parameters_); - - // get lanes used for detection - lanelet::ConstLanelets check_lanes; - if (!lane_change_paths.empty()) { - const auto & longest_path = lane_change_paths.front(); - // we want to see check_distance [m] behind vehicle so add lane changing length - const double check_distance_with_path = check_distance + longest_path.length.sum(); - check_lanes = route_handler->getCheckTargetLanesFromPath( - longest_path.path, lane_change_lanes, check_distance_with_path); - } - - // select valid path - const LaneChangePaths valid_paths = lane_change_utils::selectValidPaths( - lane_change_paths, current_lanes, check_lanes, *route_handler, current_pose, - route_handler->getGoalPose(), - common_parameters.minimum_lane_change_length + - common_parameters.backward_length_buffer_for_end_of_lane + - parameters_->lane_change_finish_judge_buffer); - - if (valid_paths.empty()) { - return std::make_pair(false, false); - } - debug_valid_path_ = valid_paths; - - // select safe path - const bool found_safe_path = lane_change_utils::selectSafePath( - valid_paths, current_lanes, check_lanes, planner_data_->dynamic_object, current_pose, - current_twist, common_parameters, *parameters_, &safe_path, object_debug_); + if (lane_change_lanes.empty()) { + return std::make_pair(false, false); + } - if (parameters_->publish_debug_marker) { - setObjectDebugVisualization(); - } else { - debug_marker_.markers.clear(); - } + // find candidate paths + const auto lane_change_paths = lane_change_utils::getLaneChangePaths( + *route_handler, current_lanes, lane_change_lanes, current_pose, current_twist, + common_parameters, *parameters_); - return std::make_pair(true, found_safe_path); + if (lane_change_paths.empty()) { + return std::make_pair(false, false); + } + // get lanes used for detection + lanelet::ConstLanelets check_lanes; + const auto & longest_path = lane_change_paths.front(); + // we want to see check_distance [m] behind vehicle so add lane changing length + const double check_distance_with_path = check_distance + longest_path.length.sum(); + check_lanes = route_handler->getCheckTargetLanesFromPath( + longest_path.path, lane_change_lanes, check_distance_with_path); + + // select valid path + const LaneChangePaths valid_paths = lane_change_utils::selectValidPaths( + lane_change_paths, current_lanes, check_lanes, *route_handler, current_pose, + route_handler->getGoalPose(), + common_parameters.minimum_lane_change_length + + common_parameters.backward_length_buffer_for_end_of_lane + + parameters_->lane_change_finish_judge_buffer); + + if (valid_paths.empty()) { + return std::make_pair(false, false); + } + debug_valid_path_ = valid_paths; + + // select safe path + const auto backward_lanes = lane_change_utils::getExtendedTargetLanesForCollisionCheck( + *route_handler, lane_change_paths.front().target_lanelets.front(), current_pose, + check_distance); + const bool found_safe_path = lane_change_utils::selectSafePath( + valid_paths, backward_lanes, planner_data_->dynamic_object, current_pose, current_twist, + common_parameters, *parameters_, &safe_path, object_debug_); + + if (parameters_->publish_debug_marker) { + setObjectDebugVisualization(); + } else { + debug_marker_.markers.clear(); } - return std::make_pair(false, false); + return std::make_pair(true, found_safe_path); + // get lanes used for detection } bool LaneChangeModule::isSafe() const { return status_.is_safe; } @@ -669,16 +674,13 @@ bool LaneChangeModule::isApprovedPathSafe(Pose & ego_pose_before_collision) cons const auto current_pose = getEgoPose(); const auto current_twist = getEgoTwist(); const auto & dynamic_objects = planner_data_->dynamic_object; - const auto & current_lanes = status_.current_lanes; const auto & common_parameters = planner_data_->parameters; const auto & route_handler = planner_data_->route_handler; const auto & path = status_.lane_change_path; - constexpr double check_distance = 100.0; // get lanes used for detection - const double check_distance_with_path = check_distance + path.length.sum(); - const auto check_lanes = route_handler->getCheckTargetLanesFromPath( - path.path, status_.lane_change_lanes, check_distance_with_path); + const auto check_lanes = lane_change_utils::getExtendedTargetLanesForCollisionCheck( + *route_handler, path.target_lanelets.front(), current_pose, check_distance_); std::unordered_map debug_data; @@ -686,7 +688,7 @@ bool LaneChangeModule::isApprovedPathSafe(Pose & ego_pose_before_collision) cons path.path.points, current_pose, common_parameters.ego_nearest_dist_threshold, common_parameters.ego_nearest_yaw_threshold); return lane_change_utils::isLaneChangePathSafe( - path, current_lanes, check_lanes, dynamic_objects, current_pose, current_seg_idx, current_twist, + path, check_lanes, dynamic_objects, current_pose, current_seg_idx, current_twist, common_parameters, *parameters_, common_parameters.expected_front_deceleration_for_abort, common_parameters.expected_rear_deceleration_for_abort, ego_pose_before_collision, debug_data, false, status_.lane_change_path.acceleration); diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp index fed7da8849a11..0050d4330d53a 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp @@ -21,6 +21,7 @@ #include "behavior_path_planner/scene_module/utils/path_shifter.hpp" #include "behavior_path_planner/utilities.hpp" +#include #include #include #include @@ -51,14 +52,13 @@ using tier4_autoware_utils::Polygon2d; void filterObjectIndices( const PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes, const PathWithLaneId & ego_path, - const Pose & current_pose, const double forward_path_length, const double filter_width, - std::vector & current_lane_obj_indices, std::vector & target_lane_obj_indices, + const lanelet::ConstLanelets & target_lanes, const lanelet::ConstLanelets & target_backward_lanes, + const PathWithLaneId & ego_path, const Pose & current_pose, const double forward_path_length, + const double filter_width, std::vector & in_lane_obj_indices, std::vector & others_obj_indices, const bool ignore_unknown_obj = false) { // Reserve maximum amount possible - current_lane_obj_indices.reserve(objects.objects.size()); - target_lane_obj_indices.reserve(objects.objects.size()); + in_lane_obj_indices.reserve(objects.objects.size()); others_obj_indices.reserve(objects.objects.size()); const auto get_basic_polygon = @@ -100,15 +100,30 @@ void filterObjectIndices( const double distance = boost::geometry::distance(obj_polygon, ego_path_linestring); if (distance < filter_width) { - current_lane_obj_indices.push_back(i); + in_lane_obj_indices.push_back(i); continue; } } const bool is_intersect_with_target = boost::geometry::intersects(target_polygon, obj_polygon); if (is_intersect_with_target) { - target_lane_obj_indices.push_back(i); - } else { + in_lane_obj_indices.push_back(i); + continue; + } + + const bool is_intersect_with_backward = std::invoke([&]() { + for (const auto & ll : target_backward_lanes) { + const bool is_intersect_with_backward = + boost::geometry::intersects(ll.polygon2d().basicPolygon(), obj_polygon); + if (is_intersect_with_backward) { + in_lane_obj_indices.push_back(i); + return true; + } + } + return false; + }); + + if (!is_intersect_with_backward) { others_obj_indices.push_back(i); } } @@ -479,8 +494,7 @@ LaneChangePaths selectValidPaths( } bool selectSafePath( - const LaneChangePaths & paths, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes, + const LaneChangePaths & paths, const lanelet::ConstLanelets & backward_lanes, const PredictedObjects::ConstSharedPtr dynamic_objects, const Pose & current_pose, const Twist & current_twist, const BehaviorPathPlannerParameters & common_parameters, const LaneChangeParameters & ros_parameters, LaneChangePath * selected_path, @@ -493,9 +507,8 @@ bool selectSafePath( common_parameters.ego_nearest_yaw_threshold); Pose ego_pose_before_collision; if (isLaneChangePathSafe( - path, current_lanes, target_lanes, dynamic_objects, current_pose, current_seg_idx, - current_twist, common_parameters, ros_parameters, - common_parameters.expected_front_deceleration, + path, backward_lanes, dynamic_objects, current_pose, current_seg_idx, current_twist, + common_parameters, ros_parameters, common_parameters.expected_front_deceleration, common_parameters.expected_rear_deceleration, ego_pose_before_collision, debug_data, true, path.acceleration)) { *selected_path = path; @@ -549,8 +562,7 @@ bool hasEnoughDistance( } bool isLaneChangePathSafe( - const LaneChangePath & lane_change_path, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes, + const LaneChangePath & lane_change_path, const lanelet::ConstLanelets & backward_lanes, const PredictedObjects::ConstSharedPtr dynamic_objects, const Pose & current_pose, const size_t current_seg_idx, const Twist & current_twist, const BehaviorPathPlannerParameters & common_parameters, @@ -564,7 +576,8 @@ bool isLaneChangePathSafe( } const auto & path = lane_change_path.path; - if (path.points.empty() || target_lanes.empty() || current_lanes.empty()) { + const auto & target_lanes = lane_change_path.target_lanelets; + if (path.points.empty()) { return false; } @@ -583,19 +596,23 @@ bool isLaneChangePathSafe( lane_change_parameters.prepare_phase_ignore_target_speed_thresh; const auto & vehicle_info = common_parameters.vehicle_info; - std::vector current_lane_object_indices{}; - std::vector target_lane_object_indices{}; + std::vector in_lane_object_indices{}; std::vector other_lane_object_indices{}; { const auto lateral_buffer = (use_buffer) ? 0.5 : 0.0; const auto current_obj_filtering_buffer = lateral_buffer + common_parameters.vehicle_width / 2; filterObjectIndices( - *dynamic_objects, lane_change_path.reference_lanelets, lane_change_path.target_lanelets, path, + *dynamic_objects, lane_change_path.reference_lanelets, target_lanes, backward_lanes, path, current_pose, common_parameters.forward_path_length, current_obj_filtering_buffer, - current_lane_object_indices, target_lane_object_indices, other_lane_object_indices, true); + in_lane_object_indices, other_lane_object_indices, true); } + RCLCPP_DEBUG( + rclcpp::get_logger("lane_change"), "number of object -> total: %lu, in lane: %lu, others: %lu", + dynamic_objects->objects.size(), in_lane_object_indices.size(), + other_lane_object_indices.size()); + const auto assignDebugData = [](const PredictedObject & obj) { CollisionCheckDebug debug; const auto key = util::getUuidStr(obj); @@ -616,7 +633,7 @@ bool isLaneChangePathSafe( } }; - for (const auto & i : current_lane_object_indices) { + for (const auto & i : in_lane_object_indices) { const auto & obj = dynamic_objects->objects.at(i); const auto object_speed = util::l2Norm(obj.kinematics.initial_twist_with_covariance.twist.linear); @@ -639,20 +656,10 @@ bool isLaneChangePathSafe( } // Collision check for objects in lane change target lane - for (const auto & i : target_lane_object_indices) { + for (const auto & i : in_lane_object_indices) { const auto & obj = dynamic_objects->objects.at(i); auto current_debug_data = assignDebugData(obj); current_debug_data.second.ego_predicted_path.push_back(vehicle_predicted_path); - bool is_object_in_target = false; - if (lane_change_parameters.use_predicted_path_outside_lanelet) { - is_object_in_target = true; - } else { - for (const auto & llt : target_lanes) { - if (lanelet::utils::isInLanelet(obj.kinematics.initial_pose_with_covariance.pose, llt)) { - is_object_in_target = true; - } - } - } const auto predicted_paths = util::getPredictedPathFromObj(obj, lane_change_parameters.use_all_predicted_path); @@ -663,27 +670,45 @@ bool isLaneChangePathSafe( (object_speed > prepare_phase_ignore_target_speed_thresh)) ? 0.0 : lane_change_prepare_duration; - if (is_object_in_target) { - for (const auto & obj_path : predicted_paths) { - if (!util::isSafeInLaneletCollisionCheck( - current_pose, current_twist, vehicle_predicted_path, vehicle_info, check_start_time, - check_end_time, time_resolution, obj, obj_path, common_parameters, front_decel, - rear_decel, ego_pose_before_collision, current_debug_data.second)) { - appendDebugInfo(current_debug_data, false); - return false; - } - } - } else { - if (!util::isSafeInFreeSpaceCollisionCheck( + for (const auto & obj_path : predicted_paths) { + if (!util::isSafeInLaneletCollisionCheck( current_pose, current_twist, vehicle_predicted_path, vehicle_info, check_start_time, - check_end_time, time_resolution, obj, common_parameters, front_decel, rear_decel, - current_debug_data.second)) { + check_end_time, time_resolution, obj, obj_path, common_parameters, front_decel, + rear_decel, ego_pose_before_collision, current_debug_data.second)) { appendDebugInfo(current_debug_data, false); return false; } } appendDebugInfo(current_debug_data, true); } + + if (!lane_change_parameters.use_predicted_path_outside_lanelet) { + return true; + } + + for (const auto & i : other_lane_object_indices) { + const auto & obj = dynamic_objects->objects.at(i); + auto current_debug_data = assignDebugData(obj); + current_debug_data.second.ego_predicted_path.push_back(vehicle_predicted_path); + + const auto predicted_paths = + util::getPredictedPathFromObj(obj, lane_change_parameters.use_all_predicted_path); + + const auto object_speed = + util::l2Norm(obj.kinematics.initial_twist_with_covariance.twist.linear); + const double check_start_time = (enable_collision_check_at_prepare_phase && + (object_speed > prepare_phase_ignore_target_speed_thresh)) + ? 0.0 + : lane_change_prepare_duration; + if (!util::isSafeInFreeSpaceCollisionCheck( + current_pose, current_twist, vehicle_predicted_path, vehicle_info, check_start_time, + check_end_time, time_resolution, obj, common_parameters, front_decel, rear_decel, + current_debug_data.second)) { + appendDebugInfo(current_debug_data, false); + return false; + } + appendDebugInfo(current_debug_data, true); + } return true; } @@ -1076,4 +1101,37 @@ bool hasEnoughDistanceToLaneChangeAfterAbort( return true; } + +// TODO(Azu): In the future, get back lanelet within `to_back_dist` [m] from queried lane +lanelet::ConstLanelets getExtendedTargetLanesForCollisionCheck( + const RouteHandler & route_handler, const lanelet::ConstLanelet & target_lane, + const Pose & current_pose, const double backward_length) +{ + const auto arc_length = lanelet::utils::getArcCoordinates({target_lane}, current_pose); + + if (arc_length.length >= backward_length) { + return {}; + } + + const auto preceding_lanes = route_handler.getPrecedingLaneletSequence( + target_lane, std::abs(backward_length - arc_length.length), {target_lane}); + + lanelet::ConstLanelets backward_lanes{}; + const auto num_of_lanes = std::invoke([&preceding_lanes]() { + size_t sum{0}; + for (const auto & lanes : preceding_lanes) { + sum += lanes.size(); + } + return sum; + }); + + backward_lanes.reserve(num_of_lanes); + + for (const auto & lanes : preceding_lanes) { + backward_lanes.insert(backward_lanes.end(), lanes.begin(), lanes.end()); + } + + return backward_lanes; +} + } // namespace behavior_path_planner::lane_change_utils diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp index 66c312431c50f..78b2ef55c0ae8 100644 --- a/planning/route_handler/include/route_handler/route_handler.hpp +++ b/planning/route_handler/include/route_handler/route_handler.hpp @@ -234,6 +234,19 @@ class RouteHandler const lanelet::ConstLanelet & lanelet, bool is_right = true, bool is_left = true, bool is_opposite = true) const noexcept; + /** + * Retrieves a sequence of lanelets before the given lanelet. + * The total length of retrieved lanelet sequence at least given length. Returned lanelet sequence + * does not include input lanelet.] + * @param graph [input lanelet routing graph] + * @param lanelet [input lanelet] + * @param length [minimum length of retrieved lanelet sequence] + * @return [lanelet sequence that leads to given lanelet] + */ + std::vector getPrecedingLaneletSequence( + const lanelet::ConstLanelet & lanelet, const double length, + const lanelet::ConstLanelets & exclude_lanelets = {}) const; + int getNumLaneToPreferredLane(const lanelet::ConstLanelet & lanelet) const; bool getClosestLaneletWithinRoute( const Pose & search_pose, lanelet::ConstLanelet * closest_lanelet) const; diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index 74a17ffb31c7f..b0d4fe247d862 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -1084,6 +1084,14 @@ lanelet::ConstLineStrings3d RouteHandler::getFurthestLinestring( return linestrings; } +std::vector RouteHandler::getPrecedingLaneletSequence( + const lanelet::ConstLanelet & lanelet, const double length, + const lanelet::ConstLanelets & exclude_lanelets) const +{ + return lanelet::utils::query::getPrecedingLaneletSequences( + routing_graph_ptr_, lanelet, length, exclude_lanelets); +} + bool RouteHandler::getLaneChangeTarget( const lanelet::ConstLanelets & lanelets, lanelet::ConstLanelet * target_lanelet) const { From 5f3b503620b773e8a5f54573883a23243e821872 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Tue, 21 Feb 2023 15:10:54 +0900 Subject: [PATCH 2/3] refactor(lane_change): compute obj indices return if safe path found (#2825) Signed-off-by: Muhammad Zulfaqar Azmi --- .../lane_change/lane_change_module_data.hpp | 7 + .../scene_module/lane_change/util.hpp | 36 +- .../external_request_lane_change_module.cpp | 63 ++- .../lane_change/lane_change_module.cpp | 61 ++- .../src/scene_module/lane_change/util.cpp | 361 +++++++++--------- 5 files changed, 244 insertions(+), 284 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp index 2b06c6d70224d..df0183d80668b 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp @@ -70,6 +70,13 @@ struct LaneChangePhaseInfo [[nodiscard]] double sum() const { return prepare + lane_changing; } }; + +struct LaneChangeTargetObjectIndices +{ + std::vector current_lane{}; + std::vector target_lane{}; + std::vector other_lane{}; +}; } // namespace behavior_path_planner #endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__LANE_CHANGE_MODULE_DATA_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp index e9c2368e2ae05..f13b4489e7da4 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp @@ -69,33 +69,22 @@ std::optional constructCandidatePath( const double prepare_speed, const double lane_change_distance, const double lane_changing_speed, const BehaviorPathPlannerParameters & params, const LaneChangeParameters & lane_change_param); -LaneChangePaths getLaneChangePaths( +std::pair getLaneChangePaths( const RouteHandler & route_handler, const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets, const Pose & pose, const Twist & twist, - const BehaviorPathPlannerParameters & common_parameter, - const behavior_path_planner::LaneChangeParameters & parameter); - -LaneChangePaths selectValidPaths( - const LaneChangePaths & paths, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes, const RouteHandler & route_handler, - const Pose & current_pose, const Pose & goal_pose, const double minimum_lane_change_length); - -bool selectSafePath( - const LaneChangePaths & paths, const lanelet::ConstLanelets & backward_lanes, - const PredictedObjects::ConstSharedPtr dynamic_objects, const Pose & current_pose, - const Twist & current_twist, const BehaviorPathPlannerParameters & common_parameters, - const behavior_path_planner::LaneChangeParameters & ros_parameters, - LaneChangePath * selected_path, - std::unordered_map & debug_data); + const PredictedObjects::ConstSharedPtr dynamic_objects, + const BehaviorPathPlannerParameters & common_parameter, const LaneChangeParameters & parameter, + const double check_distance, LaneChangePaths * candidate_paths, + std::unordered_map * debug_data); bool isLaneChangePathSafe( - const LaneChangePath & lane_change_path, const lanelet::ConstLanelets & backward_lanes, - const PredictedObjects::ConstSharedPtr dynamic_objects, const Pose & current_pose, + const LaneChangePath & lane_change_path, const PredictedObjects::ConstSharedPtr dynamic_objects, + const LaneChangeTargetObjectIndices & dynamic_object_indices, const Pose & current_pose, const size_t current_seg_idx, const Twist & current_twist, const BehaviorPathPlannerParameters & common_parameters, const behavior_path_planner::LaneChangeParameters & lane_change_parameters, const double front_decel, const double rear_decel, Pose & ego_pose_before_collision, - std::unordered_map & debug_data, const bool use_buffer = true, + std::unordered_map & debug_data, const double acceleration = 0.0); bool hasEnoughDistance( @@ -150,5 +139,14 @@ bool hasEnoughDistanceToLaneChangeAfterAbort( lanelet::ConstLanelets getExtendedTargetLanesForCollisionCheck( const RouteHandler & route_handler, const lanelet::ConstLanelet & target_lanes, const Pose & current_pose, const double backward_length); + +LaneChangeTargetObjectIndices filterObjectIndices( + const LaneChangePaths & lane_change_paths, const PredictedObjects & objects, + const lanelet::ConstLanelets & target_backward_lanes, const Pose & current_pose, + const double forward_path_length, const double filter_width, + const bool ignore_unknown_obj = false); + +double calcLateralBufferForFiltering(const double vehicle_width, const double lateral_buffer = 0.0); + } // namespace behavior_path_planner::lane_change_utils #endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__UTIL_HPP_ diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp index 055f53304b5c8..f5589d4ed6263 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp @@ -376,51 +376,31 @@ std::pair ExternalRequestLaneChangeModule::getSafePath( } // find candidate paths - const auto lane_change_paths = lane_change_utils::getLaneChangePaths( + LaneChangePaths valid_paths; + const auto [found_valid_path, found_safe_path] = lane_change_utils::getLaneChangePaths( *route_handler, current_lanes, lane_change_lanes, current_pose, current_twist, - common_parameters, *parameters_); - - if (lane_change_paths.empty()) { - return std::make_pair(false, false); - } - - // get lanes used for detection - lanelet::ConstLanelets check_lanes; - const auto & longest_path = lane_change_paths.front(); - // we want to see check_distance [m] behind vehicle so add lane changing length - const double check_distance_with_path = check_distance + longest_path.length.sum(); - check_lanes = route_handler->getCheckTargetLanesFromPath( - longest_path.path, lane_change_lanes, check_distance_with_path); - - // select valid path - const LaneChangePaths valid_paths = lane_change_utils::selectValidPaths( - lane_change_paths, current_lanes, check_lanes, *route_handler, current_pose, - route_handler->getGoalPose(), - common_parameters.minimum_lane_change_length + - common_parameters.backward_length_buffer_for_end_of_lane + - parameters_->lane_change_finish_judge_buffer); - - if (valid_paths.empty()) { - return std::make_pair(false, false); - } + planner_data_->dynamic_object, common_parameters, *parameters_, check_distance, &valid_paths, + &object_debug_); debug_valid_path_ = valid_paths; - // get lanes used for detection - const auto backward_lanes = lane_change_utils::getExtendedTargetLanesForCollisionCheck( - *route_handler, lane_change_lanes.front(), current_pose, check_distance); - - // select safe path - const bool found_safe_path = lane_change_utils::selectSafePath( - valid_paths, backward_lanes, planner_data_->dynamic_object, current_pose, current_twist, - common_parameters, *parameters_, &safe_path, object_debug_); - if (parameters_->publish_debug_marker) { setObjectDebugVisualization(); } else { debug_marker_.markers.clear(); } - return std::make_pair(true, found_safe_path); + if (!found_valid_path) { + return {false, false}; + } + + if (found_safe_path) { + safe_path = valid_paths.back(); + } else { + // force candidate + safe_path = valid_paths.front(); + } + + return {found_valid_path, found_safe_path}; } bool ExternalRequestLaneChangeModule::isSafe() const { return status_.is_safe; } @@ -686,14 +666,21 @@ bool ExternalRequestLaneChangeModule::isApprovedPathSafe(Pose & ego_pose_before_ std::unordered_map debug_data; + constexpr auto ignore_unknown{true}; + const auto lateral_buffer = + lane_change_utils::calcLateralBufferForFiltering(common_parameters.vehicle_width); + const auto dynamic_object_indices = lane_change_utils::filterObjectIndices( + {path}, *dynamic_objects, check_lanes, current_pose, common_parameters.forward_path_length, + lateral_buffer, ignore_unknown); + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( path.path.points, current_pose, common_parameters.ego_nearest_dist_threshold, common_parameters.ego_nearest_yaw_threshold); return lane_change_utils::isLaneChangePathSafe( - path, check_lanes, dynamic_objects, current_pose, current_seg_idx, current_twist, + path, dynamic_objects, dynamic_object_indices, current_pose, current_seg_idx, current_twist, common_parameters, *parameters_, common_parameters.expected_front_deceleration_for_abort, common_parameters.expected_rear_deceleration_for_abort, ego_pose_before_collision, debug_data, - false, status_.lane_change_path.acceleration); + status_.lane_change_path.acceleration); } void ExternalRequestLaneChangeModule::updateOutputTurnSignal(BehaviorModuleOutput & output) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp index ef647d7ac1ea3..6381039c217a8 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp @@ -376,50 +376,31 @@ std::pair LaneChangeModule::getSafePath( } // find candidate paths - const auto lane_change_paths = lane_change_utils::getLaneChangePaths( + LaneChangePaths valid_paths; + const auto [found_valid_path, found_safe_path] = lane_change_utils::getLaneChangePaths( *route_handler, current_lanes, lane_change_lanes, current_pose, current_twist, - common_parameters, *parameters_); - - if (lane_change_paths.empty()) { - return std::make_pair(false, false); - } - // get lanes used for detection - lanelet::ConstLanelets check_lanes; - const auto & longest_path = lane_change_paths.front(); - // we want to see check_distance [m] behind vehicle so add lane changing length - const double check_distance_with_path = check_distance + longest_path.length.sum(); - check_lanes = route_handler->getCheckTargetLanesFromPath( - longest_path.path, lane_change_lanes, check_distance_with_path); - - // select valid path - const LaneChangePaths valid_paths = lane_change_utils::selectValidPaths( - lane_change_paths, current_lanes, check_lanes, *route_handler, current_pose, - route_handler->getGoalPose(), - common_parameters.minimum_lane_change_length + - common_parameters.backward_length_buffer_for_end_of_lane + - parameters_->lane_change_finish_judge_buffer); - - if (valid_paths.empty()) { - return std::make_pair(false, false); - } + planner_data_->dynamic_object, common_parameters, *parameters_, check_distance, &valid_paths, + &object_debug_); debug_valid_path_ = valid_paths; - // select safe path - const auto backward_lanes = lane_change_utils::getExtendedTargetLanesForCollisionCheck( - *route_handler, lane_change_paths.front().target_lanelets.front(), current_pose, - check_distance); - const bool found_safe_path = lane_change_utils::selectSafePath( - valid_paths, backward_lanes, planner_data_->dynamic_object, current_pose, current_twist, - common_parameters, *parameters_, &safe_path, object_debug_); - if (parameters_->publish_debug_marker) { setObjectDebugVisualization(); } else { debug_marker_.markers.clear(); } - return std::make_pair(true, found_safe_path); - // get lanes used for detection + if (!found_valid_path) { + return {false, false}; + } + + if (found_safe_path) { + safe_path = valid_paths.back(); + } else { + // force candidate + safe_path = valid_paths.front(); + } + + return {found_valid_path, found_safe_path}; } bool LaneChangeModule::isSafe() const { return status_.is_safe; } @@ -683,15 +664,21 @@ bool LaneChangeModule::isApprovedPathSafe(Pose & ego_pose_before_collision) cons *route_handler, path.target_lanelets.front(), current_pose, check_distance_); std::unordered_map debug_data; + constexpr auto ignore_unknown{true}; + const auto lateral_buffer = + lane_change_utils::calcLateralBufferForFiltering(common_parameters.vehicle_width); + const auto dynamic_object_indices = lane_change_utils::filterObjectIndices( + {path}, *dynamic_objects, check_lanes, current_pose, common_parameters.forward_path_length, + lateral_buffer, ignore_unknown); const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( path.path.points, current_pose, common_parameters.ego_nearest_dist_threshold, common_parameters.ego_nearest_yaw_threshold); return lane_change_utils::isLaneChangePathSafe( - path, check_lanes, dynamic_objects, current_pose, current_seg_idx, current_twist, + path, dynamic_objects, dynamic_object_indices, current_pose, current_seg_idx, current_twist, common_parameters, *parameters_, common_parameters.expected_front_deceleration_for_abort, common_parameters.expected_rear_deceleration_for_abort, ego_pose_before_collision, debug_data, - false, status_.lane_change_path.acceleration); + status_.lane_change_path.acceleration); } void LaneChangeModule::updateOutputTurnSignal(BehaviorModuleOutput & output) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp index 0050d4330d53a..6341ce00f210c 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp @@ -42,93 +42,12 @@ namespace using autoware_auto_perception_msgs::msg::ObjectClassification; using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::PathWithLaneId; -using behavior_path_planner::util::calcObjectPolygon; -using behavior_path_planner::util::getHighestProbLabel; using geometry_msgs::msg::Pose; using route_handler::RouteHandler; using tier4_autoware_utils::LineString2d; using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; -void filterObjectIndices( - const PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes, const lanelet::ConstLanelets & target_backward_lanes, - const PathWithLaneId & ego_path, const Pose & current_pose, const double forward_path_length, - const double filter_width, std::vector & in_lane_obj_indices, - std::vector & others_obj_indices, const bool ignore_unknown_obj = false) -{ - // Reserve maximum amount possible - in_lane_obj_indices.reserve(objects.objects.size()); - others_obj_indices.reserve(objects.objects.size()); - - const auto get_basic_polygon = - [](const lanelet::ConstLanelets & lanes, const double start_dist, const double end_dist) { - const auto polygon_3d = lanelet::utils::getPolygonFromArcLength(lanes, start_dist, end_dist); - return lanelet::utils::to2D(polygon_3d).basicPolygon(); - }; - const auto arc = lanelet::utils::getArcCoordinates(current_lanes, current_pose); - const auto current_polygon = - get_basic_polygon(current_lanes, arc.length, arc.length + forward_path_length); - const auto target_polygon = - get_basic_polygon(target_lanes, 0.0, std::numeric_limits::max()); - LineString2d ego_path_linestring; - ego_path_linestring.reserve(ego_path.points.size()); - for (const auto & pt : ego_path.points) { - const auto & position = pt.point.pose.position; - boost::geometry::append(ego_path_linestring, Point2d(position.x, position.y)); - } - - for (size_t i = 0; i < objects.objects.size(); ++i) { - const auto & obj = objects.objects.at(i); - - if (ignore_unknown_obj) { - const auto t = getHighestProbLabel(obj.classification); - if (t == ObjectClassification::UNKNOWN) { - continue; - } - } - - Polygon2d obj_polygon; - if (!calcObjectPolygon(obj, &obj_polygon)) { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("behavior_path_planner").get_child("lane_change"), - "Failed to calcObjectPolygon...!!!"); - continue; - } - - if (boost::geometry::intersects(current_polygon, obj_polygon)) { - const double distance = boost::geometry::distance(obj_polygon, ego_path_linestring); - - if (distance < filter_width) { - in_lane_obj_indices.push_back(i); - continue; - } - } - - const bool is_intersect_with_target = boost::geometry::intersects(target_polygon, obj_polygon); - if (is_intersect_with_target) { - in_lane_obj_indices.push_back(i); - continue; - } - - const bool is_intersect_with_backward = std::invoke([&]() { - for (const auto & ll : target_backward_lanes) { - const bool is_intersect_with_backward = - boost::geometry::intersects(ll.polygon2d().basicPolygon(), obj_polygon); - if (is_intersect_with_backward) { - in_lane_obj_indices.push_back(i); - return true; - } - } - return false; - }); - - if (!is_intersect_with_backward) { - others_obj_indices.push_back(i); - } - } -} - std::vector> getSortedLaneIds( const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, const double rough_shift_length) @@ -194,6 +113,7 @@ namespace behavior_path_planner::lane_change_utils { using autoware_auto_planning_msgs::msg::PathPointWithLaneId; using lanelet::ArcCoordinates; +using util::getHighestProbLabel; inline double calcLaneChangeResamplingInterval( const double lane_changing_distance, const double lane_changing_speed) @@ -348,37 +268,46 @@ std::optional constructCandidatePath( return std::optional{candidate_path}; } -LaneChangePaths getLaneChangePaths( +std::pair getLaneChangePaths( const RouteHandler & route_handler, const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets, const Pose & pose, const Twist & twist, - const BehaviorPathPlannerParameters & common_parameter, const LaneChangeParameters & parameter) + const PredictedObjects::ConstSharedPtr dynamic_objects, + const BehaviorPathPlannerParameters & common_parameter, const LaneChangeParameters & parameter, + const double check_distance, LaneChangePaths * candidate_paths, + std::unordered_map * debug_data) { - LaneChangePaths candidate_paths{}; - + debug_data->clear(); if (original_lanelets.empty() || target_lanelets.empty()) { - return candidate_paths; + return {false, false}; } + Pose ego_pose_before_collision{}; + // rename parameter - const auto & backward_path_length = common_parameter.backward_path_length; - const auto & forward_path_length = common_parameter.forward_path_length; - const auto & lane_change_prepare_duration = parameter.lane_change_prepare_duration; - const auto & minimum_lane_change_prepare_distance = + const auto backward_path_length = common_parameter.backward_path_length; + const auto forward_path_length = common_parameter.forward_path_length; + const auto lane_change_prepare_duration = parameter.lane_change_prepare_duration; + const auto minimum_lane_change_prepare_distance = common_parameter.minimum_lane_change_prepare_distance; - const auto & minimum_lane_change_velocity = parameter.minimum_lane_change_velocity; - const auto & maximum_deceleration = parameter.maximum_deceleration; - const auto & lane_change_sampling_num = parameter.lane_change_sampling_num; + const auto minimum_lane_change_velocity = parameter.minimum_lane_change_velocity; + const auto maximum_deceleration = parameter.maximum_deceleration; + const auto lane_change_sampling_num = parameter.lane_change_sampling_num; // get velocity - const double current_velocity = util::l2Norm(twist.linear); - const double acceleration_resolution = std::abs(maximum_deceleration) / lane_change_sampling_num; - const double target_distance = + const auto current_velocity = util::l2Norm(twist.linear); + + const auto acceleration_resolution = std::abs(maximum_deceleration) / lane_change_sampling_num; + + const auto target_distance = util::getArcLengthToTargetLanelet(original_lanelets, target_lanelets.front(), pose); const auto num_to_preferred_lane = std::abs(route_handler.getNumLaneToPreferredLane(target_lanelets.back())); + const auto goal_pose = route_handler.getGoalPose(); + const auto is_goal_in_route = route_handler.isInGoalRouteSection(target_lanelets.back()); + const auto end_of_lane_dist = std::invoke([&]() { const auto required_dist = util::calcLaneChangeBuffer(common_parameter, num_to_preferred_lane); if (is_goal_in_route) { @@ -398,19 +327,24 @@ LaneChangePaths getLaneChangePaths( const auto sorted_lane_ids = getSortedLaneIds( route_handler, original_lanelets, target_lanelets, arc_position_from_target.distance); + constexpr auto ignore_unknown{true}; + const auto lateral_buffer = calcLateralBufferForFiltering(common_parameter.vehicle_width, 0.5); + + LaneChangeTargetObjectIndices dynamic_object_indices; + candidate_paths->reserve(lane_change_sampling_num); for (double acceleration = 0.0; acceleration >= -maximum_deceleration; acceleration -= acceleration_resolution) { - const double prepare_speed = getExpectedVelocityWhenDecelerate( + const auto prepare_speed = getExpectedVelocityWhenDecelerate( current_velocity, acceleration, lane_change_prepare_duration); // skip if velocity becomes less than zero before starting lane change if (prepare_speed < 0.0) { - continue; + break; } // get path on original lanes - const double prepare_distance = getDistanceWhenDecelerate( + const auto prepare_distance = getDistanceWhenDecelerate( current_velocity, acceleration, lane_change_prepare_duration, minimum_lane_change_prepare_distance); @@ -444,22 +378,25 @@ LaneChangePaths getLaneChangePaths( continue; } - const Pose & lane_changing_start_pose = prepare_segment_reference.points.back().point.pose; + const auto & lane_changing_start_pose = prepare_segment_reference.points.back().point.pose; const auto resample_interval = calcLaneChangeResamplingInterval(lane_changing_distance, lane_changing_speed); + const auto target_lane_reference_path = getReferencePathFromTargetLane( route_handler, target_lanelets, lane_changing_start_pose, target_lane_length, lc_dist, required_total_min_distance, forward_path_length, resample_interval, is_goal_in_route); + if (target_lane_reference_path.points.empty()) { continue; } - const ShiftLine shift_line = getLaneChangeShiftLine( + const auto shift_line = getLaneChangeShiftLine( prepare_segment_reference, lane_changing_segment_reference, target_lanelets, target_lane_reference_path); const auto lc_speed = LaneChangePhaseInfo{prepare_speed, lane_changing_speed}; + const auto candidate_path = constructCandidatePath( prepare_segment_reference, lane_changing_segment_reference, target_lane_reference_path, shift_line, original_lanelets, target_lanelets, sorted_lane_ids, acceleration, lc_dist, @@ -469,60 +406,46 @@ LaneChangePaths getLaneChangePaths( continue; } - candidate_paths.push_back(*candidate_path); - } - - return candidate_paths; -} + const auto is_valid = hasEnoughDistance( + *candidate_path, original_lanelets, target_lanelets, pose, goal_pose, route_handler, + common_parameter.minimum_lane_change_length); -LaneChangePaths selectValidPaths( - const LaneChangePaths & paths, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes, const RouteHandler & route_handler, - const Pose & current_pose, const Pose & goal_pose, const double minimum_lane_change_length) -{ - LaneChangePaths available_paths; + if (!is_valid) { + continue; + } - for (const auto & path : paths) { - if (hasEnoughDistance( - path, current_lanes, target_lanes, current_pose, goal_pose, route_handler, - minimum_lane_change_length)) { - available_paths.push_back(path); + if (candidate_paths->empty()) { + // only compute dynamic object indices once + const auto backward_lanes = lane_change_utils::getExtendedTargetLanesForCollisionCheck( + route_handler, target_lanelets.front(), pose, check_distance); + dynamic_object_indices = filterObjectIndices( + {*candidate_path}, *dynamic_objects, backward_lanes, pose, + common_parameter.forward_path_length, lateral_buffer, ignore_unknown); } - } + candidate_paths->push_back(*candidate_path); - return available_paths; -} + const auto current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + candidate_path->path.points, pose, common_parameter.ego_nearest_dist_threshold, + common_parameter.ego_nearest_yaw_threshold); -bool selectSafePath( - const LaneChangePaths & paths, const lanelet::ConstLanelets & backward_lanes, - const PredictedObjects::ConstSharedPtr dynamic_objects, const Pose & current_pose, - const Twist & current_twist, const BehaviorPathPlannerParameters & common_parameters, - const LaneChangeParameters & ros_parameters, LaneChangePath * selected_path, - std::unordered_map & debug_data) -{ - debug_data.clear(); - for (const auto & path : paths) { - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.path.points, current_pose, common_parameters.ego_nearest_dist_threshold, - common_parameters.ego_nearest_yaw_threshold); - Pose ego_pose_before_collision; - if (isLaneChangePathSafe( - path, backward_lanes, dynamic_objects, current_pose, current_seg_idx, current_twist, - common_parameters, ros_parameters, common_parameters.expected_front_deceleration, - common_parameters.expected_rear_deceleration, ego_pose_before_collision, debug_data, true, - path.acceleration)) { - *selected_path = path; - return true; + const auto is_safe = isLaneChangePathSafe( + *candidate_path, dynamic_objects, dynamic_object_indices, pose, current_seg_idx, twist, + common_parameter, parameter, common_parameter.expected_front_deceleration, + common_parameter.expected_rear_deceleration, ego_pose_before_collision, *debug_data, + acceleration); + + if (is_valid && is_safe) { + return {true, true}; } } - // set first path for force lane change if no valid path found - if (!paths.empty()) { - *selected_path = paths.front(); - return false; + + if (candidate_paths->empty()) { + return {false, false}; } - return false; + return {true, false}; } + bool hasEnoughDistance( const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes, [[maybe_unused]] const lanelet::ConstLanelets & target_lanes, const Pose & current_pose, @@ -562,21 +485,20 @@ bool hasEnoughDistance( } bool isLaneChangePathSafe( - const LaneChangePath & lane_change_path, const lanelet::ConstLanelets & backward_lanes, - const PredictedObjects::ConstSharedPtr dynamic_objects, const Pose & current_pose, + const LaneChangePath & lane_change_path, const PredictedObjects::ConstSharedPtr dynamic_objects, + const LaneChangeTargetObjectIndices & dynamic_objects_indices, const Pose & current_pose, const size_t current_seg_idx, const Twist & current_twist, const BehaviorPathPlannerParameters & common_parameters, const LaneChangeParameters & lane_change_parameters, const double front_decel, const double rear_decel, Pose & ego_pose_before_collision, - std::unordered_map & debug_data, const bool use_buffer, - const double acceleration) + std::unordered_map & debug_data, const double acceleration) { if (dynamic_objects == nullptr) { return true; } const auto & path = lane_change_path.path; - const auto & target_lanes = lane_change_path.target_lanelets; + if (path.points.empty()) { return false; } @@ -596,22 +518,15 @@ bool isLaneChangePathSafe( lane_change_parameters.prepare_phase_ignore_target_speed_thresh; const auto & vehicle_info = common_parameters.vehicle_info; - std::vector in_lane_object_indices{}; - std::vector other_lane_object_indices{}; - { - const auto lateral_buffer = (use_buffer) ? 0.5 : 0.0; - const auto current_obj_filtering_buffer = lateral_buffer + common_parameters.vehicle_width / 2; - - filterObjectIndices( - *dynamic_objects, lane_change_path.reference_lanelets, target_lanes, backward_lanes, path, - current_pose, common_parameters.forward_path_length, current_obj_filtering_buffer, - in_lane_object_indices, other_lane_object_indices, true); - } + auto in_lane_object_indices = dynamic_objects_indices.target_lane; + in_lane_object_indices.insert( + in_lane_object_indices.end(), dynamic_objects_indices.current_lane.begin(), + dynamic_objects_indices.current_lane.end()); RCLCPP_DEBUG( rclcpp::get_logger("lane_change"), "number of object -> total: %lu, in lane: %lu, others: %lu", dynamic_objects->objects.size(), in_lane_object_indices.size(), - other_lane_object_indices.size()); + dynamic_objects_indices.other_lane.size()); const auto assignDebugData = [](const PredictedObject & obj) { CollisionCheckDebug debug; @@ -655,38 +570,11 @@ bool isLaneChangePathSafe( } } - // Collision check for objects in lane change target lane - for (const auto & i : in_lane_object_indices) { - const auto & obj = dynamic_objects->objects.at(i); - auto current_debug_data = assignDebugData(obj); - current_debug_data.second.ego_predicted_path.push_back(vehicle_predicted_path); - - const auto predicted_paths = - util::getPredictedPathFromObj(obj, lane_change_parameters.use_all_predicted_path); - - const auto object_speed = - util::l2Norm(obj.kinematics.initial_twist_with_covariance.twist.linear); - const double check_start_time = (enable_collision_check_at_prepare_phase && - (object_speed > prepare_phase_ignore_target_speed_thresh)) - ? 0.0 - : lane_change_prepare_duration; - for (const auto & obj_path : predicted_paths) { - if (!util::isSafeInLaneletCollisionCheck( - current_pose, current_twist, vehicle_predicted_path, vehicle_info, check_start_time, - check_end_time, time_resolution, obj, obj_path, common_parameters, front_decel, - rear_decel, ego_pose_before_collision, current_debug_data.second)) { - appendDebugInfo(current_debug_data, false); - return false; - } - } - appendDebugInfo(current_debug_data, true); - } - if (!lane_change_parameters.use_predicted_path_outside_lanelet) { return true; } - for (const auto & i : other_lane_object_indices) { + for (const auto & i : dynamic_objects_indices.other_lane) { const auto & obj = dynamic_objects->objects.at(i); auto current_debug_data = assignDebugData(obj); current_debug_data.second.ego_predicted_path.push_back(vehicle_predicted_path); @@ -1134,4 +1022,97 @@ lanelet::ConstLanelets getExtendedTargetLanesForCollisionCheck( return backward_lanes; } +LaneChangeTargetObjectIndices filterObjectIndices( + const LaneChangePaths & lane_change_paths, const PredictedObjects & objects, + const lanelet::ConstLanelets & target_backward_lanes, const Pose & current_pose, + const double forward_path_length, const double filter_width, const bool ignore_unknown_obj) +{ + // Reserve maximum amount possible + + std::vector current_lane_obj_indices{}; + std::vector target_lane_obj_indices{}; + std::vector others_obj_indices{}; + current_lane_obj_indices.reserve(objects.objects.size()); + target_lane_obj_indices.reserve(objects.objects.size()); + others_obj_indices.reserve(objects.objects.size()); + + const auto & longest_path = lane_change_paths.front(); + const auto & current_lanes = longest_path.reference_lanelets; + const auto & target_lanes = longest_path.target_lanelets; + const auto & ego_path = longest_path.path; + + const auto get_basic_polygon = + [](const lanelet::ConstLanelets & lanes, const double start_dist, const double end_dist) { + const auto polygon_3d = lanelet::utils::getPolygonFromArcLength(lanes, start_dist, end_dist); + return lanelet::utils::to2D(polygon_3d).basicPolygon(); + }; + const auto arc = lanelet::utils::getArcCoordinates(current_lanes, current_pose); + const auto current_polygon = + get_basic_polygon(current_lanes, arc.length, arc.length + forward_path_length); + const auto target_polygon = + get_basic_polygon(target_lanes, 0.0, std::numeric_limits::max()); + LineString2d ego_path_linestring; + ego_path_linestring.reserve(ego_path.points.size()); + for (const auto & pt : ego_path.points) { + const auto & position = pt.point.pose.position; + boost::geometry::append(ego_path_linestring, Point2d(position.x, position.y)); + } + + for (size_t i = 0; i < objects.objects.size(); ++i) { + const auto & obj = objects.objects.at(i); + + if (ignore_unknown_obj) { + const auto t = getHighestProbLabel(obj.classification); + if (t == ObjectClassification::UNKNOWN) { + continue; + } + } + + Polygon2d obj_polygon; + if (!util::calcObjectPolygon(obj, &obj_polygon)) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("behavior_path_planner").get_child("lane_change"), + "Failed to calcObjectPolygon...!!!"); + continue; + } + + if (boost::geometry::intersects(current_polygon, obj_polygon)) { + const double distance = boost::geometry::distance(obj_polygon, ego_path_linestring); + + if (distance < filter_width) { + current_lane_obj_indices.push_back(i); + continue; + } + } + + const bool is_intersect_with_target = boost::geometry::intersects(target_polygon, obj_polygon); + if (is_intersect_with_target) { + target_lane_obj_indices.push_back(i); + continue; + } + + const bool is_intersect_with_backward = std::invoke([&]() { + for (const auto & ll : target_backward_lanes) { + const bool is_intersect_with_backward = + boost::geometry::intersects(ll.polygon2d().basicPolygon(), obj_polygon); + if (is_intersect_with_backward) { + target_lane_obj_indices.push_back(i); + return true; + } + } + return false; + }); + + if (!is_intersect_with_backward) { + others_obj_indices.push_back(i); + } + } + + return {current_lane_obj_indices, target_lane_obj_indices, others_obj_indices}; +} +double calcLateralBufferForFiltering(const double vehicle_width, const double lateral_buffer) +{ + return lateral_buffer + 0.5 * vehicle_width; +} + } // namespace behavior_path_planner::lane_change_utils From 856da10770a29bcb56553e88f7f254eec8f658f3 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Tue, 28 Feb 2023 12:06:05 +0900 Subject: [PATCH 3/3] perf(lane_change): only compute interpolate ego once (#2839) * fix(lane_change): improvement on isLaneChangePathSafe computation Signed-off-by: Muhammad Zulfaqar Azmi * re-add logic to ignore parked vehicle Signed-off-by: Muhammad Zulfaqar Azmi * fix the duration issues and some code editing Signed-off-by: Muhammad Zulfaqar Azmi --------- Signed-off-by: Muhammad Zulfaqar Azmi Signed-off-by: tomoya.kimura --- .../behavior_path_planner/utilities.hpp | 22 +++--- .../src/scene_module/lane_change/util.cpp | 54 +++++++------ .../behavior_path_planner/src/utilities.cpp | 76 ++++++++++++------- 3 files changed, 91 insertions(+), 61 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp index 6292867db7053..17c9b27e60e80 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp @@ -17,6 +17,7 @@ #include "behavior_path_planner/data_manager.hpp" #include "behavior_path_planner/debug_utilities.hpp" +#include "behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp" #include "behavior_path_planner/scene_module/pull_out/pull_out_path.hpp" #include @@ -513,19 +514,20 @@ bool isLateralDistanceEnough( const double & relative_lateral_distance, const double & lateral_distance_threshold); bool isSafeInLaneletCollisionCheck( - const Pose & ego_current_pose, const Twist & ego_current_twist, - const PredictedPath & ego_predicted_path, const VehicleInfo & ego_info, - const double check_start_time, const double check_end_time, const double check_time_resolution, - const PredictedObject & target_object, const PredictedPath & target_object_path, - const BehaviorPathPlannerParameters & common_parameters, const double front_decel, + const std::vector> & interpolated_ego, + const Twist & ego_current_twist, const std::vector & check_duration, + const double prepare_duration, const PredictedObject & target_object, + const PredictedPath & target_object_path, const BehaviorPathPlannerParameters & common_parameters, + const double prepare_phase_ignore_target_speed_thresh, const double front_decel, const double rear_decel, Pose & ego_pose_before_collision, CollisionCheckDebug & debug); bool isSafeInFreeSpaceCollisionCheck( - const Pose & ego_current_pose, const Twist & ego_current_twist, - const PredictedPath & ego_predicted_path, const VehicleInfo & ego_info, - const double check_start_time, const double check_end_time, const double check_time_resolution, - const PredictedObject & target_object, const BehaviorPathPlannerParameters & common_parameters, - const double front_decel, const double rear_decel, CollisionCheckDebug & debug); + const std::vector> & interpolated_ego, + const Twist & ego_current_twist, const std::vector & check_duration, + const double prepare_duration, const PredictedObject & target_object, + const BehaviorPathPlannerParameters & common_parameters, + const double prepare_phase_ignore_target_speed_thresh, const double front_decel, + const double rear_decel, CollisionCheckDebug & debug); bool checkPathRelativeAngle(const PathWithLaneId & path, const double angle_threshold); diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp index 6341ce00f210c..0415763ef535b 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp @@ -504,18 +504,16 @@ bool isLaneChangePathSafe( } const double time_resolution = lane_change_parameters.prediction_time_resolution; - const auto & lane_change_prepare_duration = lane_change_path.duration.prepare; - const auto & enable_collision_check_at_prepare_phase = + const auto check_at_prepare_phase = lane_change_parameters.enable_collision_check_at_prepare_phase; - const auto & lane_changing_safety_check_duration = lane_change_path.duration.lane_changing; - const double check_end_time = lane_change_prepare_duration + lane_changing_safety_check_duration; + + const double check_start_time = check_at_prepare_phase ? 0.0 : lane_change_path.duration.prepare; + const double check_end_time = lane_change_path.duration.sum(); const double min_lc_speed{lane_change_parameters.minimum_lane_change_velocity}; const auto vehicle_predicted_path = util::convertToPredictedPath( path, current_twist, current_pose, static_cast(current_seg_idx), check_end_time, time_resolution, acceleration, min_lc_speed); - const auto prepare_phase_ignore_target_speed_thresh = - lane_change_parameters.prepare_phase_ignore_target_speed_thresh; const auto & vehicle_info = common_parameters.vehicle_info; auto in_lane_object_indices = dynamic_objects_indices.target_lane; @@ -548,26 +546,42 @@ bool isLaneChangePathSafe( } }; + const auto reserve_size = + static_cast((check_end_time - check_start_time) / time_resolution); + std::vector check_durations{}; + std::vector> interpolated_ego{}; + check_durations.reserve(reserve_size); + interpolated_ego.reserve(reserve_size); + + { + Pose expected_ego_pose = current_pose; + for (double t = check_start_time; t < check_end_time; t += time_resolution) { + std::string failed_reason; + tier4_autoware_utils::Polygon2d ego_polygon; + [[maybe_unused]] const auto get_ego_info = util::getEgoExpectedPoseAndConvertToPolygon( + current_pose, vehicle_predicted_path, ego_polygon, t, vehicle_info, expected_ego_pose, + failed_reason); + check_durations.push_back(t); + interpolated_ego.emplace_back(expected_ego_pose, ego_polygon); + } + } + for (const auto & i : in_lane_object_indices) { const auto & obj = dynamic_objects->objects.at(i); - const auto object_speed = - util::l2Norm(obj.kinematics.initial_twist_with_covariance.twist.linear); - const double check_start_time = (enable_collision_check_at_prepare_phase && - (object_speed > prepare_phase_ignore_target_speed_thresh)) - ? 0.0 - : lane_change_prepare_duration; auto current_debug_data = assignDebugData(obj); const auto predicted_paths = util::getPredictedPathFromObj(obj, lane_change_parameters.use_all_predicted_path); for (const auto & obj_path : predicted_paths) { if (!util::isSafeInLaneletCollisionCheck( - current_pose, current_twist, vehicle_predicted_path, vehicle_info, check_start_time, - check_end_time, time_resolution, obj, obj_path, common_parameters, front_decel, + interpolated_ego, current_twist, check_durations, lane_change_path.duration.prepare, + obj, obj_path, common_parameters, + lane_change_parameters.prepare_phase_ignore_target_speed_thresh, front_decel, rear_decel, ego_pose_before_collision, current_debug_data.second)) { appendDebugInfo(current_debug_data, false); return false; } } + appendDebugInfo(current_debug_data, true); } if (!lane_change_parameters.use_predicted_path_outside_lanelet) { @@ -582,16 +596,10 @@ bool isLaneChangePathSafe( const auto predicted_paths = util::getPredictedPathFromObj(obj, lane_change_parameters.use_all_predicted_path); - const auto object_speed = - util::l2Norm(obj.kinematics.initial_twist_with_covariance.twist.linear); - const double check_start_time = (enable_collision_check_at_prepare_phase && - (object_speed > prepare_phase_ignore_target_speed_thresh)) - ? 0.0 - : lane_change_prepare_duration; if (!util::isSafeInFreeSpaceCollisionCheck( - current_pose, current_twist, vehicle_predicted_path, vehicle_info, check_start_time, - check_end_time, time_resolution, obj, common_parameters, front_decel, rear_decel, - current_debug_data.second)) { + interpolated_ego, current_twist, check_durations, lane_change_path.duration.prepare, obj, + common_parameters, lane_change_parameters.prepare_phase_ignore_target_speed_thresh, + front_decel, rear_decel, current_debug_data.second)) { appendDebugInfo(current_debug_data, false); return false; } diff --git a/planning/behavior_path_planner/src/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp index d974da47d9cbd..9d95d12b5b8fa 100644 --- a/planning/behavior_path_planner/src/utilities.cpp +++ b/planning/behavior_path_planner/src/utilities.cpp @@ -2466,29 +2466,38 @@ bool isLateralDistanceEnough( } bool isSafeInLaneletCollisionCheck( - const Pose & ego_current_pose, const Twist & ego_current_twist, - const PredictedPath & ego_predicted_path, const VehicleInfo & ego_info, - const double check_start_time, const double check_end_time, const double check_time_resolution, - const PredictedObject & target_object, const PredictedPath & target_object_path, - const BehaviorPathPlannerParameters & common_parameters, const double front_decel, + const std::vector> & interpolated_ego, + const Twist & ego_current_twist, const std::vector & check_duration, + const double prepare_duration, const PredictedObject & target_object, + const PredictedPath & target_object_path, const BehaviorPathPlannerParameters & common_parameters, + const double prepare_phase_ignore_target_speed_thresh, const double front_decel, const double rear_decel, Pose & ego_pose_before_collision, CollisionCheckDebug & debug) { - const auto lerp_path_reserve = (check_end_time - check_start_time) / check_time_resolution; - if (lerp_path_reserve > 1e-3) { - debug.lerped_path.reserve(static_cast(lerp_path_reserve)); - } + debug.lerped_path.reserve(check_duration.size()); Pose expected_obj_pose = target_object.kinematics.initial_pose_with_covariance.pose; - Pose expected_ego_pose = ego_current_pose; - for (double t = check_start_time; t < check_end_time; t += check_time_resolution) { + const auto & object_twist = target_object.kinematics.initial_twist_with_covariance.twist; + const auto object_speed = l2Norm(object_twist.linear); + const auto ignore_check_at_time = [&](const double current_time) { + return ( + (current_time < prepare_duration) && + (object_speed < prepare_phase_ignore_target_speed_thresh)); + }; + + for (size_t i = 0; i < check_duration.size(); ++i) { + const auto current_time = check_duration.at(i); + + if (ignore_check_at_time(current_time)) { + continue; + } + tier4_autoware_utils::Polygon2d obj_polygon; [[maybe_unused]] const auto get_obj_info = util::getObjectExpectedPoseAndConvertToPolygon( - target_object_path, target_object, obj_polygon, t, expected_obj_pose, debug.failed_reason); - - tier4_autoware_utils::Polygon2d ego_polygon; - [[maybe_unused]] const auto get_ego_info = util::getEgoExpectedPoseAndConvertToPolygon( - ego_current_pose, ego_predicted_path, ego_polygon, t, ego_info, expected_ego_pose, + target_object_path, target_object, obj_polygon, current_time, expected_obj_pose, debug.failed_reason); + const auto & ego_info = interpolated_ego.at(i); + auto expected_ego_pose = ego_info.first; + const auto & ego_polygon = ego_info.second; debug.ego_polygon = ego_polygon; debug.obj_polygon = obj_polygon; @@ -2504,7 +2513,6 @@ bool isSafeInLaneletCollisionCheck( debug.expected_ego_pose = expected_ego_pose; debug.expected_obj_pose = expected_obj_pose; - const auto & object_twist = target_object.kinematics.initial_twist_with_covariance.twist; if (!util::hasEnoughDistance( expected_ego_pose, ego_current_twist, expected_obj_pose, object_twist, common_parameters, front_decel, rear_decel, debug)) { @@ -2517,22 +2525,34 @@ bool isSafeInLaneletCollisionCheck( } bool isSafeInFreeSpaceCollisionCheck( - const Pose & ego_current_pose, const Twist & ego_current_twist, - const PredictedPath & ego_predicted_path, const VehicleInfo & ego_info, - const double check_start_time, const double check_end_time, const double check_time_resolution, - const PredictedObject & target_object, const BehaviorPathPlannerParameters & common_parameters, - const double front_decel, const double rear_decel, CollisionCheckDebug & debug) + const std::vector> & interpolated_ego, + const Twist & ego_current_twist, const std::vector & check_duration, + const double prepare_duration, const PredictedObject & target_object, + const BehaviorPathPlannerParameters & common_parameters, + const double prepare_phase_ignore_target_speed_thresh, const double front_decel, + const double rear_decel, CollisionCheckDebug & debug) { tier4_autoware_utils::Polygon2d obj_polygon; if (!util::calcObjectPolygon(target_object, &obj_polygon)) { return false; } - Pose expected_ego_pose = ego_current_pose; - for (double t = check_start_time; t < check_end_time; t += check_time_resolution) { - tier4_autoware_utils::Polygon2d ego_polygon; - [[maybe_unused]] const auto get_ego_info = util::getEgoExpectedPoseAndConvertToPolygon( - ego_current_pose, ego_predicted_path, ego_polygon, t, ego_info, expected_ego_pose, - debug.failed_reason); + + const auto & object_twist = target_object.kinematics.initial_twist_with_covariance.twist; + const auto object_speed = l2Norm(object_twist.linear); + const auto ignore_check_at_time = [&](const double current_time) { + return ( + (current_time < prepare_duration) && + (object_speed < prepare_phase_ignore_target_speed_thresh)); + }; + for (size_t i = 0; i < check_duration.size(); ++i) { + const auto current_time = check_duration.at(i); + + if (ignore_check_at_time(current_time)) { + continue; + } + const auto & ego_info = interpolated_ego.at(i); + auto expected_ego_pose = ego_info.first; + const auto & ego_polygon = ego_info.second; debug.ego_polygon = ego_polygon; debug.obj_polygon = obj_polygon;