diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/CMakeLists.txt b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/CMakeLists.txt index 30d147de3d786..896db96fd573d 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/CMakeLists.txt +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/CMakeLists.txt @@ -9,4 +9,15 @@ ament_auto_add_library(${PROJECT_NAME} SHARED DIRECTORY src ) +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + ament_add_ros_isolated_gtest(test_${PROJECT_NAME} + test/test_object_filtering.cpp + ) + target_link_libraries(test_${PROJECT_NAME} + ${PROJECT_NAME} + ) +endif() + ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/package.xml index 070843cacb2b5..7c03655fd5f57 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/package.xml @@ -27,6 +27,7 @@ tier4_planning_msgs visualization_msgs + ament_cmake_ros ament_lint_auto autoware_lint_common diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp index 0b78aa9707f53..2ab3c09e64edd 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 TIER IV, Inc. +// Copyright 2023-2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -18,75 +18,97 @@ #include +#include + #include #include namespace autoware::motion_velocity_planner::dynamic_obstacle_stop { -/// @brief filter the given predicted objects -/// @param objects predicted objects -/// @param ego_data ego data, including its trajectory and pose -/// @param params parameters -/// @param hysteresis [m] extra distance threshold used for filtering -/// @return filtered predicted objects +bool is_vehicle(const autoware_perception_msgs::msg::PredictedObject & object) +{ + constexpr auto is_vehicle_class = [](const auto & c) { + return c.label == autoware_perception_msgs::msg::ObjectClassification::CAR || + c.label == autoware_perception_msgs::msg::ObjectClassification::BUS || + c.label == autoware_perception_msgs::msg::ObjectClassification::TRUCK || + c.label == autoware_perception_msgs::msg::ObjectClassification::TRAILER || + c.label == autoware_perception_msgs::msg::ObjectClassification::MOTORCYCLE || + c.label == autoware_perception_msgs::msg::ObjectClassification::BICYCLE; + }; + return std::find_if( + object.classification.begin(), object.classification.end(), is_vehicle_class) != + object.classification.end(); +} + +bool is_in_range( + const autoware_perception_msgs::msg::PredictedObject & object, + const TrajectoryPoints & ego_trajectory, const PlannerParam & params, const double hysteresis) +{ + const auto distance = std::abs(motion_utils::calcLateralOffset( + ego_trajectory, object.kinematics.initial_pose_with_covariance.pose.position)); + return distance <= params.minimum_object_distance_from_ego_trajectory + + params.ego_lateral_offset + object.shape.dimensions.y / 2.0 + hysteresis; +}; + +bool is_not_too_close( + const autoware_perception_msgs::msg::PredictedObject & object, const EgoData & ego_data, + const double & ego_longitudinal_offset) +{ + const auto obj_arc_length = motion_utils::calcSignedArcLength( + ego_data.trajectory, ego_data.pose.position, + object.kinematics.initial_pose_with_covariance.pose.position); + return std::abs(obj_arc_length) > ego_data.longitudinal_offset_to_first_trajectory_idx + + ego_longitudinal_offset + object.shape.dimensions.x / 2.0; +}; + +bool is_unavoidable( + const autoware_perception_msgs::msg::PredictedObject & object, + const geometry_msgs::msg::Pose & ego_pose, + const std::optional & ego_earliest_stop_pose, + const PlannerParam & params) +{ + constexpr auto same_direction_diff_threshold = M_PI_2 + M_PI_4; + const auto & o_pose = object.kinematics.initial_pose_with_covariance.pose; + const auto o_yaw = tf2::getYaw(o_pose.orientation); + const auto ego_yaw = tf2::getYaw(ego_pose.orientation); + const auto yaw_diff = std::abs(universe_utils::normalizeRadian(o_yaw - ego_yaw)); + const auto opposite_heading = yaw_diff > same_direction_diff_threshold; + const auto collision_distance_threshold = + params.ego_lateral_offset + object.shape.dimensions.y / 2.0 + params.hysteresis; + const auto lat_distance = + std::abs(universe_utils::calcLateralDeviation(o_pose, ego_pose.position)); + auto has_collision = opposite_heading && lat_distance <= collision_distance_threshold; + if (ego_earliest_stop_pose) { + const auto direction_yaw = std::atan2( + o_pose.position.y - ego_earliest_stop_pose->position.y, + o_pose.position.x - ego_earliest_stop_pose->position.x); + const auto yaw_diff_at_earliest_stop_pose = + std::abs(universe_utils::normalizeRadian(o_yaw - direction_yaw)); + const auto lat_distance_at_earliest_stop_pose = + std::abs(universe_utils::calcLateralDeviation(o_pose, ego_earliest_stop_pose->position)); + const auto collision_at_earliest_stop_pose = + yaw_diff_at_earliest_stop_pose > same_direction_diff_threshold && + lat_distance_at_earliest_stop_pose <= collision_distance_threshold; + has_collision |= collision_at_earliest_stop_pose; + } + return has_collision; +}; + std::vector filter_predicted_objects( const autoware_perception_msgs::msg::PredictedObjects & objects, const EgoData & ego_data, const PlannerParam & params, const double hysteresis) { std::vector filtered_objects; - const auto is_vehicle = [](const auto & o) { - return std::find_if(o.classification.begin(), o.classification.end(), [](const auto & c) { - return c.label == autoware_perception_msgs::msg::ObjectClassification::CAR || - c.label == autoware_perception_msgs::msg::ObjectClassification::BUS || - c.label == autoware_perception_msgs::msg::ObjectClassification::TRUCK || - c.label == autoware_perception_msgs::msg::ObjectClassification::TRAILER || - c.label == autoware_perception_msgs::msg::ObjectClassification::MOTORCYCLE || - c.label == autoware_perception_msgs::msg::ObjectClassification::BICYCLE; - }) != o.classification.end(); - }; - const auto is_in_range = [&](const auto & o) { - const auto distance = std::abs(autoware::motion_utils::calcLateralOffset( - ego_data.trajectory, o.kinematics.initial_pose_with_covariance.pose.position)); - return distance <= params.minimum_object_distance_from_ego_trajectory + - params.ego_lateral_offset + o.shape.dimensions.y / 2.0 + hysteresis; - }; - const auto is_not_too_close = [&](const auto & o) { - const auto obj_arc_length = autoware::motion_utils::calcSignedArcLength( - ego_data.trajectory, ego_data.pose.position, - o.kinematics.initial_pose_with_covariance.pose.position); - return obj_arc_length > ego_data.longitudinal_offset_to_first_trajectory_idx + - params.ego_longitudinal_offset + o.shape.dimensions.x / 2.0; - }; - const auto is_unavoidable = [&](const auto & o) { - const auto & o_pose = o.kinematics.initial_pose_with_covariance.pose; - const auto o_yaw = tf2::getYaw(o_pose.orientation); - const auto ego_yaw = tf2::getYaw(ego_data.pose.orientation); - const auto yaw_diff = std::abs(autoware::universe_utils::normalizeRadian(o_yaw - ego_yaw)); - const auto opposite_heading = yaw_diff > M_PI_2 + M_PI_4; - const auto collision_distance_threshold = - params.ego_lateral_offset + o.shape.dimensions.y / 2.0 + params.hysteresis; - // https://en.wikipedia.org/wiki/Distance_from_a_point_to_a_line#Line_defined_by_point_and_angle - const auto lat_distance = std::abs( - (o_pose.position.y - ego_data.pose.position.y) * std::cos(o_yaw) - - (o_pose.position.x - ego_data.pose.position.x) * std::sin(o_yaw)); - auto has_collision = lat_distance <= collision_distance_threshold; - if (ego_data.earliest_stop_pose) { - const auto collision_at_earliest_stop_pose = - std::abs( - (o_pose.position.y - ego_data.earliest_stop_pose->position.y) * std::cos(o_yaw) - - (o_pose.position.x - ego_data.earliest_stop_pose->position.x) * std::sin(o_yaw)) <= - collision_distance_threshold; - has_collision |= collision_at_earliest_stop_pose; - } - return opposite_heading && has_collision; - }; for (const auto & object : objects.objects) { const auto is_not_too_slow = object.kinematics.initial_twist_with_covariance.twist.linear.x >= params.minimum_object_velocity; if ( - is_vehicle(object) && is_not_too_slow && is_in_range(object) && is_not_too_close(object) && - (!params.ignore_unavoidable_collisions || !is_unavoidable(object))) + is_vehicle(object) && is_not_too_slow && + is_in_range(object, ego_data.trajectory, params, hysteresis) && + is_not_too_close(object, ego_data, params.ego_longitudinal_offset) && + (!params.ignore_unavoidable_collisions || + !is_unavoidable(object, ego_data.pose, ego_data.earliest_stop_pose, params))) filtered_objects.push_back(object); } return filtered_objects; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.hpp index b366b5fc37a5e..d20d6354066c6 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 TIER IV, Inc. +// Copyright 2023-2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -23,6 +23,49 @@ namespace autoware::motion_velocity_planner::dynamic_obstacle_stop { +/// @brief check if the object is a vehicle +/// @details the object classification labels are used to determine if the object is a vehicle +/// (classification probabilities are ignored) +/// @param object a predicted object with a label to check +/// @return true if the object is a vehicle +bool is_vehicle(const autoware_perception_msgs::msg::PredictedObject & object); + +/// @brief check if the object is within the lateral range of the ego trajectory +/// @details the lateral offset of the object position to the ego trajectory is compared to the ego +/// + object width + minimum_object_distance_from_ego_trajectory parameter + hysteresis +/// @param object a predicted object with its pose and the lateral offset of its shape +/// @param ego_trajectory trajectory of the ego vehicle +/// @param params parameters with the minimum lateral range and the ego lateral offset +/// @param hysteresis [m] currently used hysteresis distance +/// @return true if the object is in range of the ego trajectory +bool is_in_range( + const autoware_perception_msgs::msg::PredictedObject & object, + const TrajectoryPoints & ego_trajectory, const PlannerParam & params, const double hysteresis); + +/// @brief check if an object is not too close to the ego vehicle +/// @details the arc length difference of the ego and of the object position on the ego trajectory +/// are compared with the ego and object length +/// @param object a predicted object with its pose and shape +/// @param ego_data ego data with its trajectory, pose, and arc length of the pose on the trajectory +/// @param ego_earliest_stop_pose pose the ego vehicle would reach if it were to stop now +/// @param params parameters with the lateral ego offset distance and the hysteresis distance +/// @return true if the object is in range of the ego vehicle +bool is_not_too_close( + const autoware_perception_msgs::msg::PredictedObject & object, const EgoData & ego_data, + const double & ego_longitudinal_offset); + +/// @brief check if a collision would occur with the object, even if ego were stopping now +/// @details we roughly check for a collision at the current ego pose or the earliest stop pose +/// @param object a predicted object with a label to check +/// @param ego_pose pose of the ego vehicle +/// @param ego_earliest_stop_pose pose the ego vehicle would reach if it were to stop now +/// @param params parameters with the lateral ego offset distance and the hysteresis distance +/// @return true if the object is in range of the ego vehicle +bool is_unavoidable( + const autoware_perception_msgs::msg::PredictedObject & object, + const geometry_msgs::msg::Pose & ego_pose, + const std::optional & ego_earliest_stop_pose, + const PlannerParam & params); /// @brief filter the given predicted objects /// @param objects predicted objects diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/types.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/types.hpp index 294b789a679aa..8cf83f63f4d01 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/types.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/types.hpp @@ -38,22 +38,22 @@ using Rtree = boost::geometry::index::rtree stop_pose{}; + std::optional stop_pose; size_t prev_collisions_nb{}; double z{}; void reset_data() diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/test/test_object_filtering.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/test/test_object_filtering.cpp new file mode 100644 index 0000000000000..3fa179903c23f --- /dev/null +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/test/test_object_filtering.cpp @@ -0,0 +1,211 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "../src/object_filtering.hpp" + +#include + +#include +#include +#include +#include +#include +#include + +#include +#include + +TEST(TestObjectFiltering, isVehicle) +{ + using autoware::motion_velocity_planner::dynamic_obstacle_stop::is_vehicle; + using autoware_perception_msgs::msg::ObjectClassification; + autoware_perception_msgs::msg::PredictedObject object; + EXPECT_NO_THROW(is_vehicle(object)); + ObjectClassification classification; + object.classification = {}; + EXPECT_FALSE(is_vehicle(object)); + classification.label = ObjectClassification::PEDESTRIAN; + object.classification.push_back(classification); + EXPECT_FALSE(is_vehicle(object)); + classification.label = ObjectClassification::UNKNOWN; + object.classification.push_back(classification); + EXPECT_FALSE(is_vehicle(object)); + classification.label = ObjectClassification::TRAILER; + object.classification.push_back(classification); + EXPECT_TRUE(is_vehicle(object)); + object.classification.clear(); + for (const auto label : + {ObjectClassification::CAR, ObjectClassification::BUS, ObjectClassification::BICYCLE, + ObjectClassification::TRUCK, ObjectClassification::TRAILER, + ObjectClassification::MOTORCYCLE}) { + classification.label = label; + object.classification = {classification}; + EXPECT_TRUE(is_vehicle(object)); + } +} + +TEST(TestObjectFiltering, isInRange) +{ + using autoware::motion_velocity_planner::dynamic_obstacle_stop::is_in_range; + autoware_perception_msgs::msg::PredictedObject object; + autoware::motion_velocity_planner::dynamic_obstacle_stop::TrajectoryPoints ego_trajectory; + autoware_planning_msgs::msg::TrajectoryPoint trajectory_p; + autoware::motion_velocity_planner::dynamic_obstacle_stop::PlannerParam params; + EXPECT_NO_THROW(is_in_range(object, ego_trajectory, params, {})); + trajectory_p.pose.position.y = 0.0; + for (auto x = -10.0; x <= 10.0; x += 1.0) { + trajectory_p.pose.position.x = x; + ego_trajectory.push_back(trajectory_p); + } + // object 4m from the ego trajectory + object.kinematics.initial_pose_with_covariance.pose.position.x = 0.0; + object.kinematics.initial_pose_with_covariance.pose.position.y = 4.0; + object.shape.dimensions.y = 2.0; + params.minimum_object_distance_from_ego_trajectory = 1.0; + params.ego_lateral_offset = 1.0; + double hysteresis = 0.0; + EXPECT_FALSE(is_in_range(object, ego_trajectory, params, hysteresis)); + // object 3m from the ego trajectory + object.kinematics.initial_pose_with_covariance.pose.position.y = 3.0; + EXPECT_TRUE(is_in_range(object, ego_trajectory, params, hysteresis)); + hysteresis = 1.0; + EXPECT_TRUE(is_in_range(object, ego_trajectory, params, hysteresis)); + // object 2m from the ego trajectory + object.kinematics.initial_pose_with_covariance.pose.position.y = 2.0; + hysteresis = 0.0; + EXPECT_TRUE(is_in_range(object, ego_trajectory, params, hysteresis)); + hysteresis = 1.0; + EXPECT_TRUE(is_in_range(object, ego_trajectory, params, hysteresis)); + // object exactly on the trajectory + object.kinematics.initial_pose_with_covariance.pose.position.y = 0.0; + for (auto ego_lat_offset = 0.1; ego_lat_offset < 2.0; ego_lat_offset += 0.1) { + for (auto object_width = 0.1; object_width < 2.0; object_width += 0.1) { + for (auto min_dist = 0.1; min_dist < 2.0; min_dist += 0.1) { + for (hysteresis = 0.1; hysteresis < 2.0; hysteresis += 0.1) { + params.ego_lateral_offset = ego_lat_offset; + object.shape.dimensions.y = object_width; + params.minimum_object_distance_from_ego_trajectory = min_dist; + EXPECT_TRUE(is_in_range(object, ego_trajectory, params, hysteresis)); + } + } + } + } +} + +TEST(TestObjectFiltering, isNotTooClose) +{ + using autoware::motion_velocity_planner::dynamic_obstacle_stop::is_not_too_close; + autoware_perception_msgs::msg::PredictedObject object; + autoware::motion_velocity_planner::dynamic_obstacle_stop::EgoData ego_data; + EXPECT_NO_THROW(is_not_too_close(object, ego_data, {})); + + double ego_longitudinal_offset = 1.0; + ego_data.longitudinal_offset_to_first_trajectory_idx = 0.0; + ego_data.pose.position.x = 0.0; + ego_data.pose.position.y = 0.0; + autoware_planning_msgs::msg::TrajectoryPoint trajectory_p; + trajectory_p.pose.position.y = 0.0; + for (auto x = -10.0; x <= 10.0; x += 1.0) { + trajectory_p.pose.position.x = x; + ego_data.trajectory.push_back(trajectory_p); + } + object.shape.dimensions.x = 2.0; + object.kinematics.initial_pose_with_covariance.pose.position.y = 2.0; + // object ego with 1m offset = too close if poses are within 2m of arc length + for (auto obj_x = -2.0; obj_x <= 2.0; obj_x += 0.1) { + object.kinematics.initial_pose_with_covariance.pose.position.x = obj_x; + EXPECT_FALSE(is_not_too_close(object, ego_data, ego_longitudinal_offset)); + } + for (auto obj_x = -2.1; obj_x >= -10.0; obj_x -= 0.1) { + object.kinematics.initial_pose_with_covariance.pose.position.x = obj_x; + EXPECT_TRUE(is_not_too_close(object, ego_data, ego_longitudinal_offset)); + } + for (auto obj_x = 2.1; obj_x <= 10.0; obj_x += 0.1) { + object.kinematics.initial_pose_with_covariance.pose.position.x = obj_x; + EXPECT_TRUE(is_not_too_close(object, ego_data, ego_longitudinal_offset)); + } +} + +TEST(TestObjectFiltering, isUnavoidable) +{ + using autoware::motion_velocity_planner::dynamic_obstacle_stop::is_unavoidable; + autoware_perception_msgs::msg::PredictedObject object; + geometry_msgs::msg::Pose ego_pose; + std::optional ego_earliest_stop_pose; + autoware::motion_velocity_planner::dynamic_obstacle_stop::PlannerParam params; + EXPECT_NO_THROW(is_unavoidable(object, ego_pose, ego_earliest_stop_pose, params)); + + params.ego_lateral_offset = 1.0; + params.hysteresis = 0.0; + + object.kinematics.initial_pose_with_covariance.pose.position.x = 5.0; + object.kinematics.initial_pose_with_covariance.pose.position.y = 0.0; + object.shape.dimensions.y = 2.0; + + ego_pose.position.x = 0.0; + ego_pose.position.y = 0.0; + // ego and object heading in the same direction -> not unavoidable + for (auto ego_yaw = -0.4; ego_yaw <= 0.4; ego_yaw += 0.1) { + ego_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(ego_yaw); + for (auto obj_yaw = -0.4; obj_yaw <= 0.4; obj_yaw += 0.1) { + object.kinematics.initial_pose_with_covariance.pose.orientation = + autoware::universe_utils::createQuaternionFromYaw(obj_yaw); + EXPECT_FALSE(is_unavoidable(object, ego_pose, ego_earliest_stop_pose, params)); + } + } + // ego and object heading in opposite direction -> unavoidable + for (auto ego_yaw = -0.4; ego_yaw <= 0.4; ego_yaw += 0.1) { + ego_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(ego_yaw); + object.kinematics.initial_pose_with_covariance.pose.orientation = + autoware::universe_utils::createQuaternionFromYaw(ego_yaw + M_PI); + EXPECT_TRUE(is_unavoidable(object, ego_pose, ego_earliest_stop_pose, params)); + } + + // shift the object : even if they drive in opposite direction they are no longer aligned + object.kinematics.initial_pose_with_covariance.pose.position.x = 5.0; + object.kinematics.initial_pose_with_covariance.pose.position.y = 5.0; + for (auto ego_yaw = -0.4; ego_yaw <= 0.4; ego_yaw += 0.1) { + ego_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(ego_yaw); + object.kinematics.initial_pose_with_covariance.pose.orientation = + autoware::universe_utils::createQuaternionFromYaw(ego_yaw + M_PI); + EXPECT_FALSE(is_unavoidable(object, ego_pose, ego_earliest_stop_pose, params)); + } + + // perpendicular case + object.kinematics.initial_pose_with_covariance.pose.orientation = + autoware::universe_utils::createQuaternionFromYaw(-M_PI_2); + ego_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(0.0); + EXPECT_FALSE(is_unavoidable(object, ego_pose, ego_earliest_stop_pose, params)); + // with earliest stop pose away from the object path -> no collision + ego_earliest_stop_pose.emplace(); + ego_earliest_stop_pose->position.x = 2.0; + ego_earliest_stop_pose->position.y = 0.0; + EXPECT_FALSE(is_unavoidable(object, ego_pose, ego_earliest_stop_pose, params)); + // with earliest stop pose in on the object path (including the ego and object sizes) -> collision + for (auto x = 3.1; x < 7.0; x += 0.1) { + ego_earliest_stop_pose->position.x = x; + EXPECT_TRUE(is_unavoidable(object, ego_pose, ego_earliest_stop_pose, params)); + } +} + +TEST(TestObjectFiltering, filterPredictedObjects) +{ + using autoware::motion_velocity_planner::dynamic_obstacle_stop::filter_predicted_objects; + autoware_perception_msgs::msg::PredictedObjects objects; + autoware_perception_msgs::msg::PredictedObject object; + autoware::motion_velocity_planner::dynamic_obstacle_stop::EgoData ego_data; + autoware::motion_velocity_planner::dynamic_obstacle_stop::PlannerParam params; + double hysteresis{}; + EXPECT_NO_THROW(filter_predicted_objects(objects, ego_data, params, hysteresis)); +}