feat(behavior_path_planner_common): add safety target object located …

…on shoulder lane (autowarefoundation#6839)

feat(behavior_path_planner_common): add target object located on shoulder lane

Signed-off-by: kyoichi-sugahara <[email protected]>

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;
 }

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 <vector>
 
 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<ExtendedPredictedObject> 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