diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp index e10ba6c274887..c78be341b4e16 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp +++ b/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{}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp index 0c1fff52c74e2..a4c2c2e695752 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp +++ b/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); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp index 344822dce113a..de31011fbf7f6 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp +++ b/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 & 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_ diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp index f4a75daa6c764..e72691ea55391 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp +++ b/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(*node, ns + "car"); - p.check_truck = getOrDeclareParameter(*node, ns + "truck"); - p.check_bus = getOrDeclareParameter(*node, ns + "bus"); - p.check_trailer = getOrDeclareParameter(*node, ns + "trailer"); - p.check_unknown = getOrDeclareParameter(*node, ns + "unknown"); - p.check_bicycle = getOrDeclareParameter(*node, ns + "bicycle"); - p.check_motorcycle = getOrDeclareParameter(*node, ns + "motorcycle"); - p.check_pedestrian = getOrDeclareParameter(*node, ns + "pedestrian"); + p.object_types_to_check.check_car = getOrDeclareParameter(*node, ns + "car"); + p.object_types_to_check.check_truck = getOrDeclareParameter(*node, ns + "truck"); + p.object_types_to_check.check_bus = getOrDeclareParameter(*node, ns + "bus"); + p.object_types_to_check.check_trailer = getOrDeclareParameter(*node, ns + "trailer"); + p.object_types_to_check.check_unknown = getOrDeclareParameter(*node, ns + "unknown"); + p.object_types_to_check.check_bicycle = getOrDeclareParameter(*node, ns + "bicycle"); + p.object_types_to_check.check_motorcycle = + getOrDeclareParameter(*node, ns + "motorcycle"); + p.object_types_to_check.check_pedestrian = + getOrDeclareParameter(*node, ns + "pedestrian"); } // lane change cancel diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index 333231e0ed0d4..376bac3ed65fb 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/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 diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp index 29c139ab5a654..e4fbf41e5af00 100644 --- a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp +++ b/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; diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp index 92e4c9c54d8ea..73de4cf948606 100644 --- a/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp +++ b/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> 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