From d25e9a1bbedc176d4e29c70b3ed9586808bdf813 Mon Sep 17 00:00:00 2001 From: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Date: Tue, 24 Sep 2024 18:01:03 +0900 Subject: [PATCH] chore(compare_map_segmentation): add node tests (#8907) * chore(compare_map_segmentation): add test for voxel_based_compare_map_filter Signed-off-by: badai-nguyen * feat: add test for other compare map filter Signed-off-by: badai-nguyen * style(pre-commit): autofix --------- Signed-off-by: badai-nguyen Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../CMakeLists.txt | 24 ++++ .../package.xml | 2 + ...test_distance_based_compare_map_filter.cpp | 111 +++++++++++++++++ ...l_based_approximate_compare_map_filter.cpp | 112 ++++++++++++++++++ .../test_voxel_based_compare_map_filter.cpp | 111 +++++++++++++++++ ...oxel_distance_based_compare_map_filter.cpp | 111 +++++++++++++++++ 6 files changed, 471 insertions(+) create mode 100644 perception/autoware_compare_map_segmentation/test/test_distance_based_compare_map_filter.cpp create mode 100644 perception/autoware_compare_map_segmentation/test/test_voxel_based_approximate_compare_map_filter.cpp create mode 100644 perception/autoware_compare_map_segmentation/test/test_voxel_based_compare_map_filter.cpp create mode 100644 perception/autoware_compare_map_segmentation/test/test_voxel_distance_based_compare_map_filter.cpp diff --git a/perception/autoware_compare_map_segmentation/CMakeLists.txt b/perception/autoware_compare_map_segmentation/CMakeLists.txt index 664b3f3d8066e..f4b363ea42130 100644 --- a/perception/autoware_compare_map_segmentation/CMakeLists.txt +++ b/perception/autoware_compare_map_segmentation/CMakeLists.txt @@ -89,6 +89,30 @@ install( RUNTIME DESTINATION bin ) +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + ament_auto_add_gtest(test_voxel_based_compare_map_filter + test/test_voxel_based_compare_map_filter.cpp + ) + target_link_libraries(test_voxel_based_compare_map_filter ${PROJECT_NAME}) + + ament_auto_add_gtest(test_distance_based_compare_map_filter + test/test_distance_based_compare_map_filter.cpp + ) + target_link_libraries(test_distance_based_compare_map_filter ${PROJECT_NAME}) + + ament_auto_add_gtest(test_voxel_based_approximate_compare_map_filter + test/test_voxel_based_approximate_compare_map_filter.cpp + ) + target_link_libraries(test_voxel_based_approximate_compare_map_filter ${PROJECT_NAME}) + + ament_auto_add_gtest(test_voxel_distance_based_compare_map_filter + test/test_voxel_distance_based_compare_map_filter.cpp + ) + target_link_libraries(test_voxel_distance_based_compare_map_filter ${PROJECT_NAME}) + +endif() ament_auto_package( INSTALL_TO_SHARE launch diff --git a/perception/autoware_compare_map_segmentation/package.xml b/perception/autoware_compare_map_segmentation/package.xml index 32d1039f36f4e..0fc34b697e366 100644 --- a/perception/autoware_compare_map_segmentation/package.xml +++ b/perception/autoware_compare_map_segmentation/package.xml @@ -18,7 +18,9 @@ autoware_cmake autoware_map_msgs + autoware_point_types autoware_pointcloud_preprocessor + autoware_test_utils autoware_universe_utils grid_map_pcl grid_map_ros diff --git a/perception/autoware_compare_map_segmentation/test/test_distance_based_compare_map_filter.cpp b/perception/autoware_compare_map_segmentation/test/test_distance_based_compare_map_filter.cpp new file mode 100644 index 0000000000000..3d4ee272d7eaf --- /dev/null +++ b/perception/autoware_compare_map_segmentation/test/test_distance_based_compare_map_filter.cpp @@ -0,0 +1,111 @@ +// 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/distance_based_compare_map_filter/node.hpp" + +#include +#include +#include +#include + +#include + +#include + +using autoware::compare_map_segmentation::DistanceBasedCompareMapFilterComponent; +using autoware_point_types::PointXYZIRC; +using autoware_point_types::PointXYZIRCGenerator; +using point_cloud_msg_wrapper::PointCloud2Modifier; +using sensor_msgs::msg::PointCloud2; + +std::shared_ptr generateTestManager() +{ + auto test_manager = std::make_shared(); + return test_manager; +}; + +std::shared_ptr generateNode( + const bool use_dynamic_map_loading) +{ + auto node_options = rclcpp::NodeOptions{}; + const auto compare_map_segmentation_dir = + ament_index_cpp::get_package_share_directory("autoware_compare_map_segmentation"); + node_options.arguments( + {"--ros-args", "--params-file", + compare_map_segmentation_dir + "/config/distance_based_compare_map_filter.param.yaml"}); + + node_options.append_parameter_override("use_dynamic_map_loading", use_dynamic_map_loading); + node_options.append_parameter_override("input_frame", "map"); + node_options.append_parameter_override("publish_debug_pcd", true); + + return std::make_unique(node_options); +}; +PointCloud2 create_pointcloud(const int number_of_point) +{ + PointCloud2 pointcloud; + PointCloud2Modifier modifier(pointcloud, "map"); + modifier.resize(number_of_point); + for (int i = 0; i < number_of_point; ++i) { + modifier[i].x = static_cast(i) / 10.0F; + modifier[i].y = 0.0F; + modifier[i].z = 0.0F; + modifier[i].intensity = 0U; + modifier[i].return_type = 0U; + modifier[i].channel = 0U; + } + return pointcloud; +} + +TEST(DistanceBasedCompareMapFilterComponentTest, testEmptyInputPointcloud) +{ + rclcpp::init(0, nullptr); + const std::string input_map_topic = "/map"; + const std::string input_pc_topic = "/input"; + const std::string output_pc_topic = "/output"; + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(false); + + int counter = 0; + PointCloud2 output_pc_msg; + auto callback = [&counter, &output_pc_msg](const PointCloud2::ConstSharedPtr msg) { + output_pc_msg = *msg; + ++counter; + }; + rclcpp::QoS output_qos(5); + output_qos.durability_volatile(); + output_qos.keep_last(1); + output_qos.best_effort(); + test_manager->set_subscriber(output_pc_topic, callback, output_qos); + + // create and publish map pointcloud topic + PointCloud2 input_map_pointcloud = create_pointcloud(10); + rclcpp::QoS map_qos(5); + map_qos.transient_local(); + map_qos.keep_last(1); + map_qos.reliable(); + test_manager->test_pub_msg( + test_target_node, input_map_topic, input_map_pointcloud, map_qos); + + rclcpp::QoS input_qos(10); + input_qos.durability_volatile(); + input_qos.keep_last(1); + input_qos.best_effort(); + // create and publish input pointcloud topic + PointCloud2 input_pc_msg = create_pointcloud(100); + test_manager->test_pub_msg( + test_target_node, input_pc_topic, input_pc_msg, input_qos); + EXPECT_EQ(counter, 1); + EXPECT_GT(output_pc_msg.data.size(), 0); + EXPECT_GT(input_pc_msg.data.size(), output_pc_msg.data.size()); +} diff --git a/perception/autoware_compare_map_segmentation/test/test_voxel_based_approximate_compare_map_filter.cpp b/perception/autoware_compare_map_segmentation/test/test_voxel_based_approximate_compare_map_filter.cpp new file mode 100644 index 0000000000000..e165efa640d15 --- /dev/null +++ b/perception/autoware_compare_map_segmentation/test/test_voxel_based_approximate_compare_map_filter.cpp @@ -0,0 +1,112 @@ +// 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/voxel_based_approximate_compare_map_filter/node.hpp" + +#include +#include +#include +#include + +#include + +#include + +using autoware::compare_map_segmentation::VoxelBasedApproximateCompareMapFilterComponent; +using autoware_point_types::PointXYZIRC; +using autoware_point_types::PointXYZIRCGenerator; +using point_cloud_msg_wrapper::PointCloud2Modifier; +using sensor_msgs::msg::PointCloud2; + +std::shared_ptr generateTestManager() +{ + auto test_manager = std::make_shared(); + return test_manager; +}; + +std::shared_ptr generateNode( + const bool use_dynamic_map_loading) +{ + auto node_options = rclcpp::NodeOptions{}; + const auto compare_map_segmentation_dir = + ament_index_cpp::get_package_share_directory("autoware_compare_map_segmentation"); + node_options.arguments( + {"--ros-args", "--params-file", + compare_map_segmentation_dir + + "/config/voxel_based_approximate_compare_map_filter.param.yaml"}); + + node_options.append_parameter_override("use_dynamic_map_loading", use_dynamic_map_loading); + node_options.append_parameter_override("input_frame", "map"); + node_options.append_parameter_override("publish_debug_pcd", true); + + return std::make_unique(node_options); +}; +PointCloud2 create_pointcloud(const int number_of_point) +{ + PointCloud2 pointcloud; + PointCloud2Modifier modifier(pointcloud, "map"); + modifier.resize(number_of_point); + for (int i = 0; i < number_of_point; ++i) { + modifier[i].x = static_cast(i) / 10.0F; + modifier[i].y = 0.0F; + modifier[i].z = 0.0F; + modifier[i].intensity = 0U; + modifier[i].return_type = 0U; + modifier[i].channel = 0U; + } + return pointcloud; +} + +TEST(VoxelBasedApproximateCompareMapFilterComponentTest, testEmptyInputPointcloud) +{ + rclcpp::init(0, nullptr); + const std::string input_map_topic = "/map"; + const std::string input_pc_topic = "/input"; + const std::string output_pc_topic = "/output"; + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(false); + + int counter = 0; + PointCloud2 output_pc_msg; + auto callback = [&counter, &output_pc_msg](const PointCloud2::ConstSharedPtr msg) { + output_pc_msg = *msg; + ++counter; + }; + rclcpp::QoS output_qos(5); + output_qos.durability_volatile(); + output_qos.keep_last(1); + output_qos.best_effort(); + test_manager->set_subscriber(output_pc_topic, callback, output_qos); + + // create and publish map pointcloud topic + PointCloud2 input_map_pointcloud = create_pointcloud(10); + rclcpp::QoS map_qos(5); + map_qos.transient_local(); + map_qos.keep_last(1); + map_qos.reliable(); + test_manager->test_pub_msg( + test_target_node, input_map_topic, input_map_pointcloud, map_qos); + + rclcpp::QoS input_qos(10); + input_qos.durability_volatile(); + input_qos.keep_last(1); + input_qos.best_effort(); + // create and publish input pointcloud topic + PointCloud2 input_pc_msg = create_pointcloud(100); + test_manager->test_pub_msg( + test_target_node, input_pc_topic, input_pc_msg, input_qos); + EXPECT_EQ(counter, 1); + EXPECT_GT(output_pc_msg.data.size(), 0); + EXPECT_GT(input_pc_msg.data.size(), output_pc_msg.data.size()); +} diff --git a/perception/autoware_compare_map_segmentation/test/test_voxel_based_compare_map_filter.cpp b/perception/autoware_compare_map_segmentation/test/test_voxel_based_compare_map_filter.cpp new file mode 100644 index 0000000000000..654d7443bd8d0 --- /dev/null +++ b/perception/autoware_compare_map_segmentation/test/test_voxel_based_compare_map_filter.cpp @@ -0,0 +1,111 @@ +// 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/voxel_based_compare_map_filter/node.hpp" + +#include +#include +#include +#include + +#include + +#include + +using autoware::compare_map_segmentation::VoxelBasedCompareMapFilterComponent; +using autoware_point_types::PointXYZIRC; +using autoware_point_types::PointXYZIRCGenerator; +using point_cloud_msg_wrapper::PointCloud2Modifier; +using sensor_msgs::msg::PointCloud2; + +std::shared_ptr generateTestManager() +{ + auto test_manager = std::make_shared(); + return test_manager; +}; + +std::shared_ptr generateNode( + const bool use_dynamic_map_loading) +{ + auto node_options = rclcpp::NodeOptions{}; + const auto compare_map_segmentation_dir = + ament_index_cpp::get_package_share_directory("autoware_compare_map_segmentation"); + node_options.arguments( + {"--ros-args", "--params-file", + compare_map_segmentation_dir + "/config/voxel_based_compare_map_filter.param.yaml"}); + + node_options.append_parameter_override("use_dynamic_map_loading", use_dynamic_map_loading); + node_options.append_parameter_override("input_frame", "map"); + node_options.append_parameter_override("publish_debug_pcd", true); + + return std::make_unique(node_options); +}; +PointCloud2 create_pointcloud(const int number_of_point) +{ + PointCloud2 pointcloud; + PointCloud2Modifier modifier(pointcloud, "map"); + modifier.resize(number_of_point); + for (int i = 0; i < number_of_point; ++i) { + modifier[i].x = static_cast(i) / 10.0F; + modifier[i].y = 0.0F; + modifier[i].z = 0.0F; + modifier[i].intensity = 0U; + modifier[i].return_type = 0U; + modifier[i].channel = 0U; + } + return pointcloud; +} + +TEST(VoxelBasedCompareMapFilterComponentTest, testEmptyInputPointcloud) +{ + rclcpp::init(0, nullptr); + const std::string input_map_topic = "/map"; + const std::string input_pc_topic = "/input"; + const std::string output_pc_topic = "/output"; + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(false); + + int counter = 0; + PointCloud2 output_pc_msg; + auto callback = [&counter, &output_pc_msg](const PointCloud2::ConstSharedPtr msg) { + output_pc_msg = *msg; + ++counter; + }; + rclcpp::QoS output_qos(5); + output_qos.durability_volatile(); + output_qos.keep_last(1); + output_qos.best_effort(); + test_manager->set_subscriber(output_pc_topic, callback, output_qos); + + // create and publish map pointcloud topic + PointCloud2 input_map_pointcloud = create_pointcloud(10); + rclcpp::QoS map_qos(5); + map_qos.transient_local(); + map_qos.keep_last(1); + map_qos.reliable(); + test_manager->test_pub_msg( + test_target_node, input_map_topic, input_map_pointcloud, map_qos); + + rclcpp::QoS input_qos(10); + input_qos.durability_volatile(); + input_qos.keep_last(1); + input_qos.best_effort(); + // create and publish input pointcloud topic + PointCloud2 input_pc_msg = create_pointcloud(100); + test_manager->test_pub_msg( + test_target_node, input_pc_topic, input_pc_msg, input_qos); + EXPECT_EQ(counter, 1); + EXPECT_GT(output_pc_msg.data.size(), 0); + EXPECT_GT(input_pc_msg.data.size(), output_pc_msg.data.size()); +} diff --git a/perception/autoware_compare_map_segmentation/test/test_voxel_distance_based_compare_map_filter.cpp b/perception/autoware_compare_map_segmentation/test/test_voxel_distance_based_compare_map_filter.cpp new file mode 100644 index 0000000000000..4440a5eddc426 --- /dev/null +++ b/perception/autoware_compare_map_segmentation/test/test_voxel_distance_based_compare_map_filter.cpp @@ -0,0 +1,111 @@ +// 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/voxel_distance_based_compare_map_filter/node.hpp" + +#include +#include +#include +#include + +#include + +#include + +using autoware::compare_map_segmentation::VoxelDistanceBasedCompareMapFilterComponent; +using autoware_point_types::PointXYZIRC; +using autoware_point_types::PointXYZIRCGenerator; +using point_cloud_msg_wrapper::PointCloud2Modifier; +using sensor_msgs::msg::PointCloud2; + +std::shared_ptr generateTestManager() +{ + auto test_manager = std::make_shared(); + return test_manager; +}; + +std::shared_ptr generateNode( + const bool use_dynamic_map_loading) +{ + auto node_options = rclcpp::NodeOptions{}; + const auto compare_map_segmentation_dir = + ament_index_cpp::get_package_share_directory("autoware_compare_map_segmentation"); + node_options.arguments( + {"--ros-args", "--params-file", + compare_map_segmentation_dir + "/config/voxel_distance_based_compare_map_filter.param.yaml"}); + + node_options.append_parameter_override("use_dynamic_map_loading", use_dynamic_map_loading); + node_options.append_parameter_override("input_frame", "map"); + node_options.append_parameter_override("publish_debug_pcd", true); + + return std::make_unique(node_options); +}; +PointCloud2 create_pointcloud(const int number_of_point) +{ + PointCloud2 pointcloud; + PointCloud2Modifier modifier(pointcloud, "map"); + modifier.resize(number_of_point); + for (int i = 0; i < number_of_point; ++i) { + modifier[i].x = static_cast(i) / 10.0F; + modifier[i].y = 0.0F; + modifier[i].z = 0.0F; + modifier[i].intensity = 0U; + modifier[i].return_type = 0U; + modifier[i].channel = 0U; + } + return pointcloud; +} + +TEST(VoxelDistanceBasedCompareMapFilterComponentTest, testEmptyInputPointcloud) +{ + rclcpp::init(0, nullptr); + const std::string input_map_topic = "/map"; + const std::string input_pc_topic = "/input"; + const std::string output_pc_topic = "/output"; + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(false); + + int counter = 0; + PointCloud2 output_pc_msg; + auto callback = [&counter, &output_pc_msg](const PointCloud2::ConstSharedPtr msg) { + output_pc_msg = *msg; + ++counter; + }; + rclcpp::QoS output_qos(5); + output_qos.durability_volatile(); + output_qos.keep_last(1); + output_qos.best_effort(); + test_manager->set_subscriber(output_pc_topic, callback, output_qos); + + // create and publish map pointcloud topic + PointCloud2 input_map_pointcloud = create_pointcloud(10); + rclcpp::QoS map_qos(5); + map_qos.transient_local(); + map_qos.keep_last(1); + map_qos.reliable(); + test_manager->test_pub_msg( + test_target_node, input_map_topic, input_map_pointcloud, map_qos); + + rclcpp::QoS input_qos(10); + input_qos.durability_volatile(); + input_qos.keep_last(1); + input_qos.best_effort(); + // create and publish input pointcloud topic + PointCloud2 input_pc_msg = create_pointcloud(100); + test_manager->test_pub_msg( + test_target_node, input_pc_topic, input_pc_msg, input_qos); + EXPECT_EQ(counter, 1); + EXPECT_GT(output_pc_msg.data.size(), 0); + EXPECT_GT(input_pc_msg.data.size(), output_pc_msg.data.size()); +}