refactor(lane_change): separate target lane leading based on obj beha…

…vior (#9372) * separate target lane leading objects based on behavior (RT1-8532) Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * fixed overlapped filtering and lanes debug marker Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * combine filteredObjects function Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * renaming functions and type Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * update some logic to check is stopped Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * rename expanded to stopped_outside_boundary Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * Include docstring Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * rename stopped_outside_boundary → stopped_at_bound Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * Update planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp Co-authored-by: mkquda <168697710+mkquda@users.noreply.github.com> Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * Update planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp Co-authored-by: mkquda <168697710+mkquda@users.noreply.github.com> Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * spell-check Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> --------- Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> Co-authored-by: mkquda <168697710+mkquda@users.noreply.github.com>
autowarefoundation · Nov 22, 2024 · 25e7d5d · 25e7d5d
1 parent e5181a5
commit 25e7d5d
Show file tree

Hide file tree

Showing 13 changed files with 284 additions and 313 deletions.
diff --git a/...avior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp b/...avior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp
@@ -121,16 +121,13 @@ class NormalLaneChange : public LaneChangeBase
   TurnSignalInfo get_terminal_turn_signal_info() const final;
 
   lane_change::TargetObjects get_target_objects(
-    const FilteredByLanesExtendedObjects & filtered_objects,
+    const FilteredLanesObjects & filtered_objects,
     const lanelet::ConstLanelets & current_lanes) const;
 
-  FilteredByLanesExtendedObjects filterObjects() const;
+  FilteredLanesObjects filter_objects() const;
 
   void filterOncomingObjects(PredictedObjects & objects) const;
 
-  FilteredByLanesObjects filterObjectsByLanelets(
-    const PredictedObjects & objects, const PathWithLaneId & current_lanes_ref_path) const;
-
   bool get_prepare_segment(
     PathWithLaneId & prepare_segment, const double prepare_length) const override;
 

diff --git a/...lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp b/...lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp
@@ -276,7 +276,7 @@ class LaneChangeBase
   std::shared_ptr<LaneChangePath> abort_path_{};
   std::shared_ptr<const PlannerData> planner_data_{};
   lane_change::CommonDataPtr common_data_ptr_{};
-  FilteredByLanesExtendedObjects filtered_objects_{};
+  FilteredLanesObjects filtered_objects_{};
   BehaviorModuleOutput prev_module_output_{};
   std::optional<Pose> lane_change_stop_pose_{std::nullopt};
 

diff --git a/...ne_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp b/...ne_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp
@@ -262,25 +262,28 @@ struct Info
   }
 };
 
-template <typename Object>
-struct LanesObjects
+struct TargetLaneLeadingObjects
 {
-  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 moving;
+  ExtendedPredictedObjects stopped;
+
+  // for objects outside of target lanes, but close to its boundaries
+  ExtendedPredictedObjects stopped_at_bound;
+
+  [[nodiscard]] size_t size() const
   {
+    return moving.size() + stopped.size() + stopped_at_bound.size();
   }
 };
 
+struct FilteredLanesObjects
+{
+  ExtendedPredictedObjects others;
+  ExtendedPredictedObjects current_lane;
+  ExtendedPredictedObjects target_lane_trailing;
+  TargetLaneLeadingObjects target_lane_leading;
+};
+
 struct TargetObjects
 {
   ExtendedPredictedObjects leading;
@@ -418,8 +421,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 FilteredLanesObjects = lane_change::FilteredLanesObjects;
 using LateralAccelerationMap = lane_change::LateralAccelerationMap;
 }  // namespace autoware::behavior_path_planner
 

diff --git a/...e_change_module/include/autoware/behavior_path_lane_change_module/utils/debug_structs.hpp b/...e_change_module/include/autoware/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;
-  FilteredByLanesExtendedObjects filtered_objects;
+  FilteredLanesObjects filtered_objects;
   geometry_msgs::msg::Polygon execution_area;
   geometry_msgs::msg::Pose ego_pose;
   lanelet::ConstLanelets current_lanes;
@@ -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.stopped_at_bound.clear();
+    filtered_objects.others.clear();
     execution_area.points.clear();
     current_lanes.clear();
     target_lanes.clear();

diff --git a/...th_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/markers.hpp b/...th_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/markers.hpp
@@ -29,7 +29,7 @@
 
 namespace marker_utils::lane_change_markers
 {
-using autoware::behavior_path_planner::FilteredByLanesExtendedObjects;
+using autoware::behavior_path_planner::FilteredLanesObjects;
 using autoware::behavior_path_planner::LaneChangePath;
 using autoware::behavior_path_planner::lane_change::Debug;
 using autoware::behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObjects;
@@ -40,7 +40,7 @@ MarkerArray createLaneChangingVirtualWallMarker(
   const geometry_msgs::msg::Pose & lane_changing_pose, const std::string & module_name,
   const rclcpp::Time & now, const std::string & ns);
 MarkerArray showFilteredObjects(
-  const FilteredByLanesExtendedObjects & filtered_objects, const std::string & ns);
+  const FilteredLanesObjects & filtered_objects, const std::string & ns);
 MarkerArray createExecutionArea(const geometry_msgs::msg::Polygon & execution_area);
 MarkerArray showExecutionInfo(const Debug & debug_data, const geometry_msgs::msg::Pose & ego_pose);
 MarkerArray createDebugMarkerArray(

diff --git a/...path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp b/...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);
@@ -274,7 +271,7 @@ double get_distance_to_next_regulatory_element(
  * found, returns the maximum possible double value.
  */
 double get_min_dist_to_current_lanes_obj(
-  const CommonDataPtr & common_data_ptr, const FilteredByLanesExtendedObjects & filtered_objects,
+  const CommonDataPtr & common_data_ptr, const FilteredLanesObjects & filtered_objects,
   const double dist_to_target_lane_start, const PathWithLaneId & path);
 
 /**
@@ -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,34 @@ std::vector<LineString2d> get_line_string_paths(const ExtendedPredictedObject &
  */
 bool has_overtaking_turn_lane_object(
   const CommonDataPtr & common_data_ptr, const ExtendedPredictedObjects & trailing_objects);
+
+/**
+ * @brief Filters objects based on their positions and velocities relative to the ego vehicle and
+ * the target lane.
+ *
+ * This function evaluates whether an object should be classified as a leading or trailing object
+ * in the context of a lane change. Objects are filtered based on their lateral distance from
+ * the ego vehicle, velocity, and whether they are within the target lane or its expanded
+ * boundaries.
+ *
+ * @param common_data_ptr Shared pointer to CommonData containing information about current lanes,
+ *                        vehicle dimensions, lane polygons, and behavior parameters.
+ * @param object An extended predicted object representing a potential obstacle in the environment.
+ * @param dist_ego_to_current_lanes_center Distance from the ego vehicle to the center of the
+ * current lanes.
+ * @param ahead_of_ego Boolean flag indicating if the object is ahead of the ego vehicle.
+ * @param before_terminal Boolean flag indicating if the ego vehicle is before the terminal point of
+ * the lane.
+ * @param leading_objects Reference to a structure for storing leading objects (stopped, moving, or
+ * outside boundaries).
+ * @param trailing_objects Reference to a collection for storing trailing objects.
+ *
+ * @return true if the object is classified as either leading or trailing, false otherwise.
+ */
+bool 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_
diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/package.xml b/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>