refactor(behavior_path_planner): generalize safety check function (#3968

) * temp Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * temp Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * update Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * fix(compare_map_segmentation): initialize timer_callback_group_ (#3914) Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp> * fix(behavior_path_planner): small refactoring for resamplePathWithSpline (#3922) Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * fix(behavior_path_planner): avoid invalid access (#3923) Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp> * temo Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * update Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * update Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * update Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * update Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * update Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * create PredictedPath debug marker Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * update Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * update Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * update Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * delete debug print Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * refactoring Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * refactor Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * refactoring Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * refactoring Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * update Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * update Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * add param for predicted path generation Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * refactoring Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * temp Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * generalize safety check function Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * delete unnecessary difference Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * modify call refactored function Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * Update planning/behavior_path_planner/include/behavior_path_planner/utils/safety_check.hpp Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> * Update planning/behavior_path_planner/src/utils/safety_check.cpp Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> --------- Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp> Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> Co-authored-by: Tomoya Kimura <tomoya.kimura@tier4.jp> Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com>
autowarefoundation · Jun 14, 2023 · 9e7dc27 · 9e7dc27
1 parent b8ddaf5
commit 9e7dc27
Show file tree

Hide file tree

Showing 3 changed files with 43 additions and 25 deletions.
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/safety_check.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/safety_check.hpp
@@ -78,16 +78,31 @@ double calcMinimumLongitudinalLength(
 /**
  * @brief Iterate the points in the ego and target's predicted path and
  *        perform safety check for each of the iterated points.
+ * @param planned_path The predicted path of the ego vehicle.
+ * @param interpolated_ego A vector of pairs of ego vehicle's pose and its polygon at each moment in
+ * the future.
+ * @param ego_current_velocity Current velocity of the ego vehicle.
+ * @param check_duration The vector of times in the future at which safety check is
+ * performed.(relative time in sec from the current time)
+ * @param target_object The predicted object to check collision with.
+ * @param target_object_path The predicted path of the target object.
+ * @param common_parameters The common parameters used in behavior path planner.
+ * @param front_object_deceleration The deceleration of the object in the front.(used in RSS)
+ * @param rear_object_deceleration The deceleration of the object in the rear.(used in RSS)
+ * @param debug The debug information for collision checking.
+ * @param prepare_duration The duration to prepare before shifting lane.
+ * @param velocity_threshold_for_prepare_duration The threshold for the target velocity to
+ * ignore during preparation phase.
  * @return true if distance is safe.
  */
 bool isSafeInLaneletCollisionCheck(
-  const PathWithLaneId & path,
-  const std::vector<std::pair<Pose, tier4_autoware_utils::Polygon2d>> & interpolated_ego,
-  const Twist & ego_current_twist, const std::vector<double> & 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, CollisionCheckDebug & debug);
+  const PathWithLaneId & planned_path,
+  const std::vector<std::pair<Pose, tier4_autoware_utils::Polygon2d>> & predicted_ego_poses,
+  const double ego_current_velocity, const std::vector<double> & check_duration,
+  const PredictedObject & target_object, const PredictedPath & target_object_path,
+  const BehaviorPathPlannerParameters & common_parameters, const double front_object_deceleration,
+  const double rear_object_deceleration, CollisionCheckDebug & debug,
+  const double prepare_duration = 0.0, const double velocity_threshold_for_prepare_duration = 0.0);
 
 /**
  * @brief Iterate the points in the ego and target's predicted path and

diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp
@@ -433,10 +433,10 @@ PathSafetyStatus isLaneChangePathSafe(
       utils::getPredictedPathFromObj(obj, lane_change_parameter.use_all_predicted_path);
     for (const auto & obj_path : obj_predicted_paths) {
       if (!utils::safety_check::isSafeInLaneletCollisionCheck(
-            path, interpolated_ego, current_twist, check_durations,
-            lane_change_path.duration.prepare, obj, obj_path, common_parameter,
-            lane_change_parameter.prepare_segment_ignore_object_velocity_thresh, front_decel,
-            rear_decel, current_debug_data.second)) {
+            path, interpolated_ego, current_twist.linear.x, check_durations, obj, obj_path,
+            common_parameter, front_decel, rear_decel, current_debug_data.second,
+            lane_change_path.duration.prepare,
+            lane_change_parameter.prepare_segment_ignore_object_velocity_thresh)) {
         path_safety_status.is_safe = false;
         appendDebugInfo(current_debug_data, false);
         if (isObjectIndexIncluded(i, dynamic_objects_indices.target_lane)) {

diff --git a/planning/behavior_path_planner/src/utils/safety_check.cpp b/planning/behavior_path_planner/src/utils/safety_check.cpp
@@ -149,26 +149,28 @@ double calcMinimumLongitudinalLength(
 }
 
 bool isSafeInLaneletCollisionCheck(
-  const PathWithLaneId & path,
-  const std::vector<std::pair<Pose, tier4_autoware_utils::Polygon2d>> & interpolated_ego,
-  const Twist & ego_current_twist, const std::vector<double> & 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_velocity_thresh, const double front_object_deceleration,
-  const double rear_object_deceleration, CollisionCheckDebug & debug)
+  const PathWithLaneId & planned_path,
+  const std::vector<std::pair<Pose, tier4_autoware_utils::Polygon2d>> & predicted_ego_poses,
+  const double ego_current_velocity, const std::vector<double> & check_duration,
+  const PredictedObject & target_object, const PredictedPath & target_object_path,
+  const BehaviorPathPlannerParameters & common_parameters, const double front_object_deceleration,
+  const double rear_object_deceleration, CollisionCheckDebug & debug, const double prepare_duration,
+  const double velocity_threshold_for_prepare_duration)
 {
   debug.lerped_path.reserve(check_duration.size());
 
-  const auto & ego_velocity = ego_current_twist.linear.x;
+  const auto & ego_velocity = ego_current_velocity;
   const auto & object_velocity =
     target_object.kinematics.initial_twist_with_covariance.twist.linear.x;
 
   for (size_t i = 0; i < check_duration.size(); ++i) {
     const auto current_time = check_duration.at(i);
 
-    if (
-      current_time < prepare_duration &&
-      object_velocity < prepare_phase_ignore_target_velocity_thresh) {
+    // ignore low velocity object during prepare duration
+    const bool prepare_phase = current_time < prepare_duration;
+    const bool ignore_target_velocity_during_prepare_phase =
+      object_velocity < velocity_threshold_for_prepare_duration;
+    if (prepare_phase && ignore_target_velocity_during_prepare_phase) {
       continue;
     }
 
@@ -178,8 +180,8 @@ bool isSafeInLaneletCollisionCheck(
     }
 
     const auto obj_polygon = tier4_autoware_utils::toPolygon2d(*obj_pose, target_object.shape);
-    const auto & ego_pose = interpolated_ego.at(i).first;
-    const auto & ego_polygon = interpolated_ego.at(i).second;
+    const auto & ego_pose = predicted_ego_poses.at(i).first;
+    const auto & ego_polygon = predicted_ego_poses.at(i).second;
 
     // check overlap
     debug.ego_polygon = ego_polygon;
@@ -191,7 +193,7 @@ bool isSafeInLaneletCollisionCheck(
 
     // compute which one is at the front of the other
     const bool is_object_front =
-      isTargetObjectFront(path, ego_pose, common_parameters.vehicle_info, obj_polygon);
+      isTargetObjectFront(planned_path, ego_pose, common_parameters.vehicle_info, obj_polygon);
     const auto & [front_object_velocity, rear_object_velocity] =
       is_object_front ? std::make_pair(object_velocity, ego_velocity)
                       : std::make_pair(ego_velocity, object_velocity);
@@ -221,6 +223,7 @@ bool isSafeInLaneletCollisionCheck(
     debug.expected_obj_pose = *obj_pose;
     debug.is_front = is_object_front;
 
+    // check overlap with extended polygon
     if (boost::geometry::overlaps(extended_ego_polygon, extended_obj_polygon)) {
       debug.failed_reason = "overlap_extended_polygon";
       return false;