perf(lane_change): rework object filter

planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp

@@ -145,7 +145,9 @@ void AvoidanceByLaneChange::fillAvoidanceTargetObjects(
   const auto [object_within_target_lane, object_outside_target_lane] =
     utils::path_safety_checker::separateObjectsByLanelets(
       *planner_data_->dynamic_object, data.current_lanelets,
-      utils::path_safety_checker::isPolygonOverlapLanelet);
+      [](const auto & obj, const auto & lane) {
+        return utils::path_safety_checker::isPolygonOverlapLanelet(obj, lane);
+      });
 
   // Assume that the maximum allocation for data.other object is the sum of
   // objects_within_target_lane and object_outside_target_lane. The maximum allocation for

planning/behavior_path_goal_planner_module/src/util.cpp

@@ -145,7 +145,9 @@ PredictedObjects extractObjectsInExpandedPullOverLanes(
     route_handler, left_side, backward_distance, forward_distance, bound_offset);
 
   const auto [objects_in_lanes, others] = utils::path_safety_checker::separateObjectsByLanelets(
-    objects, lanes, utils::path_safety_checker::isPolygonOverlapLanelet);
+    objects, lanes, [](const auto & obj, const auto & lanelet) {
+      return utils::path_safety_checker::isPolygonOverlapLanelet(obj, lanelet);
+    });
 
   return objects_in_lanes;
 }

planning/behavior_path_lane_change_module/README.md

@@ -319,6 +319,171 @@ The detection area for the target lane can be expanded beyond its original bound
   </table>
 </div>
 
+##### Object filtering
+
+```plantuml
+@startuml
+skinparam defaultTextAlignment center
+skinparam backgroundColor #WHITE
+
+title NormalLaneChange::filterObjects Method Execution Flow
+
+start
+
+group "Filter Objects by Class" {
+:Iterate through each object in objects list;
+while (has not finished iterating through object list) is (TRUE)
+  if (current object type != param.object_types_to_check?) then (TRUE)
+  #LightPink:Remove current object;
+else (FALSE)
+  :Keep current object;
+endif
+end while
+end group
+
+if (object list is empty?) then (TRUE)
+  :Return empty result;
+  stop
+else (FALSE)
+endif
+
+group "Filter Oncoming Objects" #PowderBlue {
+:Iterate through each object in target lane objects list;
+while (has not finished iterating through object list?) is (TRUE)
+:check object's yaw with reference to ego's yaw.;
+if (yaw difference < 90 degree?) then (TRUE)
+  :Keep current object;
+else (FALSE)
+if (object is stopping?) then (TRUE)
+  :Keep current object;
+else (FALSE)
+  #LightPink:Remove current object;
+endif
+endif
+endwhile
+end group
+
+if (object list is empty?) then (TRUE)
+  :Return empty result;
+  stop
+else (FALSE)
+endif
+
+group "Filter Objects Ahead Terminal" #Beige {
+:Calculate lateral distance from ego to current lanes center;
+
+:Iterate through each object in objects list;
+while (has not finished iterating through object list) is (TRUE)
+  :Get current object's polygon;
+  :Initialize distance to terminal from object to max;
+  while (has not finished iterating through object polygon's vertices) is (TRUE)
+    :Calculate object's lateral distance to end of lane;
+    :Update minimum distance to terminal from object;
+  end while
+  if (Is object's distance to terminal exceeds minimum lane change length?) then (TRUE)
+      #LightPink:Remove current object;
+  else (FALSE)
+  endif
+end while
+end group
+
+if (object list is empty?) then (TRUE)
+  :Return empty result;
+  stop
+else (FALSE)
+endif
+
+group "Filter Objects By Lanelets" #LightGreen {
+
+:Iterate through each object in objects list;
+while (has not finished iterating through object list) is (TRUE)
+  :lateral distance diff = difference between object's lateral distance and ego's lateral distance to the current lanes' centerline.;
+  if (Object in target lane polygon, and lateral distance diff is more than half of ego's width?) then (TRUE)
+    :Add to target_lane_objects;
+    else (FALSE)
+      if (Object overlaps with backward target lanes?) then (TRUE)
+        :Add to target_lane_objects;
+      else (FALSE)
+        if (Object in current lane polygon?) then (TRUE)
+          :Add to current_lane_objects;
+        else (FALSE)
+          :Add to other_lane_objects;
+        endif
+      endif
+  endif
+end while
+
+:Return target lanes object,  current lanes object and other lanes object;
+end group
+
+:Generate path from current lanes;
+
+if (path empty?) then (TRUE)
+  :Return empty result;
+  stop
+else (FALSE)
+endif
+
+group "Filter Target Lanes' objects" #LightCyan {
+
+:Iterate through each object in target lane objects list;
+while (has not finished iterating through object list) is (TRUE)
+  :check object's velocity;
+  if(velocity is within threshold?) then (TRUE)
+  :Keep current object;
+  else (FALSE)
+    :check whether object is ahead of ego;
+    if(object is ahead of ego?) then (TRUE)
+      :keep current object;
+    else (FALSE)
+      #LightPink:remove current object;
+    endif
+   endif
+endwhile
+end group
+
+group "Filter Current Lanes' objects"  #LightYellow {
+
+:Iterate through each object in current lane objects list;
+while (has not finished iterating through object list) is (TRUE)
+  :check object's velocity;
+  if(velocity is within threshold?) then (TRUE)
+  :check whether object is ahead of ego;
+    if(object is ahead of ego?) then (TRUE)
+      :keep current object;
+    else (FALSE)
+      #LightPink:remove current object;
+    endif
+    else (FALSE)
+      #LightPink:remove current object;
+   endif
+endwhile
+end group
+
+group "Filter Other Lanes' objects"  #Lavender {
+
+:Iterate through each object in other lane objects list;
+while (has not finished iterating through object list) is (TRUE)
+  :check object's velocity;
+  if(velocity is within threshold?) then (TRUE)
+  :check whether object is ahead of ego;
+    if(object is ahead of ego?) then (TRUE)
+      :keep current object;
+    else (FALSE)
+      #LightPink:remove current object;
+    endif
+    else (FALSE)
+      #LightPink:remove current object;
+   endif
+endwhile
+end group
+
+:Trasform the objects into extended predicted object and return them as lane_change_target_objects;
+stop
+
+@enduml
+```
+
 ##### Collision check in prepare phase
 
 The ego vehicle may need to secure ample inter-vehicle distance ahead of the target vehicle before attempting a lane change. The flag `enable_collision_check_at_prepare_phase` can be enabled to gain this behavior. The following image illustrates the differences between the `false` and `true` cases.

planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp

@@ -30,10 +30,12 @@ using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject
 using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityAndPolygonStamped;
 using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped;
 using behavior_path_planner::utils::path_safety_checker::PredictedPathWithPolygon;
+using data::lane_change::LanesPolygon;
 using geometry_msgs::msg::Point;
 using geometry_msgs::msg::Pose;
 using geometry_msgs::msg::Twist;
 using route_handler::Direction;
+using utils::path_safety_checker::ExtendedPredictedObjects;
 
 class NormalLaneChange : public LaneChangeBase
 {
@@ -115,10 +117,26 @@ class NormalLaneChange : public LaneChangeBase
     const lanelet::ConstLanelets & current_lanes,
     const lanelet::ConstLanelets & target_lanes) const;
 
-  LaneChangeTargetObjects getTargetObjects(
+  ExtendedPredictedObjects getTargetObjects(
+    const LaneChangeLanesFilteredObjects & predicted_objects,
+    const lanelet::ConstLanelets & current_lanes) const;
+
+  LaneChangeLanesFilteredObjects filterObjects(
     const lanelet::ConstLanelets & current_lanes,
     const lanelet::ConstLanelets & target_lanes) const;
 
+  void filterOncomingObjects(PredictedObjects & objects) const;
+
+  void filterAheadTerminalObjects(
+    PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes) const;
+
+  void filterObjectsByLanelets(
+    const PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes,
+    const lanelet::ConstLanelets & target_lanes,
+    std::vector<PredictedObject> & current_lane_objects,
+    std::vector<PredictedObject> & target_lane_objects,
+    std::vector<PredictedObject> & other_lane_objects) const;
+
   PathWithLaneId getPrepareSegment(
     const lanelet::ConstLanelets & current_lanes, const double backward_path_length,
     const double prepare_length) const override;
@@ -154,18 +172,11 @@ class NormalLaneChange : public LaneChangeBase
   bool isValidPath(const PathWithLaneId & path) const override;
 
   PathSafetyStatus isLaneChangePathSafe(
-    const LaneChangePath & lane_change_path, const LaneChangeTargetObjects & target_objects,
-    const utils::path_safety_checker::RSSparams & rss_params, const bool is_stuck,
+    const LaneChangePath & lane_change_path,
+    const ExtendedPredictedObjects & collision_check_objects,
+    const utils::path_safety_checker::RSSparams & rss_params,
     CollisionCheckDebugMap & debug_data) const;
 
-  LaneChangeTargetObjectIndices filterObject(
-    const PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes,
-    const lanelet::ConstLanelets & target_lanes,
-    const lanelet::ConstLanelets & target_backward_lanes) const;
-
-  std::vector<ExtendedPredictedObject> filterObjectsInTargetLane(
-    const LaneChangeTargetObjects & objects, const lanelet::ConstLanelets & target_lanes) const;
-
   //! @brief Check if the ego vehicle is in stuck by a stationary obstacle.
   //! @param obstacle_check_distance Distance to check ahead for any objects that might be
   //! obstructing ego path. It makes sense to use values like the maximum lane change distance.

planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/data_structs.hpp

@@ -20,6 +20,7 @@
 #include <interpolation/linear_interpolation.hpp>
 
 #include <lanelet2_core/primitives/Lanelet.h>
+#include <lanelet2_core/primitives/Polygon.h>
 
 #include <utility>
 #include <vector>
@@ -195,11 +196,11 @@ struct LaneChangeTargetObjectIndices
   std::vector<size_t> other_lane{};
 };
 
-struct LaneChangeTargetObjects
+struct LaneChangeLanesFilteredObjects
 {
-  std::vector<utils::path_safety_checker::ExtendedPredictedObject> current_lane{};
-  std::vector<utils::path_safety_checker::ExtendedPredictedObject> target_lane{};
-  std::vector<utils::path_safety_checker::ExtendedPredictedObject> other_lane{};
+  utils::path_safety_checker::ExtendedPredictedObjects current_lane{};
+  utils::path_safety_checker::ExtendedPredictedObjects target_lane{};
+  utils::path_safety_checker::ExtendedPredictedObjects other_lane{};
 };
 
 enum class LaneChangeModuleType {
@@ -216,6 +217,13 @@ struct PathSafetyStatus
   bool is_safe{true};
   bool is_object_coming_from_rear{false};
 };
+
+struct LanesPolygon
+{
+  std::optional<lanelet::BasicPolygon2d> current;
+  std::optional<lanelet::BasicPolygon2d> target;
+  std::vector<lanelet::BasicPolygon2d> target_backward;
+};
 }  // namespace behavior_path_planner::data::lane_change
 
 #endif  // BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DATA_STRUCTS_HPP_

planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/debug_structs.hpp

@@ -35,7 +35,7 @@ struct Debug
   LaneChangePaths valid_paths;
   CollisionCheckDebugMap collision_check_objects;
   CollisionCheckDebugMap collision_check_objects_after_approval;
-  LaneChangeTargetObjects filtered_objects;
+  LaneChangeLanesFilteredObjects filtered_objects;
   geometry_msgs::msg::Polygon execution_area;
   geometry_msgs::msg::Pose ego_pose;
   lanelet::ConstLanelets current_lanes;

planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp

@@ -46,6 +46,7 @@ using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject
 using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityAndPolygonStamped;
 using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped;
 using behavior_path_planner::utils::path_safety_checker::PredictedPathWithPolygon;
+using data::lane_change::LanesPolygon;
 using data::lane_change::PathSafetyStatus;
 using geometry_msgs::msg::Point;
 using geometry_msgs::msg::Pose;
@@ -292,6 +293,10 @@ bool isWithinTurnDirectionLanes(const lanelet::ConstLanelet & lanelet, const Pol
 double calcPhaseLength(
   const double velocity, const double maximum_velocity, const double acceleration,
   const double time);
+
+LanesPolygon createLanesPolygon(
+  const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes,
+  const std::vector<lanelet::ConstLanelets> & target_backward_lanes);
 }  // namespace behavior_path_planner::utils::lane_change
 
 namespace behavior_path_planner::utils::lane_change::debug