diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_objects_filtering.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_objects_filtering.cpp index 88ed968d27b64..e7280c5bc150b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_objects_filtering.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_objects_filtering.cpp @@ -14,12 +14,21 @@ #include "autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "autoware/behavior_path_planner_common/utils/utils.hpp" #include +#include #include +#include + #include #include +#include + +#include +#include +#include using PredictedObject = autoware_perception_msgs::msg::PredictedObject; using PredictedObjects = autoware_perception_msgs::msg::PredictedObjects; @@ -32,10 +41,34 @@ using tier4_planning_msgs::msg::PathPointWithLaneId; using autoware::test_utils::createPose; using autoware::test_utils::generateTrajectory; +using autoware::test_utils::make_lanelet; using autoware::universe_utils::createPoint; +using autoware::universe_utils::createVector3; constexpr double epsilon = 1e-6; +const auto intersection_map = + autoware::test_utils::make_map_bin_msg(autoware::test_utils::get_absolute_path_to_lanelet_map( + "autoware_test_utils", "intersection/lanelet2_map.osm")); + +PredictedObject create_bounding_box_object( + const geometry_msgs::msg::Pose pose, + const geometry_msgs::msg::Vector3 velocity = geometry_msgs::msg::Vector3(), + const double x_dimension = 1.0, const double y_dimension = 1.0, + const std::vector & classification = std::vector()) +{ + PredictedObject object; + object.object_id = autoware::universe_utils::generateUUID(); + object.kinematics.initial_pose_with_covariance.pose = pose; + object.kinematics.initial_twist_with_covariance.twist.linear = velocity; + object.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; + object.shape.dimensions.x = x_dimension; + object.shape.dimensions.y = y_dimension; + object.classification = classification; + + return object; +} + std::vector trajectory_to_path_with_lane_id(const Trajectory & trajectory) { std::vector path_with_lane_id; @@ -81,8 +114,7 @@ TEST(BehaviorPathPlanningObjectsFiltering, position_filter) using autoware::behavior_path_planner::utils::path_safety_checker::filter::position_filter; auto current_pos = createPoint(0.0, 0.0, 0.0); - PredictedObject object; - object.kinematics.initial_pose_with_covariance.pose = createPose(10.0, 0.0, 0.0, 0.0, 0.0, 0.0); + PredictedObject object = create_bounding_box_object(createPose(10.0, 0.0, 0.0, 0.0, 0.0, 0.0)); auto straight_trajectory = generateTrajectory(20, 1.0); double forward_distance = 20.0; double backward_distance = 1.0; @@ -129,23 +161,101 @@ TEST(BehaviorPathPlanningObjectsFiltering, is_within_circle) EXPECT_TRUE(is_within_circle(object_pos, ref_point, search_radius)); } +TEST(BehaviorPathPlanningObjectsFiltering, isCentroidWithinLanelet) +{ + using autoware::behavior_path_planner::utils::path_safety_checker::isCentroidWithinLanelet; + using autoware::behavior_path_planner::utils::path_safety_checker::isCentroidWithinLanelets; + + auto object = create_bounding_box_object(createPose(0.5, 0.0, 0.0, 0.0, 0.0, 0.0)); + auto lanelet = make_lanelet({0.0, 1.0}, {5.0, 1.0}, {0.0, -1.0}, {5.0, -1.0}); + double yaw_threshold = M_PI_2; + + EXPECT_TRUE(isCentroidWithinLanelet(object, lanelet, yaw_threshold)); + + object.kinematics.initial_pose_with_covariance.pose.position.x = 8.0; + EXPECT_FALSE(isCentroidWithinLanelet(object, lanelet, yaw_threshold)); + + lanelet::ConstLanelets target_lanelets; + target_lanelets.push_back(lanelet); + target_lanelets.push_back(make_lanelet({5.0, 1.0}, {10.0, 1.0}, {5.0, -1.0}, {10.0, -1.0})); + EXPECT_TRUE(isCentroidWithinLanelets(object, target_lanelets)); +} + +TEST(BehaviorPathPlanningObjectsFiltering, isPolygonOverlapLanelet) +{ + using autoware::behavior_path_planner::utils::toPolygon2d; + using autoware::behavior_path_planner::utils::path_safety_checker::isPolygonOverlapLanelet; + + PredictedObject object = create_bounding_box_object(createPose(0.5, 0.0, 0.0, 0.0, 0.0, 0.0)); + + auto lanelet = make_lanelet({0.0, 1.0}, {5.0, 1.0}, {0.0, -1.0}, {5.0, -1.0}); + double yaw_threshold = M_PI_2; + + EXPECT_TRUE(isPolygonOverlapLanelet(object, lanelet.polygon2d().basicPolygon())); + EXPECT_TRUE(isPolygonOverlapLanelet(object, toPolygon2d(lanelet))); + EXPECT_TRUE(isPolygonOverlapLanelet(object, lanelet, yaw_threshold)); + + object.kinematics.initial_pose_with_covariance.pose = createPose(10.0, 10.0, 0.0, 0.0, 0.0, 0.0); + EXPECT_FALSE(isPolygonOverlapLanelet(object, lanelet.polygon2d().basicPolygon())); + EXPECT_FALSE(isPolygonOverlapLanelet(object, toPolygon2d(lanelet))); + EXPECT_FALSE(isPolygonOverlapLanelet(object, lanelet, yaw_threshold)); +} + +TEST(BehaviorPathPlanningObjectsFiltering, filterObjects) +{ + using autoware::behavior_path_planner::utils::path_safety_checker::filterObjects; + using autoware::behavior_path_planner::utils::path_safety_checker::ObjectsFilteringParams; + using autoware::universe_utils::createVector3; + + std::shared_ptr objects = std::make_shared(); + std::shared_ptr route_handler = + std::make_shared(); + std::shared_ptr params = std::make_shared(); + params->ignore_object_velocity_threshold = false; + params->object_check_forward_distance = 20.0; + params->object_check_backward_distance = 10.0; + params->object_types_to_check.check_car = true; + route_handler->setMap(intersection_map); + lanelet::ConstLanelets current_lanes; + + current_lanes.push_back(route_handler->getLaneletsFromId(1000)); + current_lanes.push_back(route_handler->getLaneletsFromId(1010)); + auto current_pose = createPoint(360.22, 600.51, 0.0); + + EXPECT_TRUE( + filterObjects(objects, route_handler, current_lanes, current_pose, params).objects.empty()); + + ObjectClassification classification; + classification.label = ObjectClassification::Type::CAR; + classification.probability = 1.0; + + auto target_object = create_bounding_box_object( + createPose(360.22, 605.51, 0.0, 0.0, 0.0, 0.0), createVector3(1.0, 1.0, 0.0)); + target_object.classification.push_back(classification); + auto other_object = create_bounding_box_object( + createPose(370.22, 600.51, 0.0, 0.0, 0.0, 0.0), createVector3(1.0, 1.0, 0.0)); + other_object.classification.push_back(classification); + + objects->objects.push_back(target_object); + objects->objects.push_back(other_object); + + auto filtered_object = filterObjects(objects, route_handler, current_lanes, current_pose, params); + EXPECT_FALSE(filtered_object.objects.empty()); + EXPECT_EQ(filtered_object.objects.front().object_id, target_object.object_id); +} + TEST(BehaviorPathPlanningObjectsFiltering, filterObjectsByVelocity) { using autoware::behavior_path_planner::utils::path_safety_checker::filterObjectsByVelocity; PredictedObjects objects; - PredictedObject slow_obj; - PredictedObject fast_obj; + auto slow_obj = create_bounding_box_object( + createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0), createVector3(2.0, 0.0, 0.0)); + auto fast_obj = create_bounding_box_object( + createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0), createVector3(10.0, 0.0, 0.0)); double vel_thr = 5.0; - slow_obj.object_id = autoware::universe_utils::generateUUID(); - slow_obj.kinematics.initial_twist_with_covariance.twist.linear.x = 2.0; - slow_obj.kinematics.initial_twist_with_covariance.twist.linear.y = 0.0; objects.objects.push_back(slow_obj); - - fast_obj.object_id = autoware::universe_utils::generateUUID(); - fast_obj.kinematics.initial_twist_with_covariance.twist.linear.x = 10.0; - fast_obj.kinematics.initial_twist_with_covariance.twist.linear.y = 0.0; objects.objects.push_back(fast_obj); auto filtered_obj = filterObjectsByVelocity(objects, vel_thr, false); @@ -176,17 +286,12 @@ TEST(BehaviorPathPlanningObjectsFiltering, filterObjectsByPosition) double search_radius = 10.0; PredictedObjects objects; - PredictedObject far_obj; - PredictedObject near_obj; + auto far_obj = create_bounding_box_object(createPose(50.0, 0.0, 0.0, 0.0, 0.0, 0.0)); + auto near_obj = create_bounding_box_object(createPose(5.0, 0.0, 0.0, 0.0, 0.0, 0.0)); - near_obj.object_id = autoware::universe_utils::generateUUID(); - near_obj.kinematics.initial_pose_with_covariance.pose = createPose(5.0, 0.0, 0.0, 0.0, 0.0, 0.0); objects.objects.push_back(near_obj); - auto target_uuid = near_obj.object_id; - - far_obj.object_id = autoware::universe_utils::generateUUID(); - far_obj.kinematics.initial_pose_with_covariance.pose = createPose(50.0, 0.0, 0.0, 0.0, 0.0, 0.0); objects.objects.push_back(far_obj); + auto target_uuid = near_obj.object_id; filterObjectsByPosition(objects, straight_path, current_pos, forward_distance, backward_distance); @@ -202,6 +307,108 @@ TEST(BehaviorPathPlanningObjectsFiltering, filterObjectsByPosition) EXPECT_EQ(objects.objects.front().object_id, target_uuid); } +TEST(BehaviorPathPlanningObjectsFiltering, separateObjectsByLanelets) +{ + using autoware::behavior_path_planner::utils::path_safety_checker::isPolygonOverlapLanelet; + using autoware::behavior_path_planner::utils::path_safety_checker:: + separateObjectIndicesByLanelets; + using autoware::behavior_path_planner::utils::path_safety_checker::separateObjectsByLanelets; + + double yaw_threshold = M_PI_2; + + auto target_object = create_bounding_box_object(createPose(0.5, 0.0, 0.0, 0.0, 0.0, 0.0)); + auto other_object = create_bounding_box_object(createPose(-1.5, 0.0, 0.0, 0.0, 0.0, 0.0)); + + PredictedObjects objects; + objects.objects.push_back(target_object); + objects.objects.push_back(other_object); + + lanelet::ConstLanelets target_lanelets; + { + auto object_indices = separateObjectIndicesByLanelets( + objects, target_lanelets, + [](const auto & obj, const auto & lane, const auto & yaw_threshold) { + return isPolygonOverlapLanelet(obj, lane, yaw_threshold); + }, + yaw_threshold); + EXPECT_TRUE(object_indices.first.empty()); + EXPECT_TRUE(object_indices.second.empty()); + } + { + target_lanelets.push_back(make_lanelet({0.0, 1.0}, {5.0, 1.0}, {0.0, -1.0}, {5.0, -1.0})); + target_lanelets.push_back(make_lanelet({5.0, 1.0}, {10.0, 1.0}, {5.0, -1.0}, {10.0, -1.0})); + auto object_indices = separateObjectIndicesByLanelets( + objects, target_lanelets, + [](const auto & obj, const auto & lane, const auto & yaw_threshold) { + return isPolygonOverlapLanelet(obj, lane, yaw_threshold); + }, + yaw_threshold); + EXPECT_FALSE(object_indices.first.empty()); + EXPECT_FALSE(object_indices.second.empty()); + EXPECT_EQ(object_indices.first.front(), 0); + EXPECT_EQ(object_indices.second.front(), 1); + + auto filtered_object = separateObjectsByLanelets( + objects, target_lanelets, + [](const auto & obj, const auto & lane, const auto & yaw_threshold) { + return isPolygonOverlapLanelet(obj, lane, yaw_threshold); + }, + yaw_threshold); + EXPECT_FALSE(filtered_object.first.objects.empty()); + EXPECT_FALSE(filtered_object.second.objects.empty()); + EXPECT_EQ(filtered_object.first.objects.front().object_id, target_object.object_id); + EXPECT_EQ(filtered_object.second.objects.front().object_id, other_object.object_id); + } +} + +TEST(BehaviorPathPlanningObjectsFiltering, getPredictedPathFromObj) +{ + using autoware::behavior_path_planner::utils::path_safety_checker::getPredictedPathFromObj; + using autoware::behavior_path_planner::utils::path_safety_checker:: + PoseWithVelocityAndPolygonStamped; + using autoware::behavior_path_planner::utils::path_safety_checker::PredictedPathWithPolygon; + + autoware::behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject object; + std::vector predicted_paths; + PredictedPathWithPolygon predicted_path; + + autoware_perception_msgs::msg::Shape shape; + shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; + shape.dimensions.x = 1.0; + shape.dimensions.y = 1.0; + + double velocity = 1.0; + + const auto path = [&](geometry_msgs::msg::Pose initial_pose) { + std::vector path; + geometry_msgs::msg::Pose pose; + for (size_t i = 0; i < 10; i++) { + auto time = static_cast(i); + pose.position.x = initial_pose.position.x + time * velocity; + pose.position.y = initial_pose.position.y; + PoseWithVelocityAndPolygonStamped obj_pose_with_poly( + time, pose, velocity, autoware::universe_utils::toPolygon2d(pose, shape)); + path.push_back(obj_pose_with_poly); + } + return path; + }; + + for (size_t i = 0; i < 2; i++) { + predicted_path.path = path(createPose(0.0, static_cast(i), 0.0, 0.0, 0.0, 0.0)); + predicted_path.confidence = 0.1f * (static_cast(i) + 1.0f); + predicted_paths.push_back(predicted_path); + } + object.predicted_paths = predicted_paths; + + bool use_all_predicted_path = true; + EXPECT_EQ(getPredictedPathFromObj(object, use_all_predicted_path).size(), 2); + + use_all_predicted_path = false; + auto extracted_path = getPredictedPathFromObj(object, use_all_predicted_path); + EXPECT_EQ(extracted_path.size(), 1); + EXPECT_DOUBLE_EQ(extracted_path.front().path.front().pose.position.y, 1.0); +} + TEST(BehaviorPathPlanningObjectsFiltering, createPredictedPath) { using autoware::behavior_path_planner::utils::path_safety_checker::createPredictedPath; @@ -257,22 +464,14 @@ TEST(BehaviorPathPlanningObjectsFiltering, transform) { using autoware::behavior_path_planner::utils::path_safety_checker::transform; auto velocity = autoware::universe_utils::createVector3(2.0, 0.0, 0.0); - auto angular = autoware::universe_utils::createVector3(0.0, 0.0, 0.0); - PredictedObject obj; - obj.object_id = autoware::universe_utils::generateUUID(); - obj.kinematics.initial_pose_with_covariance.pose = createPose(2.0, 0.0, 0.0, 0.0, 0.0, 0.0); - obj.kinematics.initial_twist_with_covariance.twist = - autoware::universe_utils::createTwist(velocity, angular); + auto obj = create_bounding_box_object( + createPose(2.0, 0.0, 0.0, 0.0, 0.0, 0.0), createVector3(2.0, 0.0, 0.0), 2.0, 1.0); autoware_perception_msgs::msg::PredictedPath predicted_path; auto straight_path = trajectory_to_predicted_path(generateTrajectory(5, 1.0)); straight_path.confidence = 0.6; straight_path.time_step.sec = 1.0; obj.kinematics.predicted_paths.push_back(straight_path); - obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; - obj.shape.dimensions.x = 2.0; - obj.shape.dimensions.y = 1.0; - obj.shape.dimensions.z = 1.0; double safety_check_time_horizon = 2.0; double safety_check_time_resolution = 0.5; @@ -356,6 +555,50 @@ TEST(BehaviorPathPlanningObjectsFiltering, filterObjectsByClass) EXPECT_TRUE(objects.objects.empty()); } +TEST(BehaviorPathPlanningObjectsFiltering, createTargetObjectsOnLane) +{ + using autoware::behavior_path_planner::utils::path_safety_checker::createTargetObjectsOnLane; + using autoware::behavior_path_planner::utils::path_safety_checker::ObjectsFilteringParams; + using autoware::universe_utils::createVector3; + + PredictedObjects objects; + std::shared_ptr route_handler = + std::make_shared(); + std::shared_ptr params = std::make_shared(); + params->object_lane_configuration = {true, true, true, true, true}; + params->include_opposite_lane = true; + params->invert_opposite_lane = false; + params->safety_check_time_horizon = 10.0; + params->safety_check_time_resolution = 1.0; + route_handler->setMap(intersection_map); + lanelet::ConstLanelets current_lanes; + + current_lanes.push_back(route_handler->getLaneletsFromId(1001)); + current_lanes.push_back(route_handler->getLaneletsFromId(1011)); + + ObjectClassification classification; + classification.label = ObjectClassification::Type::CAR; + classification.probability = 1.0; + + PredictedObject current_lane_object = + create_bounding_box_object(createPose(363.64, 565.03, 0.0, 0.0, 0.0, 0.0)); + PredictedObject right_lane_object = + create_bounding_box_object(createPose(366.91, 523.47, 0.0, 0.0, 0.0, 0.0)); + + objects.objects.push_back(current_lane_object); + objects.objects.push_back(right_lane_object); + + auto target_objects_on_lane = + createTargetObjectsOnLane(current_lanes, route_handler, objects, params); + EXPECT_FALSE(target_objects_on_lane.on_current_lane.empty()); + EXPECT_FALSE(target_objects_on_lane.on_right_lane.empty()); + EXPECT_TRUE(target_objects_on_lane.on_left_lane.empty()); + EXPECT_TRUE(target_objects_on_lane.on_other_lane.empty()); + + EXPECT_EQ(target_objects_on_lane.on_current_lane.front().uuid, current_lane_object.object_id); + EXPECT_EQ(target_objects_on_lane.on_right_lane.front().uuid, right_lane_object.object_id); +} + TEST(BehaviorPathPlanningObjectsFiltering, isTargetObjectType) { using autoware::behavior_path_planner::utils::path_safety_checker::isTargetObjectType;