tmp

Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

Author: zulfaqar-azmi-t4

planning/behavior_path_lane_change_module/src/scene.cpp

@@ -1679,13 +1679,13 @@ bool NormalLaneChange::getLaneChangePaths(
 
         const auto is_safe = std::invoke([&]() {
           const auto safety_check_with_normal_rss = isLaneChangePathSafe(
-            *candidate_path, target_objects, common_data_ptr_->lc_param_ptr->rss_params,
+            *candidate_path, target_objects, common_data_ptr_->lc_param_ptr->rss_params, 1,
             lane_change_debug_.collision_check_objects);
 
           if (!safety_check_with_normal_rss.is_safe && is_stuck) {
             const auto safety_check_with_stuck_rss = isLaneChangePathSafe(
               *candidate_path, target_objects, common_data_ptr_->lc_param_ptr->rss_params_for_stuck,
-              lane_change_debug_.collision_check_objects);
+              1, lane_change_debug_.collision_check_objects);
             return safety_check_with_stuck_rss.is_safe;
           }
 
@@ -2108,7 +2108,8 @@ bool NormalLaneChange::calcAbortPath()
 }
 
 PathSafetyStatus NormalLaneChange::isLaneChangePathSafe(
-  const LaneChangePath & lane_change_path, const ExtendedPredictedObjects & collision_check_objects,
+  const LaneChangePath & lane_change_path,
+  const lane_change::TargetObjects & collision_check_objects,
   const utils::path_safety_checker::RSSparams & rss_params, const size_t deceleration_sampling_num,
   CollisionCheckDebugMap & debug_data) const
 {
@@ -2129,29 +2130,28 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe(
     return {!is_safe, !is_object_behind_ego};
   }
 
-  const double & time_resolution = lane_change_parameters_->prediction_time_resolution;
   const double current_acc = lane_change_path.info.longitudinal_acceleration.lane_changing;
   const double min_acc = std::min(current_acc, common_parameters.min_acc);
   const auto sampling_num =
     std::abs(min_acc - current_acc) > 1.0E-03 ? deceleration_sampling_num : 1;
   const double acc_resolution = (min_acc - current_acc) / static_cast<double>(sampling_num);
 
-  double acceleration = current_acc;
-  for (size_t i = 0; i < static_cast<size_t>(sampling_num); ++i, acceleration += acc_resolution) {
-    const auto ego_predicted_path = utils::lane_change::convertToPredictedPath(
-      lane_change_path, current_twist, current_pose, acceleration, common_parameters,
-      *lane_change_parameters_, time_resolution);
-    const auto debug_predicted_path =
-      utils::path_safety_checker::convertToPredictedPath(ego_predicted_path, time_resolution);
+  std::vector<double> acceleration_values(sampling_num);
+  std::iota(acceleration_values.begin(), acceleration_values.end(), 0);
 
-    const auto & current_lanes_polygon = common_data_ptr_->lanes_polygon_ptr->current;
-    const auto & expanded_target_polygon = common_data_ptr_->lanes_polygon_ptr->expanded_target;
+  std::transform(
+    acceleration_values.begin(), acceleration_values.end(), acceleration_values.begin(),
+    [&](double n) { return current_acc + n * acc_resolution; });
 
-  const auto check_collision = [&](const ExtendedPredictedObject & obj) {
+  const auto time_resolution = lane_change_parameters_->prediction_time_resolution;
+
+  const auto & current_lanes_polygon = common_data_ptr_->lanes_polygon_ptr->current;
+  const auto & expanded_target_polygon = common_data_ptr_->lanes_polygon_ptr->expanded_target;
+
+  const auto check_collision = [&](const ExtendedPredictedObject & obj, const std::vector<PoseWithVelocityStamped> & ego_predicted_path) {
     auto current_debug_data = utils::path_safety_checker::createObjectDebug(obj);
     const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj(
       obj, lane_change_parameters_->use_all_predicted_path);
-    auto is_safe = true;
     const auto selected_rss_param =
       (obj.initial_twist.twist.linear.x <=
        lane_change_parameters_->prepare_segment_ignore_object_velocity_thresh)
@@ -2162,32 +2162,10 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe(
         path, ego_predicted_path, obj, obj_path, common_parameters, rss_params, 1.0,
         get_max_velocity_for_safety_check(), current_debug_data.second);
 
-        if (collided_polygons.empty()) {
-          utils::path_safety_checker::updateCollisionCheckDebugMap(
-            debug_data, current_debug_data, is_safe);
-          continue;
-        }
-
-        const auto collision_in_current_lanes =
-          utils::lane_change::isCollidedPolygonsInLanelet(collided_polygons, current_lanes_polygon);
-        const auto collision_in_target_lanes = utils::lane_change::isCollidedPolygonsInLanelet(
-          collided_polygons, expanded_target_polygon);
-
-        if (!collision_in_current_lanes && !collision_in_target_lanes) {
-          utils::path_safety_checker::updateCollisionCheckDebugMap(
-            debug_data, current_debug_data, is_safe);
-          continue;
-        }
-
-        is_safe = false;
-        path_safety_status.is_safe = false;
+      if (collided_polygons.empty()) {
         utils::path_safety_checker::updateCollisionCheckDebugMap(
           debug_data, current_debug_data, is_safe);
-        const auto & obj_pose = obj.initial_pose.pose;
-        const auto obj_polygon = autoware::universe_utils::toPolygon2d(obj_pose, obj.shape);
-        path_safety_status.is_object_coming_from_rear |=
-          !utils::path_safety_checker::isTargetObjectFront(
-            path, current_pose, common_parameters.vehicle_info, obj_polygon);
+        continue;
       }
 
       const auto collision_in_current_lanes =
@@ -2210,11 +2188,29 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe(
     return is_safe;
   };
 
-  for (const auto & obj : collision_check_objects.trailing) {
-    if (!check_collision(obj)) {
+  const auto check_collision_with_decel_pattern = [&](const utils::path_safety_checker::ExtendedPredictedObjects & objects) {
+    return std::none_of(
+      acceleration_values.begin(), acceleration_values.end(), [&](const auto acceleration) {
+        const auto ego_predicted_path = utils::lane_change::convertToPredictedPath(
+          lane_change_path, current_twist, current_pose, acceleration, common_parameters,
+          *lane_change_parameters_, time_resolution);
+        const auto debug_predicted_path =
+          utils::path_safety_checker::convertToPredictedPath(ego_predicted_path, time_resolution);
+
+        for(const auto & obj : objects){
+          if(!check_collision(obj, ego_predicted_path)){
+            return false;
+          }
+        }
+
+
+      });
+  };
+
+
+  if (!check_collision_with_decel_pattern(collision_check_objects.trailing)) {
       return {!is_safe, is_object_behind_ego};
-    }
-  }
+      }
 
   for (const auto & obj : collision_check_objects.leading) {
     if (!check_collision(obj)) {

planning/behavior_path_lane_change_module/src/utils/utils.cpp

@@ -821,8 +821,8 @@ std::vector<PoseWithVelocityStamped> convertToPredictedPath(
     const double velocity = lane_changing_velocity + lane_changing_acceleration * delta_t;
     const double length = lane_changing_velocity * delta_t +
                           0.5 * lane_changing_acceleration * delta_t * delta_t + offset;
-    const auto pose = autoware::motion_utils::calcInterpolatedPose(
-      path.points, vehicle_pose_frenet.length + length);
+    const auto pose =
+      motion_utils::calcInterpolatedPose(path.points, vehicle_pose_frenet.length + length);
     predicted_path.emplace_back(t, pose, velocity);
   }