feat(goal_planner): safety check with only parking path (autowarefoun…

…dation#9293) Signed-off-by: kosuke55 <[email protected]>
tier4 · Nov 27, 2024 · 84c7193 · 84c7193
1 parent 8cc9efe
commit 84c7193
Show file tree

Hide file tree

Showing 3 changed files with 29 additions and 5 deletions.
diff --git a/...avior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/...avior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp
@@ -2158,6 +2158,10 @@ bool GoalPlannerModule::isSafePath(
   const std::shared_ptr<ObjectsFilteringParams> & objects_filtering_params,
   const std::shared_ptr<SafetyCheckParams> & safety_check_params) const
 {
+  using autoware::behavior_path_planner::utils::path_safety_checker::createPredictedPath;
+  using autoware::behavior_path_planner::utils::path_safety_checker::
+    filterPredictedPathAfterTargetPose;
+
   if (!found_pull_over_path || !pull_over_path_opt) {
     return false;
   }
@@ -2175,7 +2179,6 @@ bool GoalPlannerModule::isSafePath(
   const auto pull_over_lanes = goal_planner_utils::getPullOverLanes(
     *route_handler, left_side_parking_, parameters.backward_goal_search_length,
     parameters.forward_goal_search_length);
-  const size_t ego_seg_idx = planner_data->findEgoSegmentIndex(current_pull_over_path.points);
   const std::pair<double, double> terminal_velocity_and_accel =
     pull_over_path.getPairsTerminalVelocityAndAccel();
   RCLCPP_DEBUG(
@@ -2186,10 +2189,12 @@ bool GoalPlannerModule::isSafePath(
   // TODO(Sugahara): shoule judge is_object_front properly
   const bool is_object_front = true;
   const bool limit_to_max_velocity = true;
-  const auto ego_predicted_path =
-    autoware::behavior_path_planner::utils::path_safety_checker::createPredictedPath(
-      ego_predicted_path_params, current_pull_over_path.points, current_pose, current_velocity,
-      ego_seg_idx, is_object_front, limit_to_max_velocity);
+  const auto ego_seg_idx = planner_data->findEgoIndex(current_pull_over_path.points);
+  const auto ego_predicted_path_from_current_pose = createPredictedPath(
+    ego_predicted_path_params, current_pull_over_path.points, current_pose, current_velocity,
+    ego_seg_idx, is_object_front, limit_to_max_velocity);
+  const auto ego_predicted_path = filterPredictedPathAfterTargetPose(
+    ego_predicted_path_from_current_pose, pull_over_path.start_pose());
 
   // ==========================================================================================
   // if ego is before the entry of pull_over_lanes, the beginning of the safety check area

diff --git a/.../include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp b/.../include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp
@@ -105,6 +105,9 @@ ExtendedPredictedObjects filterObjectPredictedPathByTimeHorizon(
   const ExtendedPredictedObjects & objects, const double time_horizon,
   const bool check_all_predicted_path);
 
+std::vector<PoseWithVelocityStamped> filterPredictedPathAfterTargetPose(
+  const std::vector<PoseWithVelocityStamped> & path, const Pose & target_pose);
+
 bool checkSafetyWithRSS(
   const PathWithLaneId & planned_path,
   const std::vector<PoseWithVelocityStamped> & ego_predicted_path,

diff --git a/...nner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp b/...nner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp
@@ -489,6 +489,22 @@ ExtendedPredictedObjects filterObjectPredictedPathByTimeHorizon(
   return filtered_objects;
 }
 
+std::vector<PoseWithVelocityStamped> filterPredictedPathAfterTargetPose(
+  const std::vector<PoseWithVelocityStamped> & path, const Pose & target_pose)
+{
+  std::vector<PoseWithVelocityStamped> filtered_path;
+
+  const auto target_idx =
+    std::min_element(path.begin(), path.end(), [&target_pose](const auto & a, const auto & b) {
+      return calcDistance2d(a.pose.position, target_pose.position) <
+             calcDistance2d(b.pose.position, target_pose.position);
+    });
+
+  std::copy(target_idx, path.end(), std::back_inserter(filtered_path));
+
+  return filtered_path;
+};
+
 bool checkSafetyWithRSS(
   const PathWithLaneId & planned_path,
   const std::vector<PoseWithVelocityStamped> & ego_predicted_path,