refactor(lane_change): refactor extended object safety check

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp

@@ -169,12 +169,13 @@ class NormalLaneChange : public LaneChangeBase
   bool has_collision_with_decel_patterns(
     const LaneChangePath & lane_change_path, const ExtendedPredictedObjects & objects,
     const size_t deceleration_sampling_num, const RSSparams & rss_param,
-    CollisionCheckDebugMap & debug_data) const;
+    const bool check_prepare_phase, CollisionCheckDebugMap & debug_data) const;
 
   bool is_collided(
-    const PathWithLaneId & lane_change_path, const ExtendedPredictedObject & obj,
+    const LaneChangePath & lane_change_path, const ExtendedPredictedObject & obj,
     const std::vector<PoseWithVelocityStamped> & ego_predicted_path,
-    const RSSparams & selected_rss_param, CollisionCheckDebugMap & debug_data) const;
+    const RSSparams & selected_rss_param, const bool check_prepare_phase,
+    CollisionCheckDebugMap & debug_data) const;
 
   double get_max_velocity_for_safety_check() const;
 

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp

@@ -145,7 +145,7 @@ lanelet::BasicPolygon2d create_polygon(
 
 ExtendedPredictedObject transform(
   const PredictedObject & object, const BehaviorPathPlannerParameters & common_parameters,
-  const LaneChangeParameters & lane_change_parameters, const bool check_at_prepare_phase);
+  const LaneChangeParameters & lane_change_parameters);
 
 bool is_collided_polygons_in_lanelet(
   const std::vector<Polygon2d> & collided_polygons, const lanelet::BasicPolygon2d & lanes_polygon);
@@ -249,8 +249,7 @@ bool is_before_terminal(
 double calc_angle_to_lanelet_segment(const lanelet::ConstLanelets & lanelets, const Pose & pose);
 
 ExtendedPredictedObjects transform_to_extended_objects(
-  const CommonDataPtr & common_data_ptr, const std::vector<PredictedObject> & objects,
-  const bool check_prepare_phase);
+  const CommonDataPtr & common_data_ptr, const std::vector<PredictedObject> & objects);
 
 double get_distance_to_next_regulatory_element(
   const CommonDataPtr & common_data_ptr, const bool ignore_crosswalk = false,

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp

@@ -1,4 +1,4 @@
 // Copyright 2023 TIER IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -1006,17 +1006,16 @@
       return is_within_vel_th(object) && ahead_of_ego;
     });
 
-  const auto is_check_prepare_phase = check_prepare_phase();
   const auto target_lane_leading_extended_objects =
     utils::lane_change::transform_to_extended_objects(
-      common_data_ptr_, filtered_by_lanes_objects.target_lane_leading, is_check_prepare_phase);
+      common_data_ptr_, filtered_by_lanes_objects.target_lane_leading);
   const auto target_lane_trailing_extended_objects =
     utils::lane_change::transform_to_extended_objects(
-      common_data_ptr_, filtered_by_lanes_objects.target_lane_trailing, is_check_prepare_phase);
+      common_data_ptr_, filtered_by_lanes_objects.target_lane_trailing);
   const auto current_lane_extended_objects = utils::lane_change::transform_to_extended_objects(
-    common_data_ptr_, filtered_by_lanes_objects.current_lane, is_check_prepare_phase);
+    common_data_ptr_, filtered_by_lanes_objects.current_lane);
   const auto other_lane_extended_objects = utils::lane_change::transform_to_extended_objects(
-    common_data_ptr_, filtered_by_lanes_objects.other_lane, is_check_prepare_phase);
+    common_data_ptr_, filtered_by_lanes_objects.other_lane);
 
   FilteredByLanesExtendedObjects lane_change_target_objects(
     current_lane_extended_objects, target_lane_leading_extended_objects,
@@ -1856,10 +1855,13 @@
     return {is_safe, !is_object_behind_ego};
   }
 
+  const auto is_check_prepare_phase = check_prepare_phase();
+
   const auto all_decel_pattern_has_collision =
     [&](const utils::path_safety_checker::ExtendedPredictedObjects & objects) -> bool {
     return has_collision_with_decel_patterns(
-      lane_change_path, objects, deceleration_sampling_num, rss_params, debug_data);
+      lane_change_path, objects, deceleration_sampling_num, rss_params, is_check_prepare_phase,
+      debug_data);
   };
 
   if (all_decel_pattern_has_collision(collision_check_objects.trailing)) {
@@ -1876,7 +1878,7 @@
 bool NormalLaneChange::has_collision_with_decel_patterns(
   const LaneChangePath & lane_change_path, const ExtendedPredictedObjects & objects,
   const size_t deceleration_sampling_num, const RSSparams & rss_param,
-  CollisionCheckDebugMap & debug_data) const
+  const bool check_prepare_phase, CollisionCheckDebugMap & debug_data) const
 {
   if (objects.empty()) {
     return false;
@@ -1922,48 +1924,64 @@
             ? lane_change_parameters_->rss_params_for_parked
             : rss_param;
         return is_collided(
-          lane_change_path.path, obj, ego_predicted_path, selected_rss_param, debug_data);
+          lane_change_path, obj, ego_predicted_path, selected_rss_param, check_prepare_phase,
+          debug_data);
       });
     });
 
   return all_collided;
 }
 
 bool NormalLaneChange::is_collided(
-  const PathWithLaneId & lane_change_path, const ExtendedPredictedObject & obj,
+  const LaneChangePath & lane_change_path, const ExtendedPredictedObject & obj,
   const std::vector<PoseWithVelocityStamped> & ego_predicted_path,
-  const RSSparams & selected_rss_param, CollisionCheckDebugMap & debug_data) const
+  const RSSparams & selected_rss_param, const bool check_prepare_phase,
+  CollisionCheckDebugMap & debug_data) const
 {
   constexpr auto is_collided{true};
 
-  if (lane_change_path.points.empty()) {
+  if (lane_change_path.path.points.empty()) {
     return !is_collided;
   }
 
   if (ego_predicted_path.empty()) {
     return !is_collided;
   }
 
   const auto & lanes_polygon_ptr = common_data_ptr_->lanes_polygon_ptr;
   const auto & current_polygon = lanes_polygon_ptr->current;
   const auto & expanded_target_polygon = lanes_polygon_ptr->target;
 
   if (current_polygon.empty() || expanded_target_polygon.empty()) {
     return !is_collided;
   }
 
   constexpr auto is_safe{true};
   auto current_debug_data = utils::path_safety_checker::createObjectDebug(obj);
   constexpr auto collision_check_yaw_diff_threshold{M_PI};
   constexpr auto hysteresis_factor{1.0};
   const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj(
     obj, lane_change_parameters_->use_all_predicted_path);
   const auto safety_check_max_vel = get_max_velocity_for_safety_check();
   const auto & bpp_param = *common_data_ptr_->bpp_param_ptr;
 
+  const double velocity_threshold = lane_change_parameters_->stopped_object_velocity_threshold;
+  const double prepare_duration = lane_change_path.info.duration.prepare;
+  const double start_time = check_prepare_phase ? 0.0 : prepare_duration;
+
   for (const auto & obj_path : obj_predicted_paths) {
+    utils::path_safety_checker::PredictedPathWithPolygon predicted_obj_path;
+    predicted_obj_path.confidence = obj_path.confidence;
+    std::copy_if(
+      obj_path.path.begin(), obj_path.path.end(), std::back_inserter(predicted_obj_path.path),
+      [&](const auto & entry) {
+        return !(
+          entry.time < start_time ||
+          (entry.time < prepare_duration && entry.velocity < velocity_threshold));
+      });
+
     const auto collided_polygons = utils::path_safety_checker::get_collided_polygons(
-      lane_change_path, ego_predicted_path, obj, obj_path, bpp_param.vehicle_info,
+      lane_change_path.path, ego_predicted_path, obj, predicted_obj_path, bpp_param.vehicle_info,
       selected_rss_param, hysteresis_factor, safety_check_max_vel,
       collision_check_yaw_diff_threshold, current_debug_data.second);
 

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp

@@ -1,4 +1,4 @@
 // Copyright 2021 Tier IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -914,14 +914,11 @@
 ExtendedPredictedObject transform(
   const PredictedObject & object,
   [[maybe_unused]] const BehaviorPathPlannerParameters & common_parameters,
-  const LaneChangeParameters & lane_change_parameters, const bool check_at_prepare_phase)
+  const LaneChangeParameters & lane_change_parameters)
 {
   ExtendedPredictedObject extended_object(object);
 
   const auto & time_resolution = lane_change_parameters.prediction_time_resolution;
-  const auto & prepare_duration = lane_change_parameters.lane_change_prepare_duration;
-  const auto & velocity_threshold = lane_change_parameters.stopped_object_velocity_threshold;
-  const auto start_time = check_at_prepare_phase ? 0.0 : prepare_duration;
   const double obj_vel_norm =
     std::hypot(extended_object.initial_twist.linear.x, extended_object.initial_twist.linear.y);
 
@@ -933,11 +930,8 @@
     extended_object.predicted_paths.at(i).confidence = path.confidence;
 
     // create path
-    for (double t = start_time; t < end_time + std::numeric_limits<double>::epsilon();
+    for (double t = 0.0; t < end_time + std::numeric_limits<double>::epsilon();
          t += time_resolution) {
-      if (t < prepare_duration && obj_vel_norm < velocity_threshold) {
-        continue;
-      }
       const auto obj_pose = autoware::object_recognition_utils::calcInterpolatedPose(path, t);
       if (obj_pose) {
         const auto obj_polygon = autoware::universe_utils::toPolygon2d(*obj_pose, object.shape);
@@ -1163,8 +1157,7 @@
 }
 
 ExtendedPredictedObjects transform_to_extended_objects(
-  const CommonDataPtr & common_data_ptr, const std::vector<PredictedObject> & objects,
-  const bool check_prepare_phase)
+  const CommonDataPtr & common_data_ptr, const std::vector<PredictedObject> & objects)
 {
   ExtendedPredictedObjects extended_objects;
   extended_objects.reserve(objects.size());
@@ -1173,7 +1166,7 @@
   const auto & lc_param = *common_data_ptr->lc_param_ptr;
   std::transform(
     objects.begin(), objects.end(), std::back_inserter(extended_objects), [&](const auto & object) {
-      return utils::lane_change::transform(object, bpp_param, lc_param, check_prepare_phase);
+      return utils::lane_change::transform(object, bpp_param, lc_param);
     });
 
   return extended_objects;