diff --git a/planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp b/planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp index 188c50993e4ae..c2f9a8796c483 100644 --- a/planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp +++ b/planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp @@ -330,8 +330,9 @@ TargetObjectsOnLane createTargetObjectsOnLane( lanelet::ConstLanelets all_left_lanelets; lanelet::ConstLanelets all_right_lanelets; + // TODO(sugahara): consider right side parking and define right shoulder lanelets + lanelet::ConstLanelets all_left_shoulder_lanelets; - // Define lambda functions to update left and right lanelets const auto update_left_lanelets = [&](const lanelet::ConstLanelet & target_lane) { const auto left_lanelets = route_handler->getAllLeftSharedLinestringLanelets( target_lane, include_opposite, invert_opposite); @@ -345,13 +346,23 @@ TargetObjectsOnLane createTargetObjectsOnLane( all_right_lanelets.end(), right_lanelets.begin(), right_lanelets.end()); }; - // Update left and right lanelets for each current lane + const auto update_left_shoulder_lanelet = [&](const lanelet::ConstLanelet & target_lane) { + lanelet::ConstLanelet neighbor_shoulder_lane{}; + const bool shoulder_lane_is_found = + route_handler->getLeftShoulderLanelet(target_lane, &neighbor_shoulder_lane); + if (shoulder_lane_is_found) { + all_left_shoulder_lanelets.insert(all_left_shoulder_lanelets.end(), neighbor_shoulder_lane); + } + }; + for (const auto & current_lane : current_lanes) { update_left_lanelets(current_lane); update_right_lanelets(current_lane); + update_left_shoulder_lanelet(current_lane); } TargetObjectsOnLane target_objects_on_lane{}; + const auto append_objects_on_lane = [&](auto & lane_objects, const auto & check_lanes) { std::for_each( filtered_objects.objects.begin(), filtered_objects.objects.end(), [&](const auto & object) { @@ -362,7 +373,7 @@ TargetObjectsOnLane createTargetObjectsOnLane( }); }; - // TODO(Sugahara): Consider shoulder and other lane objects + // TODO(Sugahara): Consider other lane objects if (object_lane_configuration.check_current_lane && !current_lanes.empty()) { append_objects_on_lane(target_objects_on_lane.on_current_lane, current_lanes); } @@ -372,6 +383,9 @@ TargetObjectsOnLane createTargetObjectsOnLane( if (object_lane_configuration.check_right_lane && !all_right_lanelets.empty()) { append_objects_on_lane(target_objects_on_lane.on_right_lane, all_right_lanelets); } + if (object_lane_configuration.check_shoulder_lane && !all_left_shoulder_lanelets.empty()) { + append_objects_on_lane(target_objects_on_lane.on_shoulder_lane, all_left_shoulder_lanelets); + } return target_objects_on_lane; } diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp index f0f0ece3b1b5e..68be0171e1520 100644 --- a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp @@ -17,6 +17,7 @@ #include "behavior_path_planner_common/utils/create_vehicle_footprint.hpp" #include "behavior_path_planner_common/utils/parking_departure/utils.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" +#include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner_common/utils/path_utils.hpp" #include "behavior_path_start_planner_module/util.hpp" #include "motion_utils/trajectory/trajectory.hpp" @@ -37,6 +38,8 @@ #include using behavior_path_planner::utils::parking_departure::initializeCollisionCheckDebugMap; +using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject; +using motion_utils::calcLateralOffset; using motion_utils::calcLongitudinalOffsetPose; using tier4_autoware_utils::calcOffsetPose; @@ -1156,14 +1159,19 @@ bool StartPlannerModule::isSafePath() const const double hysteresis_factor = status_.is_safe_dynamic_objects ? 1.0 : safety_check_params_->hysteresis_factor_expand_rate; - utils::parking_departure::updateSafetyCheckTargetObjectsData( - start_planner_data_, filtered_objects, target_objects_on_lane, ego_predicted_path); - + std::vector merged_target_object; + merged_target_object.reserve( + target_objects_on_lane.on_current_lane.size() + target_objects_on_lane.on_shoulder_lane.size()); + merged_target_object.insert( + merged_target_object.end(), target_objects_on_lane.on_current_lane.begin(), + target_objects_on_lane.on_current_lane.end()); + merged_target_object.insert( + merged_target_object.end(), target_objects_on_lane.on_shoulder_lane.begin(), + target_objects_on_lane.on_shoulder_lane.end()); return behavior_path_planner::utils::path_safety_checker::checkSafetyWithRSS( - pull_out_path, ego_predicted_path, target_objects_on_lane.on_current_lane, - start_planner_data_.collision_check, planner_data_->parameters, - safety_check_params_->rss_params, objects_filtering_params_->use_all_predicted_path, - hysteresis_factor); + pull_out_path, ego_predicted_path, merged_target_object, start_planner_data_.collision_check, + planner_data_->parameters, safety_check_params_->rss_params, + objects_filtering_params_->use_all_predicted_path, hysteresis_factor); } bool StartPlannerModule::isGoalBehindOfEgoInSameRouteSegment() const