refactor(lane_change): rework object filter

Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>

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/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,24 @@ 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 filterObjectsAheadTerminal(
+    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 +170,16 @@ 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 lanelet::ConstLanelets & target_backward_lanes);
 }  // namespace behavior_path_planner::utils::lane_change
 
 namespace behavior_path_planner::utils::lane_change::debug