diff --git a/perception/autoware_raindrop_cluster_filter/CMakeLists.txt b/perception/autoware_raindrop_cluster_filter/CMakeLists.txt index cc58bd4a80c12..2afead434b15f 100644 --- a/perception/autoware_raindrop_cluster_filter/CMakeLists.txt +++ b/perception/autoware_raindrop_cluster_filter/CMakeLists.txt @@ -22,6 +22,9 @@ if(BUILD_TESTING) list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_uncrustify) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() + ament_auto_add_gtest(low_intensity_cluster_filter_tests + test/test_low_intensity_cluster_filter.cpp + ) endif() ament_auto_package( diff --git a/perception/autoware_raindrop_cluster_filter/package.xml b/perception/autoware_raindrop_cluster_filter/package.xml index 46b7afd8c4a8d..7e331a572e331 100644 --- a/perception/autoware_raindrop_cluster_filter/package.xml +++ b/perception/autoware_raindrop_cluster_filter/package.xml @@ -14,6 +14,8 @@ autoware_cmake autoware_detected_object_validation autoware_lanelet2_extension + autoware_perception_msgs + autoware_point_types autoware_universe_utils message_filters nav_msgs diff --git a/perception/autoware_raindrop_cluster_filter/test/test_low_intensity_cluster_filter.cpp b/perception/autoware_raindrop_cluster_filter/test/test_low_intensity_cluster_filter.cpp new file mode 100644 index 0000000000000..e6d058ef8f437 --- /dev/null +++ b/perception/autoware_raindrop_cluster_filter/test/test_low_intensity_cluster_filter.cpp @@ -0,0 +1,185 @@ +// 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/low_intensity_cluster_filter_node.hpp" + +#include +#include +#include +#include + +#include +#include + +#include + +#include + +using autoware::low_intensity_cluster_filter::LowIntensityClusterFilter; +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_point_types::PointXYZIRC; +using autoware_point_types::PointXYZIRCGenerator; +using point_cloud_msg_wrapper::PointCloud2Modifier; +using tier4_perception_msgs::msg::DetectedObjectsWithFeature; +using tier4_perception_msgs::msg::DetectedObjectWithFeature; + +std::shared_ptr generateTestManager() +{ + auto test_manager = std::make_shared(); + return test_manager; +} + +std::shared_ptr generateNode() +{ + auto node_options = rclcpp::NodeOptions{}; + const auto raindrop_cluster_filter_dir = + ament_index_cpp::get_package_share_directory("autoware_raindrop_cluster_filter"); + node_options.arguments( + {"--ros-args", "--params-file", + raindrop_cluster_filter_dir + "/config/low_intensity_cluster_filter.param.yaml"}); + return std::make_shared(node_options); +} + +DetectedObjectsWithFeature create_cluster( + const float x, const float y, const float z, const float existence_probability, uint8_t label, + const int cluster_size, const int intensity) +{ + DetectedObjectsWithFeature msg; + DetectedObjectWithFeature feature_obj; + feature_obj.object.kinematics.pose_with_covariance.pose.position.x = x; + feature_obj.object.kinematics.pose_with_covariance.pose.position.y = y; + feature_obj.object.kinematics.pose_with_covariance.pose.position.z = z; + feature_obj.object.existence_probability = existence_probability; + feature_obj.object.classification.resize(1); + feature_obj.object.classification[0].label = label; + sensor_msgs::msg::PointCloud2 cluster; + PointCloud2Modifier modifier( + cluster, "base_link"); + for (int i = 0; i < cluster_size; i++) { + PointXYZIRC point; + point.x = x; + point.y = y; + point.z = z; + point.intensity = intensity; + modifier.push_back(std::move(point)); + } + feature_obj.feature.cluster = cluster; + msg.feature_objects.push_back(feature_obj); + msg.header.frame_id = "base_link"; + return msg; +} + +TEST(LowIntensityClusterFilterTest, testClusterMergerEmptyObject) +{ + rclcpp::init(0, nullptr); + const std::string input_cluster = "/input/objects"; + const std::string output_cluster = "/output/objects"; + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + + int counter = 0; + DetectedObjectsWithFeature latest_msgs; + auto callback = [&counter, &latest_msgs](const DetectedObjectsWithFeature::ConstSharedPtr msg) { + latest_msgs = *msg; + ++counter; + }; + test_manager->set_subscriber(output_cluster, callback); + + DetectedObjectsWithFeature msg; + msg.header.frame_id = "base_link"; + test_manager->test_pub_msg(test_target_node, input_cluster, msg); + EXPECT_EQ(counter, 1); + EXPECT_EQ(latest_msgs.feature_objects.size(), 0); + rclcpp::shutdown(); +} + +TEST(LowIntensityClusterFilterTest, testLowIntensityClusterClusterFilterOut) +{ + rclcpp::init(0, nullptr); + const std::string input_cluster = "/input/objects"; + const std::string output_cluster = "/output/objects"; + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + + DetectedObjectsWithFeature msg = + create_cluster(10.0, 5.0, 0.0, 0.0, ObjectClassification::UNKNOWN, 10, 0); + DetectedObjectsWithFeature latest_msg; + auto callback = [&latest_msg](const DetectedObjectsWithFeature::ConstSharedPtr msg) { + latest_msg = *msg; + }; + test_manager->set_subscriber(output_cluster, callback); + test_manager->test_pub_msg(test_target_node, input_cluster, msg); + EXPECT_EQ(latest_msg.feature_objects.size(), 0); + rclcpp::shutdown(); +} + +TEST(LowIntensityClusterFilterTest, testHightIntensityCluster) +{ + rclcpp::init(0, nullptr); + const std::string input_cluster = "/input/objects"; + const std::string output_cluster = "/output/objects"; + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + + DetectedObjectsWithFeature msg = + create_cluster(10.0, 5.0, 0.0, 0.0, ObjectClassification::UNKNOWN, 10, 255); + DetectedObjectsWithFeature latest_msg; + auto callback = [&latest_msg](const DetectedObjectsWithFeature::ConstSharedPtr msg) { + latest_msg = *msg; + }; + test_manager->set_subscriber(output_cluster, callback); + test_manager->test_pub_msg(test_target_node, input_cluster, msg); + EXPECT_EQ(latest_msg.feature_objects.size(), 1); + rclcpp::shutdown(); +} + +TEST(LowIntensityClusterFilterTest, testHightExistenceProbCluster) +{ + rclcpp::init(0, nullptr); + const std::string input_cluster = "/input/objects"; + const std::string output_cluster = "/output/objects"; + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + + DetectedObjectsWithFeature msg = + create_cluster(10.0, 5.0, 0.0, 1.0, ObjectClassification::UNKNOWN, 10, 0); + DetectedObjectsWithFeature latest_msg; + auto callback = [&latest_msg](const DetectedObjectsWithFeature::ConstSharedPtr msg) { + latest_msg = *msg; + }; + test_manager->set_subscriber(output_cluster, callback); + test_manager->test_pub_msg(test_target_node, input_cluster, msg); + EXPECT_EQ(latest_msg.feature_objects.size(), 1); + rclcpp::shutdown(); +} + +TEST(LowIntensityClusterFilterTest, testKnownClassCluster) +{ + rclcpp::init(0, nullptr); + const std::string input_cluster = "/input/objects"; + const std::string output_cluster = "/output/objects"; + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + + DetectedObjectsWithFeature msg = + create_cluster(10.0, 5.0, 0.0, 0.0, ObjectClassification::PEDESTRIAN, 10, 0); + DetectedObjectsWithFeature latest_msg; + auto callback = [&latest_msg](const DetectedObjectsWithFeature::ConstSharedPtr msg) { + latest_msg = *msg; + }; + test_manager->set_subscriber(output_cluster, callback); + test_manager->test_pub_msg(test_target_node, input_cluster, msg); + EXPECT_EQ(latest_msg.feature_objects.size(), 1); + rclcpp::shutdown(); +}