additional filters and heavy revamp

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

planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp

@@ -153,7 +153,7 @@ class NormalLaneChange : public LaneChangeBase
     const utils::path_safety_checker::RSSparams & rss_params, const bool is_stuck,
     CollisionCheckDebugMap & debug_data) const;
 
-  LaneChangeTargetObjectIndices filterObject(
+  LaneChangeTargetObjects filterObject(
     const PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes,
     const lanelet::ConstLanelets & target_lanes,
     const lanelet::ConstLanelets & target_backward_lanes) const;

planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp

@@ -44,6 +44,13 @@ struct SafetyCheck
   double lane_expansion_left_offset{0.0};
   double lane_expansion_right_offset{0.0};
 };
+
+struct LanesPolygon
+{
+  lanelet::BasicPolygon2d current_polygon;
+  lanelet::BasicPolygon2d target_polygon;
+  lanelet::BasicPolygons2d target_backward_polygons;
+};
 }  // namespace behavior_path_planner::lane_change
 
 namespace behavior_path_planner
@@ -136,13 +143,6 @@ struct LaneChangeInfo
   double terminal_lane_changing_velocity{0.0};
 };
 
-struct LaneChangeTargetObjectIndices
-{
-  std::vector<size_t> current_lane{};
-  std::vector<size_t> target_lane{};
-  std::vector<size_t> other_lane{};
-};
-
 struct LaneChangeTargetObjects
 {
   std::vector<utils::path_safety_checker::ExtendedPredictedObject> current_lane{};

planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp

@@ -39,6 +39,7 @@ using autoware_auto_perception_msgs::msg::PredictedObject;
 using autoware_auto_perception_msgs::msg::PredictedObjects;
 using autoware_auto_perception_msgs::msg::PredictedPath;
 using autoware_auto_planning_msgs::msg::PathWithLaneId;
+using behavior_path_planner::lane_change::LanesPolygon;
 using behavior_path_planner::lane_change::PathSafetyStatus;
 using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject;
 using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityAndPolygonStamped;
@@ -202,5 +203,8 @@ lanelet::ConstLanelets generateExpandedLanelets(
   const lanelet::ConstLanelets & lanes, const Direction direction, const double left_offset,
   const double right_offset);
 
+LanesPolygon createPolygon(
+  const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes,
+  const lanelet::ConstLanelets & target_backward_lanes);
 }  // namespace behavior_path_planner::utils::lane_change
 #endif  // BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__UTILS_HPP_

planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp

@@ -33,6 +33,21 @@
 #include <utility>
 #include <vector>
 
+namespace behavior_path_planner::utils::path_safety_checker::filters
+{
+
+using autoware_auto_perception_msgs::msg::PredictedObject;
+using autoware_auto_planning_msgs::msg::PathPointWithLaneId;
+
+bool position_filter(
+  const PredictedObject & object, const std::vector<PathPointWithLaneId> & path_points,
+  const geometry_msgs::msg::Point & current_pose, const double backward_distance,
+  const double forward_distance);
+
+bool velocity_filter(
+                     const PredictedObject & object, double velocity_threshold, double max_velocity);
+}  // namespace behavior_path_planner::utils::path_safety_checker::filters
+
 namespace behavior_path_planner::utils::path_safety_checker
 {
 
@@ -71,8 +86,7 @@ bool isPolygonOverlapLanelet(const PredictedObject & object, const lanelet::Cons
 PredictedObjects filterObjects(
   const std::shared_ptr<const PredictedObjects> & objects,
   const std::shared_ptr<RouteHandler> & route_handler, const lanelet::ConstLanelets & current_lanes,
-  const geometry_msgs::msg::Point & current_pose,
-  const std::shared_ptr<ObjectsFilteringParams> & params);
+  const geometry_msgs::msg::Point & current_pose, const ObjectsFilteringParams & params);
 
 /**
  * @brief Filters objects based on their velocity.
@@ -118,8 +132,8 @@ PredictedObjects filterObjectsByVelocity(
  */
 void filterObjectsByPosition(
   PredictedObjects & objects, const std::vector<PathPointWithLaneId> & path_points,
-  const geometry_msgs::msg::Point & current_pose, const double forward_distance,
-  const double backward_distance);
+  const geometry_msgs::msg::Point & current_pose, const double backward_distance,
+  const double forward_distance);
 
 /**
  * @brief Filters the provided objects based on their classification.

planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp

@@ -160,10 +160,12 @@ struct ObjectsFilteringParams
   double ignore_object_velocity_threshold{0.0};  ///< Velocity threshold for ignoring objects.
   ObjectTypesToCheck object_types_to_check{};    ///< Specifies which object types to check.
   ObjectLaneConfiguration object_lane_configuration{};  ///< Configuration for which lanes to check.
-  bool include_opposite_lane{false};                    ///< Include the opposite lane in checks.
-  bool invert_opposite_lane{false};                     ///< Invert the opposite lane in checks.
-  bool check_all_predicted_path{false};                 ///< Check all predicted paths.
-  bool use_all_predicted_path{false};                   ///< Use all predicted paths.
+  bool remove_velocity_above_threshold{
+    false};  ///< Remove object with velocity above ignore_object_velocity_threshold
+  bool include_opposite_lane{false};               ///< Include the opposite lane in checks.
+  bool invert_opposite_lane{false};                ///< Invert the opposite lane in checks.
+  bool check_all_predicted_path{false};            ///< Check all predicted paths.
+  bool use_all_predicted_path{false};              ///< Use all predicted paths.
   bool use_predicted_path_outside_lanelet{false};  ///< Use predicted paths outside of lanelets.
 };
 

planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp

@@ -1787,7 +1787,7 @@ bool GoalPlannerModule::isSafePath() const
   // filtering objects with velocity, position and class
   const auto filtered_objects = utils::path_safety_checker::filterObjects(
     dynamic_object, route_handler, pull_over_lanes, current_pose.position,
-    objects_filtering_params_);
+    *objects_filtering_params_);
 
   // filtering objects based on the current position's lane
   const auto target_objects_on_lane = utils::path_safety_checker::createTargetObjectsOnLane(

planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp

@@ -119,6 +119,11 @@ LaneChangeModuleManager::LaneChangeModuleManager(
       getOrDeclareParameter<bool>(*node, parameter("safety_check.allow_loose_check_for_cancel"));
   }
 
+  {
+    p.safety_check.object_filtering.object_check_forward_distance =
+      getOrDeclareParameter<double>(*node, "forward_path_length");
+  }
+
   {
     const auto rss_params = [&node](const std::string & phase) {
       utils::path_safety_checker::RSSparams params;

planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp

@@ -778,173 +778,101 @@ LaneChangeTargetObjects NormalLaneChange::getTargetObjects(
   const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const
 {
   const auto current_pose = getEgoPose();
-  const auto & route_handler = *getRouteHandler();
-  const auto & common_parameters = planner_data_->parameters;
   auto objects = *planner_data_->dynamic_object;
   utils::path_safety_checker::filterObjectsByClass(
     objects, lane_change_parameters_->safety_check.object_filtering.object_types_to_check);
+  const auto & route_handler = getRouteHandler();
+  {
+    using utils::path_safety_checker::filters::position_filter;
+    using
+    const auto path =
+      route_handler->getCenterLinePath(current_lanes, 0.0, std::numeric_limits<double>::max());
+    utils::path_safety_checker::filterObjects(objects, [&](const auto & object) {
+      return !utils::path_safety_checker::filters::position_filter(
+        object, path.points, getEgoPosition(), std::numeric_limits<double>::lowest(), 0.0);
+    });
+  }
 
   // get backward lanes
   const auto backward_length = lane_change_parameters_->backward_lane_length;
   const auto target_backward_lanes = utils::lane_change::getBackwardLanelets(
-    route_handler, target_lanes, current_pose, backward_length);
+    *route_handler, target_lanes, current_pose, backward_length);
 
   // filter objects to get target object index
-  const auto target_obj_index =
-    filterObject(objects, current_lanes, target_lanes, target_backward_lanes);
-
-  LaneChangeTargetObjects target_objects;
-  target_objects.current_lane.reserve(target_obj_index.current_lane.size());
-  target_objects.target_lane.reserve(target_obj_index.target_lane.size());
-  target_objects.other_lane.reserve(target_obj_index.other_lane.size());
-
-  // objects in current lane
-  for (const auto & obj_idx : target_obj_index.current_lane) {
-    const auto extended_object = utils::lane_change::transform(
-      objects.objects.at(obj_idx), common_parameters, *lane_change_parameters_);
-    target_objects.current_lane.push_back(extended_object);
-  }
-
-  // objects in target lane
-  for (const auto & obj_idx : target_obj_index.target_lane) {
-    const auto extended_object = utils::lane_change::transform(
-      objects.objects.at(obj_idx), common_parameters, *lane_change_parameters_);
-    target_objects.target_lane.push_back(extended_object);
-  }
-
-  // objects in other lane
-  for (const auto & obj_idx : target_obj_index.other_lane) {
-    const auto extended_object = utils::lane_change::transform(
-      objects.objects.at(obj_idx), common_parameters, *lane_change_parameters_);
-    target_objects.other_lane.push_back(extended_object);
-  }
-
-  return target_objects;
+  return filterObject(objects, current_lanes, target_lanes, target_backward_lanes);
 }
 
-LaneChangeTargetObjectIndices NormalLaneChange::filterObject(
+LaneChangeTargetObjects NormalLaneChange::filterObject(
   const PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes,
   const lanelet::ConstLanelets & target_lanes,
   const lanelet::ConstLanelets & target_backward_lanes) const
 {
-  const auto current_pose = getEgoPose();
-  const auto & route_handler = *getRouteHandler();
-  const auto & common_parameters = planner_data_->parameters;
-
   // Guard
   if (objects.objects.empty()) {
     return {};
   }
 
-  // Get path
-  const auto path =
-    route_handler.getCenterLinePath(current_lanes, 0.0, std::numeric_limits<double>::max());
-  const auto target_path =
-    route_handler.getCenterLinePath(target_lanes, 0.0, std::numeric_limits<double>::max());
+  const auto current_pose = getEgoPose();
+  const auto & common_parameters = planner_data_->parameters;
 
-  const auto current_polygon =
-    utils::lane_change::createPolygon(current_lanes, 0.0, std::numeric_limits<double>::max());
   const auto expanded_target_lanes = utils::lane_change::generateExpandedLanelets(
     target_lanes, direction_, lane_change_parameters_->safety_check.lane_expansion_left_offset,
     lane_change_parameters_->safety_check.lane_expansion_right_offset);
-  const auto target_polygon = utils::lane_change::createPolygon(
-    expanded_target_lanes, 0.0, std::numeric_limits<double>::max());
+  const auto lanes_polygon =
+    utils::lane_change::createPolygon(current_lanes, expanded_target_lanes, target_backward_lanes);
+
   const auto dist_ego_to_current_lanes_center =
     lanelet::utils::getLateralDistanceToClosestLanelet(current_lanes, current_pose);
-  std::vector<std::optional<lanelet::BasicPolygon2d>> target_backward_polygons;
-  for (const auto & target_backward_lane : target_backward_lanes) {
-    // Check to see is target_backward_lane is in current_lanes
-    // Without this check, current lane object might be treated as target lane object
-    const auto is_current_lane = [&](const lanelet::ConstLanelet & current_lane) {
-      return current_lane.id() == target_backward_lane.id();
-    };
 
-    if (std::any_of(current_lanes.begin(), current_lanes.end(), is_current_lane)) {
-      continue;
-    }
+  LaneChangeTargetObjects filtered_objects;
+  filtered_objects.current_lane.reserve(objects.objects.size());
+  filtered_objects.target_lane.reserve(objects.objects.size());
+  filtered_objects.other_lane.reserve(objects.objects.size());
 
-    lanelet::ConstLanelets lanelet{target_backward_lane};
-    auto lane_polygon =
-      utils::lane_change::createPolygon(lanelet, 0.0, std::numeric_limits<double>::max());
-    target_backward_polygons.push_back(lane_polygon);
-  }
+  const auto create_extended_object = [&](const auto & object) {
+    return utils::lane_change::transform(object, common_parameters, *lane_change_parameters_);
+  };
 
-  LaneChangeTargetObjectIndices filtered_obj_indices;
-  for (size_t i = 0; i < objects.objects.size(); ++i) {
-    const auto & object = objects.objects.at(i);
-    const auto obj_velocity_norm = std::hypot(
-      object.kinematics.initial_twist_with_covariance.twist.linear.x,
-      object.kinematics.initial_twist_with_covariance.twist.linear.y);
+  for (const auto & object : objects.objects) {
     const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object);
 
-    // calc distance from the current ego position
-    double max_dist_ego_to_obj = std::numeric_limits<double>::lowest();
-    double min_dist_ego_to_obj = std::numeric_limits<double>::max();
-    const auto obj_polygon_outer = obj_polygon.outer();
-    for (const auto & polygon_p : obj_polygon_outer) {
-      const auto obj_p = tier4_autoware_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0);
-      const double dist_ego_to_obj = calcSignedArcLength(path.points, current_pose.position, obj_p);
-      max_dist_ego_to_obj = std::max(dist_ego_to_obj, max_dist_ego_to_obj);
-      min_dist_ego_to_obj = std::min(dist_ego_to_obj, min_dist_ego_to_obj);
-    }
+    const auto check_intersection = [&obj_polygon](const auto & polygon) {
+      return !polygon.empty() && boost::geometry::intersects(polygon, obj_polygon);
+    };
 
-    const auto is_lateral_far = [&]() {
+    const auto is_lateral_far = std::invoke([&]() {
       const auto dist_object_to_current_lanes_center =
         lanelet::utils::getLateralDistanceToClosestLanelet(
           current_lanes, object.kinematics.initial_pose_with_covariance.pose);
       const auto lateral = dist_object_to_current_lanes_center - dist_ego_to_current_lanes_center;
       return std::abs(lateral) > (common_parameters.vehicle_width / 2);
-    };
-
-    // ignore static object that are behind the ego vehicle
-    if (obj_velocity_norm < 1.0 && max_dist_ego_to_obj < 0.0) {
-      continue;
-    }
+    });
 
     // check if the object intersects with target lanes
-    if (target_polygon && boost::geometry::intersects(target_polygon.value(), obj_polygon)) {
-      // TODO(watanabe): ignore static parked object that are in front of the ego vehicle in target
-      // lanes
-
-      if (max_dist_ego_to_obj >= 0 || is_lateral_far()) {
-        filtered_obj_indices.target_lane.push_back(i);
-        continue;
-      }
+    if (check_intersection(lanes_polygon.target_polygon) && is_lateral_far) {
+      filtered_objects.target_lane.push_back(create_extended_object(object));
+      continue;
     }
 
-    const auto check_backward_polygon = [&obj_polygon](const auto & target_backward_polygon) {
-      return target_backward_polygon &&
-             boost::geometry::intersects(target_backward_polygon.value(), obj_polygon);
-    };
-
     // check if the object intersects with target backward lanes
-    if (
-      !target_backward_polygons.empty() &&
-      std::any_of(
-        target_backward_polygons.begin(), target_backward_polygons.end(), check_backward_polygon)) {
-      filtered_obj_indices.target_lane.push_back(i);
+    if (std::any_of(
+          lanes_polygon.target_backward_polygons.begin(),
+          lanes_polygon.target_backward_polygons.end(), check_intersection)) {
+      filtered_objects.target_lane.push_back(create_extended_object(object));
       continue;
     }
 
     // check if the object intersects with current lanes
-    if (
-      current_polygon && boost::geometry::intersects(current_polygon.value(), obj_polygon) &&
-      max_dist_ego_to_obj >= 0.0) {
+    if (check_intersection(lanes_polygon.current_polygon)) {
       // check only the objects that are in front of the ego vehicle
-      filtered_obj_indices.current_lane.push_back(i);
+      filtered_objects.current_lane.push_back(create_extended_object(object));
       continue;
     }
 
-    // ignore all objects that are behind the ego vehicle and not on the current and target
-    // lanes
-    if (max_dist_ego_to_obj < 0.0) {
-      continue;
-    }
-
-    filtered_obj_indices.other_lane.push_back(i);
+    filtered_objects.other_lane.push_back(create_extended_object(object));
   }
 
-  return filtered_obj_indices;
+  return filtered_objects;
 }
 
 std::vector<ExtendedPredictedObject> NormalLaneChange::filterObjectsInTargetLane(

planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp

@@ -1077,7 +1077,8 @@ bool StartPlannerModule::isSafePath() const
 
   // filtering objects with velocity, position and class
   const auto filtered_objects = utils::path_safety_checker::filterObjects(
-    dynamic_object, route_handler, current_lanes, current_pose.position, objects_filtering_params_);
+    dynamic_object, route_handler, current_lanes, current_pose.position,
+    *objects_filtering_params_);
 
   // filtering objects based on the current position's lane
   const auto target_objects_on_lane = utils::path_safety_checker::createTargetObjectsOnLane(