separate target lane leading objects based on behavior (RT1-8532)

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

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp

@@ -128,8 +128,7 @@ class NormalLaneChange : public LaneChangeBase
 
   void filterOncomingObjects(PredictedObjects & objects) const;
 
-  FilteredByLanesObjects filterObjectsByLanelets(
-    const PredictedObjects & objects, const PathWithLaneId & current_lanes_ref_path) const;
+  FilteredByLanesExtendedObjects filterObjectsByLanelets(const PredictedObjects & objects) const;
 
   bool get_prepare_segment(
     PathWithLaneId & prepare_segment, const double prepare_length) const override;

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp

@@ -262,23 +262,21 @@ struct Info
   }
 };
 
-template <typename Object>
+struct TargetLaneLeadingObjects
+{
+  ExtendedPredictedObjects moving;
+  ExtendedPredictedObjects stopped;
+  ExtendedPredictedObjects expanded;
+
+  [[nodiscard]] size_t size() const { return moving.size() + stopped.size() + expanded.size(); }
+};
+
 struct LanesObjects
 {
-  Object current_lane{};
-  Object target_lane_leading{};
-  Object target_lane_trailing{};
-  Object other_lane{};
-
-  LanesObjects() = default;
-  LanesObjects(
-    Object current_lane, Object target_lane_leading, Object target_lane_trailing, Object other_lane)
-  : current_lane(std::move(current_lane)),
-    target_lane_leading(std::move(target_lane_leading)),
-    target_lane_trailing(std::move(target_lane_trailing)),
-    other_lane(std::move(other_lane))
-  {
-  }
+  ExtendedPredictedObjects others;
+  ExtendedPredictedObjects current_lane;
+  ExtendedPredictedObjects target_lane_trailing;
+  TargetLaneLeadingObjects target_lane_leading;
 };
 
 struct TargetObjects
@@ -418,8 +416,7 @@ using LaneChangeStates = lane_change::States;
 using LaneChangePhaseInfo = lane_change::PhaseInfo;
 using LaneChangePhaseMetrics = lane_change::PhaseMetrics;
 using LaneChangeInfo = lane_change::Info;
-using FilteredByLanesObjects = lane_change::LanesObjects<std::vector<PredictedObject>>;
-using FilteredByLanesExtendedObjects = lane_change::LanesObjects<ExtendedPredictedObjects>;
+using FilteredByLanesExtendedObjects = lane_change::LanesObjects;
 using LateralAccelerationMap = lane_change::LateralAccelerationMap;
 }  // namespace autoware::behavior_path_planner
 

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/debug_structs.hpp

@@ -55,9 +55,11 @@ struct Debug
     collision_check_objects.clear();
     collision_check_objects_after_approval.clear();
     filtered_objects.current_lane.clear();
-    filtered_objects.target_lane_leading.clear();
     filtered_objects.target_lane_trailing.clear();
-    filtered_objects.other_lane.clear();
+    filtered_objects.target_lane_leading.moving.clear();
+    filtered_objects.target_lane_leading.stopped.clear();
+    filtered_objects.target_lane_leading.expanded.clear();
+    filtered_objects.others.clear();
     execution_area.points.clear();
     current_lanes.clear();
     target_lanes.clear();

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp

@@ -54,6 +54,7 @@ using behavior_path_planner::lane_change::CommonDataPtr;
 using behavior_path_planner::lane_change::LanesPolygon;
 using behavior_path_planner::lane_change::ModuleType;
 using behavior_path_planner::lane_change::PathSafetyStatus;
+using behavior_path_planner::lane_change::TargetLaneLeadingObjects;
 using geometry_msgs::msg::Point;
 using geometry_msgs::msg::Pose;
 using geometry_msgs::msg::Twist;
@@ -144,8 +145,7 @@ lanelet::BasicPolygon2d create_polygon(
   const lanelet::ConstLanelets & lanes, const double start_dist, const double end_dist);
 
 ExtendedPredictedObject transform(
-  const PredictedObject & object, const BehaviorPathPlannerParameters & common_parameters,
-  const LaneChangeParameters & lane_change_parameters);
+  const PredictedObject & object, const LaneChangeParameters & lane_change_parameters);
 
 bool is_collided_polygons_in_lanelet(
   const std::vector<Polygon2d> & collided_polygons, const lanelet::BasicPolygon2d & lanes_polygon);
@@ -240,17 +240,14 @@ bool is_same_lane_with_prev_iteration(
 
 bool is_ahead_of_ego(
   const CommonDataPtr & common_data_ptr, const PathWithLaneId & path,
-  const PredictedObject & object);
+  const ExtendedPredictedObject & object);
 
 bool is_before_terminal(
   const CommonDataPtr & common_data_ptr, const PathWithLaneId & path,
-  const PredictedObject & object);
+  const ExtendedPredictedObject & object);
 
 double calc_angle_to_lanelet_segment(const lanelet::ConstLanelets & lanelets, const Pose & pose);
 
-ExtendedPredictedObjects transform_to_extended_objects(
-  const CommonDataPtr & common_data_ptr, const std::vector<PredictedObject> & objects);
-
 double get_distance_to_next_regulatory_element(
   const CommonDataPtr & common_data_ptr, const bool ignore_crosswalk = false,
   const bool ignore_intersection = false);
@@ -294,8 +291,8 @@ double get_min_dist_to_current_lanes_obj(
  * otherwise, false.
  */
 bool has_blocking_target_object(
-  const CommonDataPtr & common_data_ptr, const FilteredByLanesExtendedObjects & filtered_objects,
-  const double stop_arc_length, const PathWithLaneId & path);
+  const TargetLaneLeadingObjects & target_leading_objects, const double stop_arc_length,
+  const PathWithLaneId & path);
 
 /**
  * @brief Checks if the ego vehicle has passed any turn direction within an intersection.
@@ -342,5 +339,11 @@ std::vector<LineString2d> get_line_string_paths(const ExtendedPredictedObject &
  */
 bool has_overtaking_turn_lane_object(
   const CommonDataPtr & common_data_ptr, const ExtendedPredictedObjects & trailing_objects);
+
+void filter_target_lane_objects(
+  const CommonDataPtr & common_data_ptr, const ExtendedPredictedObject & object,
+  const double dist_ego_to_current_lanes_center, const bool ahead_of_ego,
+  const bool before_terminal, TargetLaneLeadingObjects & leading_objects,
+  ExtendedPredictedObjects & trailing_objects);
 }  // namespace autoware::behavior_path_planner::utils::lane_change
 #endif  // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/package.xml

@@ -27,6 +27,7 @@
   <depend>autoware_rtc_interface</depend>
   <depend>autoware_universe_utils</depend>
   <depend>pluginlib</depend>
+  <depend>range-v3</depend>
   <depend>rclcpp</depend>
   <depend>tier4_planning_msgs</depend>
   <depend>visualization_msgs</depend>