diff --git a/perception/autoware_cluster_merger/CMakeLists.txt b/perception/autoware_cluster_merger/CMakeLists.txt index ce666be317d6c..13ea807104c32 100644 --- a/perception/autoware_cluster_merger/CMakeLists.txt +++ b/perception/autoware_cluster_merger/CMakeLists.txt @@ -19,6 +19,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(cluster_merger_tests + test/test_cluster_merger.cpp + ) endif() ament_auto_package( diff --git a/perception/autoware_cluster_merger/package.xml b/perception/autoware_cluster_merger/package.xml index 41bcdb27b1387..879cf1da2a565 100644 --- a/perception/autoware_cluster_merger/package.xml +++ b/perception/autoware_cluster_merger/package.xml @@ -12,6 +12,7 @@ ament_cmake_auto + autoware_test_utils autoware_universe_utils geometry_msgs message_filters diff --git a/perception/autoware_cluster_merger/test/test_cluster_merger.cpp b/perception/autoware_cluster_merger/test/test_cluster_merger.cpp new file mode 100644 index 0000000000000..d530832659a2f --- /dev/null +++ b/perception/autoware_cluster_merger/test/test_cluster_merger.cpp @@ -0,0 +1,163 @@ +// 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/cluster_merger_node.hpp" + +#include +#include + +#include + +#include + +using autoware::cluster_merger::ClusterMergerNode; +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 detected_object_validation_dir = + ament_index_cpp::get_package_share_directory("autoware_cluster_merger"); + node_options.arguments( + {"--ros-args", "--params-file", + detected_object_validation_dir + "/config/cluster_merger.param.yaml"}); + return std::make_shared(node_options); +} + +TEST(ClusterMergerTest, testClusterMergerEmptyObject) +{ + rclcpp::init(0, nullptr); + const std::string input_cluster0 = "/input/cluster0"; + const std::string input_cluster1 = "/input/cluster1"; + const std::string output_cluster = "/output/clusters"; + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + + int counter = 0; + auto callback = [&counter](const DetectedObjectsWithFeature::ConstSharedPtr msg) { + (void)msg; + ++counter; + }; + test_manager->set_subscriber(output_cluster, callback); + + DetectedObjectsWithFeature msg0; + DetectedObjectsWithFeature msg1; + msg0.header.frame_id = "base_link"; + msg1.header.frame_id = "base_link"; + + test_manager->test_pub_msg(test_target_node, input_cluster0, msg0); + test_manager->test_pub_msg(test_target_node, input_cluster1, msg1); + + EXPECT_EQ(counter, 1); + + rclcpp::shutdown(); +} + +TEST(ClusterMergerTest, testClusterMergerEmptyWithNotEmpty) +{ + rclcpp::init(0, nullptr); + const std::string input_cluster0 = "/input/cluster0"; + const std::string input_cluster1 = "/input/cluster1"; + const std::string output_cluster = "/output/clusters"; + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + + DetectedObjectsWithFeature msg0; + DetectedObjectsWithFeature msg1; + msg0.header.frame_id = "base_link"; + msg1.header.frame_id = "base_link"; + // Create object for cluster 1 + { + DetectedObjectWithFeature feature_obj; + + feature_obj.object.kinematics.pose_with_covariance.pose.position.x = 10.0; + feature_obj.object.kinematics.pose_with_covariance.pose.position.y = 5.0; + feature_obj.object.existence_probability = 1.0f; + feature_obj.object.classification.resize(1); + msg1.feature_objects.push_back(feature_obj); + } + + 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_cluster0, msg0); + test_manager->test_pub_msg(test_target_node, input_cluster1, msg1); + EXPECT_EQ(latest_msg.feature_objects.size(), 1); + rclcpp::shutdown(); +} + +TEST(ClusterMergerTest, testClusterMergerNotEmptyWithNotEmpty) +{ + rclcpp::init(0, nullptr); + const std::string input_cluster0 = "/input/cluster0"; + const std::string input_cluster1 = "/input/cluster1"; + const std::string output_cluster = "/output/clusters"; + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + + DetectedObjectsWithFeature msg0; + DetectedObjectsWithFeature msg1; + msg0.header.frame_id = "base_link"; + msg1.header.frame_id = "base_link"; + // create objects for cluster0 + + { + DetectedObjectWithFeature feature_obj00; + + feature_obj00.object.kinematics.pose_with_covariance.pose.position.x = 10.0; + feature_obj00.object.kinematics.pose_with_covariance.pose.position.y = 5.0; + feature_obj00.object.existence_probability = 1.0f; + feature_obj00.object.classification.resize(1); + msg0.feature_objects.push_back(feature_obj00); + } + { + DetectedObjectWithFeature feature_obj01; + + feature_obj01.object.kinematics.pose_with_covariance.pose.position.x = 10.0; + feature_obj01.object.kinematics.pose_with_covariance.pose.position.y = 5.0; + feature_obj01.object.existence_probability = 1.0f; + feature_obj01.object.classification.resize(1); + msg0.feature_objects.push_back(feature_obj01); + } + + // Create object for cluster 1 + { + DetectedObjectWithFeature feature_obj10; + + feature_obj10.object.kinematics.pose_with_covariance.pose.position.x = 10.0; + feature_obj10.object.kinematics.pose_with_covariance.pose.position.y = 5.0; + feature_obj10.object.existence_probability = 1.0f; + feature_obj10.object.classification.resize(1); + msg1.feature_objects.push_back(feature_obj10); + } + + 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_cluster0, msg0); + test_manager->test_pub_msg(test_target_node, input_cluster1, msg1); + EXPECT_EQ(latest_msg.feature_objects.size(), 3); + rclcpp::shutdown(); +}