refactor(behavior_path_planner): make object type filter method common

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

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

@@ -68,15 +68,8 @@ struct LaneChangeParameters
   bool check_objects_on_other_lanes{true};
   bool use_all_predicted_path{false};
 
-  // true by default
-  bool check_car{true};         // check object car
-  bool check_truck{true};       // check object truck
-  bool check_bus{true};         // check object bus
-  bool check_trailer{true};     // check object trailer
-  bool check_unknown{true};     // check object unknown
-  bool check_bicycle{true};     // check object bicycle
-  bool check_motorcycle{true};  // check object motorbike
-  bool check_pedestrian{true};  // check object pedestrian
+  // true by default for all objects
+  utils::path_safety_checker::ObjectTypesToCheck object_types_to_check;
 
   // safety check
   utils::path_safety_checker::RSSparams rss_params{};

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

@@ -142,8 +142,6 @@ lanelet::ConstLanelets getBackwardLanelets(
   const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes,
   const Pose & current_pose, const double backward_length);
 
-bool isTargetObjectType(const PredictedObject & object, const LaneChangeParameters & parameters);
-
 double calcLateralBufferForFiltering(const double vehicle_width, const double lateral_buffer = 0.0);
 
 double calcLateralBufferForFiltering(const double vehicle_width, const double lateral_buffer);

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

@@ -196,6 +196,20 @@ TargetObjectsOnLane createTargetObjectsOnLane(
   const PredictedObjects & filtered_objects,
   const std::shared_ptr<ObjectsFilteringParams> & params);
 
+/**
+ * @brief Determines whether the predicted object type matches any of the target object types
+ * specified by the user.
+ *
+ * @param object The predicted object whose type is to be checked.
+ * @param target_object_types A structure containing boolean flags for each object type that the
+ * user is interested in checking.
+ *
+ * @return Returns true if the predicted object's highest probability label matches any of the
+ * specified target object types.
+ */
+bool isTargetObjectType(
+  const PredictedObject & object, const ObjectTypesToCheck & target_object_types);
+
 }  // namespace behavior_path_planner::utils::path_safety_checker
 
 #endif  // BEHAVIOR_PATH_PLANNER__UTILS__PATH_SAFETY_CHECKER__OBJECTS_FILTERING_HPP_

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

@@ -111,14 +111,16 @@ LaneChangeModuleManager::LaneChangeModuleManager(
   // target object
   {
     std::string ns = "lane_change.target_object.";
-    p.check_car = getOrDeclareParameter<bool>(*node, ns + "car");
-    p.check_truck = getOrDeclareParameter<bool>(*node, ns + "truck");
-    p.check_bus = getOrDeclareParameter<bool>(*node, ns + "bus");
-    p.check_trailer = getOrDeclareParameter<bool>(*node, ns + "trailer");
-    p.check_unknown = getOrDeclareParameter<bool>(*node, ns + "unknown");
-    p.check_bicycle = getOrDeclareParameter<bool>(*node, ns + "bicycle");
-    p.check_motorcycle = getOrDeclareParameter<bool>(*node, ns + "motorcycle");
-    p.check_pedestrian = getOrDeclareParameter<bool>(*node, ns + "pedestrian");
+    p.object_types_to_check.check_car = getOrDeclareParameter<bool>(*node, ns + "car");
+    p.object_types_to_check.check_truck = getOrDeclareParameter<bool>(*node, ns + "truck");
+    p.object_types_to_check.check_bus = getOrDeclareParameter<bool>(*node, ns + "bus");
+    p.object_types_to_check.check_trailer = getOrDeclareParameter<bool>(*node, ns + "trailer");
+    p.object_types_to_check.check_unknown = getOrDeclareParameter<bool>(*node, ns + "unknown");
+    p.object_types_to_check.check_bicycle = getOrDeclareParameter<bool>(*node, ns + "bicycle");
+    p.object_types_to_check.check_motorcycle =
+      getOrDeclareParameter<bool>(*node, ns + "motorcycle");
+    p.object_types_to_check.check_pedestrian =
+      getOrDeclareParameter<bool>(*node, ns + "pedestrian");
   }
 
   // lane change cancel

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

@@ -672,20 +672,20 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject(
   const auto dist_ego_to_current_lanes_center =
     lanelet::utils::getLateralDistanceToClosestLanelet(current_lanes, current_pose);
 
+  auto filtered_objects = objects;
+
+  utils::path_safety_checker::filterObjectsByClass(
+    filtered_objects, lane_change_parameters_->object_types_to_check);
+
   LaneChangeTargetObjectIndices filtered_obj_indices;
-  for (size_t i = 0; i < objects.objects.size(); ++i) {
-    const auto & object = objects.objects.at(i);
+  for (size_t i = 0; i < filtered_objects.objects.size(); ++i) {
+    const auto & object = filtered_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);
     const auto extended_object =
       utils::lane_change::transform(object, common_parameters, *lane_change_parameters_);
 
-    // ignore specific object types
-    if (!utils::lane_change::isTargetObjectType(object, *lane_change_parameters_)) {
-      continue;
-    }
-
     const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object);
 
     // calc distance from the current ego position

planning/behavior_path_planner/src/utils/lane_change/utils.cpp

@@ -657,22 +657,6 @@ lanelet::ConstLanelets getBackwardLanelets(
   return backward_lanes;
 }
 
-bool isTargetObjectType(const PredictedObject & object, const LaneChangeParameters & parameters)
-{
-  using autoware_auto_perception_msgs::msg::ObjectClassification;
-  const auto t = utils::getHighestProbLabel(object.classification);
-  const auto is_object_type =
-    ((t == ObjectClassification::CAR && parameters.check_car) ||
-     (t == ObjectClassification::TRUCK && parameters.check_truck) ||
-     (t == ObjectClassification::BUS && parameters.check_bus) ||
-     (t == ObjectClassification::TRAILER && parameters.check_trailer) ||
-     (t == ObjectClassification::UNKNOWN && parameters.check_unknown) ||
-     (t == ObjectClassification::BICYCLE && parameters.check_bicycle) ||
-     (t == ObjectClassification::MOTORCYCLE && parameters.check_motorcycle) ||
-     (t == ObjectClassification::PEDESTRIAN && parameters.check_pedestrian));
-  return is_object_type;
-}
-
 double calcLateralBufferForFiltering(const double vehicle_width, const double lateral_buffer)
 {
   return lateral_buffer + 0.5 * vehicle_width;

planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp

@@ -111,21 +111,10 @@ void filterObjectsByPosition(
 void filterObjectsByClass(
   PredictedObjects & objects, const ObjectTypesToCheck & target_object_types)
 {
-  using autoware_auto_perception_msgs::msg::ObjectClassification;
-
   PredictedObjects filtered_objects;
 
   for (auto & object : objects.objects) {
-    const auto t = utils::getHighestProbLabel(object.classification);
-    const auto is_object_type =
-      ((t == ObjectClassification::CAR && target_object_types.check_car) ||
-       (t == ObjectClassification::TRUCK && target_object_types.check_truck) ||
-       (t == ObjectClassification::BUS && target_object_types.check_bus) ||
-       (t == ObjectClassification::TRAILER && target_object_types.check_trailer) ||
-       (t == ObjectClassification::UNKNOWN && target_object_types.check_unknown) ||
-       (t == ObjectClassification::BICYCLE && target_object_types.check_bicycle) ||
-       (t == ObjectClassification::MOTORCYCLE && target_object_types.check_motorcycle) ||
-       (t == ObjectClassification::PEDESTRIAN && target_object_types.check_pedestrian));
+    const auto is_object_type = isTargetObjectType(object, target_object_types);
 
     // If the object type matches any of the target types, add it to the filtered list
     if (is_object_type) {
@@ -135,8 +124,6 @@ void filterObjectsByClass(
 
   // Replace the original objects with the filtered list
   objects = std::move(filtered_objects);
-
-  return;
 }
 
 std::pair<std::vector<size_t>, std::vector<size_t>> separateObjectIndicesByLanelets(
@@ -366,4 +353,19 @@ TargetObjectsOnLane createTargetObjectsOnLane(
   return target_objects_on_lane;
 }
 
+bool isTargetObjectType(
+  const PredictedObject & object, const ObjectTypesToCheck & target_object_types)
+{
+  using autoware_auto_perception_msgs::msg::ObjectClassification;
+  const auto t = utils::getHighestProbLabel(object.classification);
+  return (
+    (t == ObjectClassification::CAR && target_object_types.check_car) ||
+    (t == ObjectClassification::TRUCK && target_object_types.check_truck) ||
+    (t == ObjectClassification::BUS && target_object_types.check_bus) ||
+    (t == ObjectClassification::TRAILER && target_object_types.check_trailer) ||
+    (t == ObjectClassification::UNKNOWN && target_object_types.check_unknown) ||
+    (t == ObjectClassification::BICYCLE && target_object_types.check_bicycle) ||
+    (t == ObjectClassification::MOTORCYCLE && target_object_types.check_motorcycle) ||
+    (t == ObjectClassification::PEDESTRIAN && target_object_types.check_pedestrian));
+}
 }  // namespace behavior_path_planner::utils::path_safety_checker