From 4cec64501cec5f60b7838eed6fb6c47f17573187 Mon Sep 17 00:00:00 2001 From: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Date: Thu, 11 Jul 2024 00:11:18 +0900 Subject: [PATCH] test(euclidean_cluster): add unit tests (#7735) * chore(euclidean_cluster): add unit tests Signed-off-by: badai-nguyen * fix: pre-commit Signed-off-by: badai-nguyen * fix: namespace update Signed-off-by: badai-nguyen --------- Signed-off-by: badai-nguyen --- perception/euclidean_cluster/CMakeLists.txt | 8 + perception/euclidean_cluster/package.xml | 3 + ...est_voxel_grid_based_euclidean_cluster.cpp | 173 ++++++++++++++++++ 3 files changed, 184 insertions(+) create mode 100644 perception/euclidean_cluster/test/test_voxel_grid_based_euclidean_cluster.cpp diff --git a/perception/euclidean_cluster/CMakeLists.txt b/perception/euclidean_cluster/CMakeLists.txt index dacbcf460a14a..d27390ae7707a 100644 --- a/perception/euclidean_cluster/CMakeLists.txt +++ b/perception/euclidean_cluster/CMakeLists.txt @@ -55,7 +55,15 @@ rclcpp_components_register_node(${PROJECT_NAME}_voxel_grid_node_core EXECUTABLE voxel_grid_based_euclidean_cluster_node ) +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + ament_auto_add_gtest(test_voxel_grid_based_euclidean_cluster_fusion + test/test_voxel_grid_based_euclidean_cluster.cpp + ) +endif() + ament_auto_package(INSTALL_TO_SHARE launch config + test ) diff --git a/perception/euclidean_cluster/package.xml b/perception/euclidean_cluster/package.xml index 2644786ead061..45b5d05031c72 100644 --- a/perception/euclidean_cluster/package.xml +++ b/perception/euclidean_cluster/package.xml @@ -12,6 +12,7 @@ autoware_cmake autoware_perception_msgs + autoware_point_types autoware_universe_utils geometry_msgs libpcl-all-dev @@ -21,6 +22,8 @@ sensor_msgs tier4_perception_msgs + ament_cmake_gtest + ament_lint_auto autoware_lint_common diff --git a/perception/euclidean_cluster/test/test_voxel_grid_based_euclidean_cluster.cpp b/perception/euclidean_cluster/test/test_voxel_grid_based_euclidean_cluster.cpp new file mode 100644 index 0000000000000..7d2edbe0de0d7 --- /dev/null +++ b/perception/euclidean_cluster/test/test_voxel_grid_based_euclidean_cluster.cpp @@ -0,0 +1,173 @@ +// 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 "autoware/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp" + +#include +#include + +#include +#include +#include + +#include + +using autoware_point_types::PointXYZI; +void setPointCloud2Fields(sensor_msgs::msg::PointCloud2 & pointcloud) +{ + pointcloud.fields.resize(4); + pointcloud.fields[0].name = "x"; + pointcloud.fields[1].name = "y"; + pointcloud.fields[2].name = "z"; + pointcloud.fields[3].name = "intensity"; + pointcloud.fields[0].offset = 0; + pointcloud.fields[1].offset = 4; + pointcloud.fields[2].offset = 8; + pointcloud.fields[3].offset = 12; + pointcloud.fields[0].datatype = sensor_msgs::msg::PointField::FLOAT32; + pointcloud.fields[1].datatype = sensor_msgs::msg::PointField::FLOAT32; + pointcloud.fields[2].datatype = sensor_msgs::msg::PointField::FLOAT32; + pointcloud.fields[3].datatype = sensor_msgs::msg::PointField::FLOAT32; + pointcloud.fields[0].count = 1; + pointcloud.fields[1].count = 1; + pointcloud.fields[2].count = 1; + pointcloud.fields[3].count = 1; + pointcloud.height = 1; + pointcloud.point_step = 16; + pointcloud.is_bigendian = false; + pointcloud.is_dense = true; + pointcloud.header.frame_id = "dummy_frame_id"; + pointcloud.header.stamp.sec = 0; + pointcloud.header.stamp.nanosec = 0; +} + +sensor_msgs::msg::PointCloud2 generateClusterWithinVoxel(const int nb_points) +{ + sensor_msgs::msg::PointCloud2 pointcloud; + setPointCloud2Fields(pointcloud); + pointcloud.data.resize(nb_points * pointcloud.point_step); + + // generate one cluster with specified number of points within 1 voxel + for (int i = 0; i < nb_points; ++i) { + PointXYZI point; + point.x = std::experimental::randint(0, 30) / 100.0; // point.x within 0.0 to 0.3 + point.y = std::experimental::randint(0, 30) / 100.0; // point.y within 0.0 to 0.3 + point.z = std::experimental::randint(0, 30) / 1.0; + point.intensity = 0.0; + memcpy(&pointcloud.data[i * pointcloud.point_step], &point, pointcloud.point_step); + } + pointcloud.width = nb_points; + pointcloud.row_step = pointcloud.point_step * nb_points; + return pointcloud; +} + +// Test case 1: Test case when the input pointcloud has only one cluster with points number equal to +// max_cluster_size +TEST(VoxelGridBasedEuclideanClusterTest, testcase1) +{ + int nb_generated_points = 100; + sensor_msgs::msg::PointCloud2 pointcloud = generateClusterWithinVoxel(nb_generated_points); + + const sensor_msgs::msg::PointCloud2::ConstSharedPtr pointcloud_msg = + std::make_shared(pointcloud); + tier4_perception_msgs::msg::DetectedObjectsWithFeature output; + std::shared_ptr cluster_; + float tolerance = 0.7; + float voxel_leaf_size = 0.3; + int min_points_number_per_voxel = 1; + int min_cluster_size = 1; + int max_cluster_size = 100; + bool use_height = false; + cluster_ = std::make_shared( + use_height, min_cluster_size, max_cluster_size, tolerance, voxel_leaf_size, + min_points_number_per_voxel); + if (cluster_->cluster(pointcloud_msg, output)) { + std::cout << "cluster success" << std::endl; + } else { + std::cout << "cluster failed" << std::endl; + } + std::cout << "number of output clusters " << output.feature_objects.size() << std::endl; + std::cout << "number points of first cluster: " << output.feature_objects[0].feature.cluster.width + << std::endl; + // the output clusters should has only one cluster with nb_generated_points points + EXPECT_EQ(output.feature_objects.size(), 1); + EXPECT_EQ(output.feature_objects[0].feature.cluster.width, nb_generated_points); +} + +// Test case 2: Test case when the input pointcloud has only one cluster with points number less +// than min_cluster_size +TEST(VoxelGridBasedEuclideanClusterTest, testcase2) +{ + int nb_generated_points = 1; + + sensor_msgs::msg::PointCloud2 pointcloud = generateClusterWithinVoxel(nb_generated_points); + + const sensor_msgs::msg::PointCloud2::ConstSharedPtr pointcloud_msg = + std::make_shared(pointcloud); + tier4_perception_msgs::msg::DetectedObjectsWithFeature output; + std::shared_ptr cluster_; + float tolerance = 0.7; + float voxel_leaf_size = 0.3; + int min_points_number_per_voxel = 1; + int min_cluster_size = 2; + int max_cluster_size = 100; + bool use_height = false; + cluster_ = std::make_shared( + use_height, min_cluster_size, max_cluster_size, tolerance, voxel_leaf_size, + min_points_number_per_voxel); + if (cluster_->cluster(pointcloud_msg, output)) { + std::cout << "cluster success" << std::endl; + } else { + std::cout << "cluster failed" << std::endl; + } + std::cout << "number of output clusters " << output.feature_objects.size() << std::endl; + // the output clusters should be empty + EXPECT_EQ(output.feature_objects.size(), 0); +} + +// Test case 3: Test case when the input pointcloud has two clusters with points number greater to +// max_cluster_size +TEST(VoxelGridBasedEuclideanClusterTest, testcase3) +{ + int nb_generated_points = 100; + sensor_msgs::msg::PointCloud2 pointcloud = generateClusterWithinVoxel(nb_generated_points); + + const sensor_msgs::msg::PointCloud2::ConstSharedPtr pointcloud_msg = + std::make_shared(pointcloud); + tier4_perception_msgs::msg::DetectedObjectsWithFeature output; + std::shared_ptr cluster_; + float tolerance = 0.7; + float voxel_leaf_size = 0.3; + int min_points_number_per_voxel = 1; + int min_cluster_size = 1; + int max_cluster_size = 99; // max_cluster_size is less than nb_generated_points + bool use_height = false; + cluster_ = std::make_shared( + use_height, min_cluster_size, max_cluster_size, tolerance, voxel_leaf_size, + min_points_number_per_voxel); + if (cluster_->cluster(pointcloud_msg, output)) { + std::cout << "cluster success" << std::endl; + } else { + std::cout << "cluster failed" << std::endl; + } + std::cout << "number of output clusters " << output.feature_objects.size() << std::endl; + // the output clusters should be emtpy + EXPECT_EQ(output.feature_objects.size(), 0); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}