fix(lane_change): fix filtering objects

Signed-off-by: kosuke55 <[email protected]>
tier4 · Oct 16, 2023 · b2a5c3f · b2a5c3f
1 parent dfa43da
commit b2a5c3f
Show file tree

Hide file tree

Showing 2 changed files with 12 additions and 13 deletions.
diff --git a/...g/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp b/...g/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.

diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp
@@ -728,15 +728,18 @@ LaneChangeTargetObjects NormalLaneChange::getTargetObjects(
   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());
@@ -768,13 +771,13 @@ LaneChangeTargetObjects NormalLaneChange::getTargetObjects(
 }
 
 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()) {
@@ -814,15 +817,10 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject(
     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 =