fix(lane_change): fix filtering objects

planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp

@@ -151,7 +151,8 @@ class NormalLaneChange : public LaneChangeBase
     CollisionCheckDebugMap & debug_data) const;
 
   LaneChangeTargetObjectIndices filterObject(
-    const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes,
+    const PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes,
+    const lanelet::ConstLanelets & target_lanes,
     const lanelet::ConstLanelets & target_backward_lanes) const;
 
   //! @brief Check if the ego vehicle is in stuck by a stationary obstacle.

planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp

@@ -732,15 +732,18 @@
   const auto current_pose = getEgoPose();
   const auto & route_handler = *getRouteHandler();
   const auto & common_parameters = planner_data_->parameters;
-  const auto & objects = *planner_data_->dynamic_object;
+  auto objects = *planner_data_->dynamic_object;
+  utils::path_safety_checker::filterObjectsByClass(
+    objects, lane_change_parameters_->object_types_to_check);
 
   // get backward lanes
   const auto backward_length = lane_change_parameters_->backward_lane_length;
   const auto target_backward_lanes = utils::lane_change::getBackwardLanelets(
     route_handler, target_lanes, current_pose, backward_length);
 
   // filter objects to get target object index
-  const auto target_obj_index = filterObject(current_lanes, target_lanes, target_backward_lanes);
+  const auto target_obj_index =
+    filterObject(objects, current_lanes, target_lanes, target_backward_lanes);
 
   LaneChangeTargetObjects target_objects;
   target_objects.current_lane.reserve(target_obj_index.current_lane.size());
@@ -772,13 +775,13 @@
 }
 
 LaneChangeTargetObjectIndices NormalLaneChange::filterObject(
-  const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes,
+  const PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes,
+  const lanelet::ConstLanelets & target_lanes,
   const lanelet::ConstLanelets & target_backward_lanes) const
 {
   const auto current_pose = getEgoPose();
   const auto & route_handler = *getRouteHandler();
   const auto & common_parameters = planner_data_->parameters;
-  const auto & objects = *planner_data_->dynamic_object;
 
   // Guard
   if (objects.objects.empty()) {
@@ -818,15 +821,10 @@
     target_backward_polygons.push_back(lane_polygon);
   }
 
-  auto filtered_objects = objects;
-
-  utils::path_safety_checker::filterObjectsByClass(
-    filtered_objects, lane_change_parameters_->object_types_to_check);
-
   LaneChangeTargetObjectIndices filtered_obj_indices;
-  for (size_t i = 0; i < filtered_objects.objects.size(); ++i) {
-    const auto & object = filtered_objects.objects.at(i);
-    const auto & obj_velocity_norm = std::hypot(
+  for (size_t i = 0; i < objects.objects.size(); ++i) {
+    const auto & object = objects.objects.at(i);
+    const auto obj_velocity_norm = std::hypot(
       object.kinematics.initial_twist_with_covariance.twist.linear.x,
       object.kinematics.initial_twist_with_covariance.twist.linear.y);
     const auto extended_object =
@@ -1086,11 +1084,11 @@
 
   for (const auto & prepare_duration : prepare_durations) {
     for (const auto & sampled_longitudinal_acc : longitudinal_acc_sampling_values) {
       const auto debug_print = [&](const auto & s) {
         RCLCPP_DEBUG_STREAM(
           logger_, "  -  " << s << " : prep_time = " << prepare_duration
                            << ", lon_acc = " << sampled_longitudinal_acc);
       };
 
       // get path on original lanes
       const auto prepare_velocity = std::max(
@@ -1144,11 +1142,11 @@
       RCLCPP_DEBUG(logger_, "  -  sampling num for lat_acc: %lu", sample_lat_acc.size());
 
       for (const auto & lateral_acc : sample_lat_acc) {
         const auto debug_print = [&](const auto & s) {
           RCLCPP_DEBUG_STREAM(
             logger_, "    -  " << s << " : prep_time = " << prepare_duration << ", lon_acc = "
                                << sampled_longitudinal_acc << ", lat_acc = " << lateral_acc);
         };
 
         const auto lane_changing_time = PathShifter::calcShiftTimeFromJerk(
           shift_length, common_parameters.lane_changing_lateral_jerk, lateral_acc);
@@ -1265,7 +1263,7 @@
             debug_print("Reject: including crosswalk!!");
             continue;
           }
           RCLCPP_INFO_THROTTLE(
             logger_, clock_, 1000, "Stop time is over threshold. Allow lane change in crosswalk.");
         }