fix(lane_change): check obj predicted path when filtering (#9385)

* RT1-8537 check object's predicted path when filtering Signed-off-by: Zulfaqar Azmi <[email protected]> * use ranges view in get_line_string_paths Signed-off-by: Zulfaqar Azmi <[email protected]> * check only vehicle type predicted path Signed-off-by: Zulfaqar Azmi <[email protected]> * Refactor naming Signed-off-by: Zulfaqar Azmi <[email protected]> * fix grammatical Signed-off-by: Zulfaqar Azmi <[email protected]> * Update planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp Co-authored-by: Maxime CLEMENT <[email protected]> * precommit and grammar fix Signed-off-by: Zulfaqar Azmi <[email protected]> --------- Signed-off-by: Zulfaqar Azmi <[email protected]> Co-authored-by: Maxime CLEMENT <[email protected]>
autowarefoundation · Dec 9, 2024 · dbdf9a1 · dbdf9a1
1 parent 9553414
commit dbdf9a1
Show file tree

Hide file tree

Showing 5 changed files with 62 additions and 14 deletions.
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
@@ -382,5 +382,22 @@ bool filter_target_lane_objects(
   const double dist_ego_to_current_lanes_center, const bool ahead_of_ego,
   const bool before_terminal, TargetLaneLeadingObjects & leading_objects,
   ExtendedPredictedObjects & trailing_objects);
+
+/**
+ * @brief Determines if the object's predicted path overlaps with the given lane polygon.
+ *
+ * This function checks whether any of the line string paths derived from the object's predicted
+ * trajectories intersect or overlap with the specified polygon representing lanes.
+ *
+ * @param object The extended predicted object containing predicted trajectories and initial
+ * polygon.
+ * @param lanes_polygon A polygon representing the lanes to check for overlaps with the object's
+ * paths.
+ *
+ * @return true if any of the object's predicted paths overlap with the lanes polygon, false
+ * otherwise.
+ */
+bool object_path_overlaps_lanes(
+  const ExtendedPredictedObject & object, const lanelet::BasicPolygon2d & lanes_polygon);
 }  // 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/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp
@@ -988,6 +988,7 @@ lane_change::TargetObjects NormalLaneChange::get_target_objects(
 
 FilteredLanesObjects NormalLaneChange::filter_objects() const
 {
+  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
   auto objects = *planner_data_->dynamic_object;
   utils::path_safety_checker::filterObjectsByClass(
     objects, lane_change_parameters_->safety.target_object_types);

diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp
@@ -35,6 +35,8 @@
 #include <autoware_lanelet2_extension/utility/utilities.hpp>
 #include <autoware_vehicle_info_utils/vehicle_info.hpp>
 #include <range/v3/algorithm/any_of.hpp>
+#include <range/v3/range/conversion.hpp>
+#include <range/v3/view/transform.hpp>
 #include <rclcpp/rclcpp.hpp>
 
 #include <geometry_msgs/msg/detail/pose__struct.hpp>
@@ -1149,11 +1151,8 @@ std::vector<LineString2d> get_line_string_paths(const ExtendedPredictedObject &
     return line_string;
   };
 
-  const auto paths = object.predicted_paths;
-  std::vector<LineString2d> line_strings;
-  std::transform(paths.begin(), paths.end(), std::back_inserter(line_strings), to_linestring_2d);
-
-  return line_strings;
+  return object.predicted_paths | ranges::views::transform(to_linestring_2d) |
+         ranges::to<std::vector>();
 }
 
 bool has_overtaking_turn_lane_object(
@@ -1164,10 +1163,6 @@ bool has_overtaking_turn_lane_object(
     return false;
   }
 
-  const auto is_path_overlap_with_target = [&](const LineString2d & path) {
-    return !boost::geometry::disjoint(path, common_data_ptr->lanes_polygon_ptr->target);
-  };
-
   const auto is_object_overlap_with_target = [&](const auto & object) {
     // to compensate for perception issue, or if object is from behind ego, and tries to overtake,
     // but stop all of sudden
@@ -1176,8 +1171,7 @@ bool has_overtaking_turn_lane_object(
       return true;
     }
 
-    const auto paths = get_line_string_paths(object);
-    return std::any_of(paths.begin(), paths.end(), is_path_overlap_with_target);
+    return object_path_overlaps_lanes(object, common_data_ptr->lanes_polygon_ptr->target);
   };
 
   return std::any_of(
@@ -1190,6 +1184,7 @@ bool filter_target_lane_objects(
   const bool before_terminal, TargetLaneLeadingObjects & leading_objects,
   ExtendedPredictedObjects & trailing_objects)
 {
+  using behavior_path_planner::utils::path_safety_checker::filter::is_vehicle;
   using behavior_path_planner::utils::path_safety_checker::filter::velocity_filter;
   const auto & current_lanes = common_data_ptr->lanes_ptr->current;
   const auto & vehicle_width = common_data_ptr->bpp_param_ptr->vehicle_info.vehicle_width_m;
@@ -1206,9 +1201,12 @@ bool filter_target_lane_objects(
   const auto is_stopped = velocity_filter(
     object.initial_twist, -std::numeric_limits<double>::epsilon(), stopped_obj_vel_th);
   if (is_lateral_far && before_terminal) {
-    const auto in_target_lanes =
-      !boost::geometry::disjoint(object.initial_polygon, lanes_polygon.target);
-    if (in_target_lanes) {
+    const auto overlapping_with_target_lanes =
+      !boost::geometry::disjoint(object.initial_polygon, lanes_polygon.target) ||
+      (!is_stopped && is_vehicle(object.classification) &&
+       object_path_overlaps_lanes(object, lanes_polygon.target));
+
+    if (overlapping_with_target_lanes) {
       if (!ahead_of_ego && !is_stopped) {
         trailing_objects.push_back(object);
         return true;
@@ -1247,4 +1245,12 @@ bool filter_target_lane_objects(
 
   return false;
 }
+
+bool object_path_overlaps_lanes(
+  const ExtendedPredictedObject & object, const lanelet::BasicPolygon2d & lanes_polygon)
+{
+  return ranges::any_of(get_line_string_paths(object), [&](const auto & path) {
+    return !boost::geometry::disjoint(path, lanes_polygon);
+  });
+}
 }  // namespace autoware::behavior_path_planner::utils::lane_change
diff --git a/...ude/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp b/...ude/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp
@@ -71,6 +71,16 @@ bool is_within_circle(
   const geometry_msgs::msg::Point & object_pos, const geometry_msgs::msg::Point & reference_point,
   const double search_radius);
 
+/**
+ * @brief Checks if the object classification represents a vehicle (CAR, TRUCK, BUS, TRAILER,
+ * MOTORCYCLE).
+ *
+ * @param classification The object classification to check.
+ * @return true If the classification is a vehicle type.
+ * @return false Otherwise.
+ */
+bool is_vehicle(const ObjectClassification & classification);
+
 }  // namespace autoware::behavior_path_planner::utils::path_safety_checker::filter
 
 namespace autoware::behavior_path_planner::utils::path_safety_checker

diff --git a/...autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp b/...autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp
@@ -57,6 +57,20 @@ bool is_within_circle(
     std::hypot(reference_point.x - object_pos.x, reference_point.y - object_pos.y);
   return dist < search_radius;
 }
+
+bool is_vehicle(const ObjectClassification & classification)
+{
+  switch (classification.label) {
+    case ObjectClassification::CAR:
+    case ObjectClassification::TRUCK:
+    case ObjectClassification::BUS:
+    case ObjectClassification::TRAILER:
+    case ObjectClassification::MOTORCYCLE:
+      return true;
+    default:
+      return false;
+  }
+}
 }  // namespace autoware::behavior_path_planner::utils::path_safety_checker::filter
 
 namespace autoware::behavior_path_planner::utils::path_safety_checker