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 <kyoichi.sugahara@tier4.jp>

Author: kyoichi-sugahara

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

@@ -16,6 +16,7 @@
 
 #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/debug.hpp"
 #include "behavior_path_start_planner_module/util.hpp"
@@ -39,6 +40,7 @@
 #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;
@@ -1324,10 +1326,18 @@ bool StartPlannerModule::isSafePath() const
     debug_data_.target_objects_on_lane = target_objects_on_lane;
     debug_data_.ego_predicted_path = 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,
-    debug_data_.collision_check, planner_data_->parameters, safety_check_params_->rss_params,
+    pull_out_path, ego_predicted_path, merged_target_object, debug_data_.collision_check,
+    planner_data_->parameters, safety_check_params_->rss_params,
     objects_filtering_params_->use_all_predicted_path, hysteresis_factor);
 }