From 62d5f8985aab64e580fda2b98dd728c27495684a Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Fri, 28 Jun 2024 01:37:23 +0900 Subject: [PATCH 1/3] chore(euclidean_cluster): add unit tests Signed-off-by: badai-nguyen --- perception/euclidean_cluster/CMakeLists.txt | 8 + perception/euclidean_cluster/package.xml | 3 + ...est_voxel_grid_based_euclidean_cluster.cpp | 172 ++++++++++++++++++ 3 files changed, 183 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 fe1fa9fda5fca..8f66f2cb09a14 100644 --- a/perception/euclidean_cluster/CMakeLists.txt +++ b/perception/euclidean_cluster/CMakeLists.txt @@ -55,7 +55,15 @@ rclcpp_components_register_node(voxel_grid_based_euclidean_cluster_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 c5cdd9c54b988..3c972929d7356 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 compare_map_segmentation 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..eed7bbfe3075c --- /dev/null +++ b/perception/euclidean_cluster/test/test_voxel_grid_based_euclidean_cluster.cpp @@ -0,0 +1,172 @@ +// 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 +#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(); +} From bfe172863b95b1bf28291eb1ee31032747e9abd9 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Wed, 3 Jul 2024 11:51:02 +0900 Subject: [PATCH 2/3] fix: pre-commit Signed-off-by: badai-nguyen --- .../test/test_voxel_grid_based_euclidean_cluster.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) 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 index eed7bbfe3075c..316221d09f48c 100644 --- a/perception/euclidean_cluster/test/test_voxel_grid_based_euclidean_cluster.cpp +++ b/perception/euclidean_cluster/test/test_voxel_grid_based_euclidean_cluster.cpp @@ -95,7 +95,7 @@ TEST(VoxelGridBasedEuclideanClusterTest, testcase1) 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; @@ -129,7 +129,7 @@ TEST(VoxelGridBasedEuclideanClusterTest, testcase2) 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); @@ -159,7 +159,7 @@ TEST(VoxelGridBasedEuclideanClusterTest, testcase3) 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); From 2d21e9b4cdbee7d70487db3a408300dfb8f473bf Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Wed, 10 Jul 2024 22:27:34 +0900 Subject: [PATCH 3/3] fix: namespace update Signed-off-by: badai-nguyen --- perception/euclidean_cluster/package.xml | 1 + .../test_voxel_grid_based_euclidean_cluster.cpp | 15 ++++++++------- 2 files changed, 9 insertions(+), 7 deletions(-) diff --git a/perception/euclidean_cluster/package.xml b/perception/euclidean_cluster/package.xml index 2809f2cab6f27..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 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 index 316221d09f48c..7d2edbe0de0d7 100644 --- a/perception/euclidean_cluster/test/test_voxel_grid_based_euclidean_cluster.cpp +++ b/perception/euclidean_cluster/test/test_voxel_grid_based_euclidean_cluster.cpp @@ -12,8 +12,9 @@ // 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 @@ -81,14 +82,14 @@ TEST(VoxelGridBasedEuclideanClusterTest, testcase1) const sensor_msgs::msg::PointCloud2::ConstSharedPtr pointcloud_msg = std::make_shared(pointcloud); tier4_perception_msgs::msg::DetectedObjectsWithFeature output; - std::shared_ptr cluster_; + 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( + 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)) { @@ -115,14 +116,14 @@ TEST(VoxelGridBasedEuclideanClusterTest, testcase2) const sensor_msgs::msg::PointCloud2::ConstSharedPtr pointcloud_msg = std::make_shared(pointcloud); tier4_perception_msgs::msg::DetectedObjectsWithFeature output; - std::shared_ptr cluster_; + 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( + 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)) { @@ -145,14 +146,14 @@ TEST(VoxelGridBasedEuclideanClusterTest, testcase3) const sensor_msgs::msg::PointCloud2::ConstSharedPtr pointcloud_msg = std::make_shared(pointcloud); tier4_perception_msgs::msg::DetectedObjectsWithFeature output; - std::shared_ptr cluster_; + 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( + 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)) {