From 44cdcb91cacf6fb53f5e7bdc1e78521108dd689e Mon Sep 17 00:00:00 2001 From: kminoda Date: Mon, 27 Mar 2023 12:52:52 +0900 Subject: [PATCH 1/7] chore(map_loader): add a ROS 2 test Signed-off-by: kminoda --- map/map_loader/CMakeLists.txt | 1 + .../pointcloud_map_loader_module.cpp | 21 +--- .../pointcloud_map_loader_module.hpp | 1 - .../src/pointcloud_map_loader/utils.cpp | 17 +++ .../src/pointcloud_map_loader/utils.hpp | 6 +- .../test_pointcloud_map_loader_module.cpp | 115 ++++++++++++++++++ 6 files changed, 139 insertions(+), 22 deletions(-) create mode 100644 map/map_loader/test/test_pointcloud_map_loader_module.cpp diff --git a/map/map_loader/CMakeLists.txt b/map/map_loader/CMakeLists.txt index 4c3b418239a5c..fe3dec6b9adf2 100644 --- a/map/map_loader/CMakeLists.txt +++ b/map/map_loader/CMakeLists.txt @@ -65,6 +65,7 @@ if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() add_testcase(test/test_sphere_box_overlap.cpp) + add_testcase(test/test_pointcloud_map_loader_module.cpp) endif() install(PROGRAMS diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp index 1c67d25f08f7a..f6002ea05b9da 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp @@ -13,32 +13,13 @@ // limitations under the License. #include "pointcloud_map_loader_module.hpp" +#include "utils.hpp" #include -#include -#include -#include #include #include -sensor_msgs::msg::PointCloud2 downsample( - const sensor_msgs::msg::PointCloud2 & msg_input, const float leaf_size) -{ - pcl::PointCloud::Ptr pcl_input(new pcl::PointCloud); - pcl::PointCloud::Ptr pcl_output(new pcl::PointCloud); - pcl::fromROSMsg(msg_input, *pcl_input); - pcl::VoxelGrid filter; - filter.setInputCloud(pcl_input); - filter.setLeafSize(leaf_size, leaf_size, leaf_size); - filter.filter(*pcl_output); - - sensor_msgs::msg::PointCloud2 msg_output; - pcl::toROSMsg(*pcl_output, msg_output); - msg_output.header = msg_input.header; - return msg_output; -} - PointcloudMapLoaderModule::PointcloudMapLoaderModule( rclcpp::Node * node, const std::vector & pcd_paths, const std::string & publisher_name, const bool use_downsample) diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.hpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.hpp index aebc5a17454ab..3885bec1c5140 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.hpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.hpp @@ -18,7 +18,6 @@ #include #include - #include #include diff --git a/map/map_loader/src/pointcloud_map_loader/utils.cpp b/map/map_loader/src/pointcloud_map_loader/utils.cpp index 8af8c0dfd8ec0..4f27873763bf9 100644 --- a/map/map_loader/src/pointcloud_map_loader/utils.cpp +++ b/map/map_loader/src/pointcloud_map_loader/utils.cpp @@ -70,3 +70,20 @@ bool isGridWithinQueriedArea( bool res = sphereAndBoxOverlapExists(center, radius, metadata.min, metadata.max); return res; } + +sensor_msgs::msg::PointCloud2 downsample( + const sensor_msgs::msg::PointCloud2 & msg_input, const float leaf_size) +{ + pcl::PointCloud::Ptr pcl_input(new pcl::PointCloud); + pcl::PointCloud::Ptr pcl_output(new pcl::PointCloud); + pcl::fromROSMsg(msg_input, *pcl_input); + pcl::VoxelGrid filter; + filter.setInputCloud(pcl_input); + filter.setLeafSize(leaf_size, leaf_size, leaf_size); + filter.filter(*pcl_output); + + sensor_msgs::msg::PointCloud2 msg_output; + pcl::toROSMsg(*pcl_output, msg_output); + msg_output.header = msg_input.header; + return msg_output; +} \ No newline at end of file diff --git a/map/map_loader/src/pointcloud_map_loader/utils.hpp b/map/map_loader/src/pointcloud_map_loader/utils.hpp index 0a0d2912bb8c5..2ac79535a25c1 100644 --- a/map/map_loader/src/pointcloud_map_loader/utils.hpp +++ b/map/map_loader/src/pointcloud_map_loader/utils.hpp @@ -17,7 +17,9 @@ #include #include - +#include +#include +#include #include struct PCDFileMetadata @@ -31,5 +33,7 @@ bool sphereAndBoxOverlapExists( const pcl::PointXYZ position_max); bool isGridWithinQueriedArea( const autoware_map_msgs::msg::AreaInfo area, const PCDFileMetadata metadata); +sensor_msgs::msg::PointCloud2 downsample( + const sensor_msgs::msg::PointCloud2 & msg_input, const float leaf_size); #endif // POINTCLOUD_MAP_LOADER__UTILS_HPP_ diff --git a/map/map_loader/test/test_pointcloud_map_loader_module.cpp b/map/map_loader/test/test_pointcloud_map_loader_module.cpp new file mode 100644 index 0000000000000..af46c939771e3 --- /dev/null +++ b/map/map_loader/test/test_pointcloud_map_loader_module.cpp @@ -0,0 +1,115 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 +#include + +#include "../src/pointcloud_map_loader/pointcloud_map_loader_module.hpp" +#include "../src/pointcloud_map_loader/utils.hpp" + + +using namespace std::chrono_literals; + +class TestPointcloudMapLoaderModule : public ::testing::Test { +protected: + rclcpp::Node::SharedPtr node; + std::string temp_pcd_path; + + void SetUp() override { + rclcpp::init(0, nullptr); + node = rclcpp::Node::make_shared("test_pointcloud_map_loader_module"); + + // Create a temporary PCD file with dummy point cloud data + pcl::PointCloud cloud; + cloud.width = 5; + cloud.height = 1; + cloud.is_dense = false; + cloud.points.resize(cloud.width * cloud.height); + + for (size_t i = 0; i < cloud.points.size(); ++i) { + cloud.points[i].x = static_cast(i); + cloud.points[i].y = static_cast(i * 2); + cloud.points[i].z = static_cast(i * 3); + } + + temp_pcd_path = "/tmp/test_pointcloud_map_loader_module.pcd"; + pcl::io::savePCDFileASCII(temp_pcd_path, cloud); + } + + void TearDown() override { + // Delete the temporary PCD file + std::filesystem::remove(temp_pcd_path); + + rclcpp::shutdown(); + } +}; + +TEST_F(TestPointcloudMapLoaderModule, LoadPCDFilesNoDownsampleTest) { + // Prepare PCD paths + std::vector pcd_paths = {temp_pcd_path}; + + // Create PointcloudMapLoaderModule instance + PointcloudMapLoaderModule loader(node.get(), pcd_paths, "pointcloud_map_no_downsample", false); + + // Create a subscriber to the published pointcloud topic + auto pointcloud_received = std::make_shared(false); + auto pointcloud_msg = std::make_shared(); + + rclcpp::QoS durable_qos{10}; + durable_qos.transient_local(); // Match publisher's Durability setting + + auto pointcloud_sub = node->create_subscription( + "pointcloud_map_no_downsample", + durable_qos, + [pointcloud_received, pointcloud_msg](const sensor_msgs::msg::PointCloud2::SharedPtr msg) { + *pointcloud_received = true; + *pointcloud_msg = *msg; + }); + + // Spin until pointcloud is received or timeout occurs + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + auto start_time = node->now(); + while (!*pointcloud_received && (node->now() - start_time).seconds() < 3) { + executor.spin_some(50ms); + } + + // Check if the point cloud is received and the content is as expected + ASSERT_TRUE(*pointcloud_received); + + // Convert the received point cloud to pcl::PointCloudpcl::PointXYZ + pcl::PointCloud received_cloud; + pcl::fromROSMsg(*pointcloud_msg, received_cloud); + + // Check if the received point cloud matches the content of the temporary PCD file + ASSERT_EQ(static_cast(received_cloud.width), 5); + ASSERT_EQ(static_cast(received_cloud.height), 1); + + for (size_t i = 0; i < received_cloud.points.size(); ++i) { + EXPECT_FLOAT_EQ(received_cloud.points[i].x, static_cast(i)); + EXPECT_FLOAT_EQ(received_cloud.points[i].y, static_cast(i * 2)); + EXPECT_FLOAT_EQ(received_cloud.points[i].z, static_cast(i * 3)); + } +} + +int main(int argc, char **argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} \ No newline at end of file From 058682cb9a8c429a395eb1badc89cec72c81d0f8 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 27 Mar 2023 03:54:35 +0000 Subject: [PATCH 2/7] style(pre-commit): autofix --- .../pointcloud_map_loader_module.cpp | 1 + .../pointcloud_map_loader_module.hpp | 1 + .../src/pointcloud_map_loader/utils.cpp | 2 +- .../src/pointcloud_map_loader/utils.hpp | 3 +- .../test_pointcloud_map_loader_module.cpp | 36 +++++++++++-------- 5 files changed, 26 insertions(+), 17 deletions(-) diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp index f6002ea05b9da..159a6f9cb0be9 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include "pointcloud_map_loader_module.hpp" + #include "utils.hpp" #include diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.hpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.hpp index 3885bec1c5140..aebc5a17454ab 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.hpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.hpp @@ -18,6 +18,7 @@ #include #include + #include #include diff --git a/map/map_loader/src/pointcloud_map_loader/utils.cpp b/map/map_loader/src/pointcloud_map_loader/utils.cpp index 4f27873763bf9..12dd09a81a39d 100644 --- a/map/map_loader/src/pointcloud_map_loader/utils.cpp +++ b/map/map_loader/src/pointcloud_map_loader/utils.cpp @@ -86,4 +86,4 @@ sensor_msgs::msg::PointCloud2 downsample( pcl::toROSMsg(*pcl_output, msg_output); msg_output.header = msg_input.header; return msg_output; -} \ No newline at end of file +} diff --git a/map/map_loader/src/pointcloud_map_loader/utils.hpp b/map/map_loader/src/pointcloud_map_loader/utils.hpp index 2ac79535a25c1..2ec5d1e2b8361 100644 --- a/map/map_loader/src/pointcloud_map_loader/utils.hpp +++ b/map/map_loader/src/pointcloud_map_loader/utils.hpp @@ -17,10 +17,11 @@ #include #include + +#include #include #include #include -#include struct PCDFileMetadata { diff --git a/map/map_loader/test/test_pointcloud_map_loader_module.cpp b/map/map_loader/test/test_pointcloud_map_loader_module.cpp index af46c939771e3..c3bf8fdf5ddb4 100644 --- a/map/map_loader/test/test_pointcloud_map_loader_module.cpp +++ b/map/map_loader/test/test_pointcloud_map_loader_module.cpp @@ -12,27 +12,31 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "../src/pointcloud_map_loader/pointcloud_map_loader_module.hpp" +#include "../src/pointcloud_map_loader/utils.hpp" + #include + #include + +#include #include #include + +#include #include #include -#include - -#include "../src/pointcloud_map_loader/pointcloud_map_loader_module.hpp" -#include "../src/pointcloud_map_loader/utils.hpp" - using namespace std::chrono_literals; -class TestPointcloudMapLoaderModule : public ::testing::Test { +class TestPointcloudMapLoaderModule : public ::testing::Test +{ protected: rclcpp::Node::SharedPtr node; std::string temp_pcd_path; - void SetUp() override { + void SetUp() override + { rclcpp::init(0, nullptr); node = rclcpp::Node::make_shared("test_pointcloud_map_loader_module"); @@ -53,7 +57,8 @@ class TestPointcloudMapLoaderModule : public ::testing::Test { pcl::io::savePCDFileASCII(temp_pcd_path, cloud); } - void TearDown() override { + void TearDown() override + { // Delete the temporary PCD file std::filesystem::remove(temp_pcd_path); @@ -61,7 +66,8 @@ class TestPointcloudMapLoaderModule : public ::testing::Test { } }; -TEST_F(TestPointcloudMapLoaderModule, LoadPCDFilesNoDownsampleTest) { +TEST_F(TestPointcloudMapLoaderModule, LoadPCDFilesNoDownsampleTest) +{ // Prepare PCD paths std::vector pcd_paths = {temp_pcd_path}; @@ -73,11 +79,10 @@ TEST_F(TestPointcloudMapLoaderModule, LoadPCDFilesNoDownsampleTest) { auto pointcloud_msg = std::make_shared(); rclcpp::QoS durable_qos{10}; - durable_qos.transient_local(); // Match publisher's Durability setting + durable_qos.transient_local(); // Match publisher's Durability setting auto pointcloud_sub = node->create_subscription( - "pointcloud_map_no_downsample", - durable_qos, + "pointcloud_map_no_downsample", durable_qos, [pointcloud_received, pointcloud_msg](const sensor_msgs::msg::PointCloud2::SharedPtr msg) { *pointcloud_received = true; *pointcloud_msg = *msg; @@ -109,7 +114,8 @@ TEST_F(TestPointcloudMapLoaderModule, LoadPCDFilesNoDownsampleTest) { } } -int main(int argc, char **argv) { +int main(int argc, char ** argv) +{ testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); -} \ No newline at end of file +} From 199cdfd64103eafdf65fbbe4f6b62ab3b28d4ef2 Mon Sep 17 00:00:00 2001 From: kminoda Date: Mon, 27 Mar 2023 12:59:33 +0900 Subject: [PATCH 3/7] debug Signed-off-by: kminoda --- .../pointcloud_map_loader_module.cpp | 18 +++++++++++++++++- .../pointcloud_map_loader_module.hpp | 4 +++- .../src/pointcloud_map_loader/utils.cpp | 17 ----------------- .../src/pointcloud_map_loader/utils.hpp | 6 ------ 4 files changed, 20 insertions(+), 25 deletions(-) diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp index 159a6f9cb0be9..fc775df719d4d 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp @@ -17,10 +17,26 @@ #include "utils.hpp" #include - #include #include +sensor_msgs::msg::PointCloud2 downsample( + const sensor_msgs::msg::PointCloud2 & msg_input, const float leaf_size) +{ + pcl::PointCloud::Ptr pcl_input(new pcl::PointCloud); + pcl::PointCloud::Ptr pcl_output(new pcl::PointCloud); + pcl::fromROSMsg(msg_input, *pcl_input); + pcl::VoxelGrid filter; + filter.setInputCloud(pcl_input); + filter.setLeafSize(leaf_size, leaf_size, leaf_size); + filter.filter(*pcl_output); + + sensor_msgs::msg::PointCloud2 msg_output; + pcl::toROSMsg(*pcl_output, msg_output); + msg_output.header = msg_input.header; + return msg_output; +} + PointcloudMapLoaderModule::PointcloudMapLoaderModule( rclcpp::Node * node, const std::vector & pcd_paths, const std::string & publisher_name, const bool use_downsample) diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.hpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.hpp index aebc5a17454ab..2e504cf3c48d4 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.hpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.hpp @@ -20,7 +20,9 @@ #include #include - +#include +#include +#include #include #include diff --git a/map/map_loader/src/pointcloud_map_loader/utils.cpp b/map/map_loader/src/pointcloud_map_loader/utils.cpp index 12dd09a81a39d..8af8c0dfd8ec0 100644 --- a/map/map_loader/src/pointcloud_map_loader/utils.cpp +++ b/map/map_loader/src/pointcloud_map_loader/utils.cpp @@ -70,20 +70,3 @@ bool isGridWithinQueriedArea( bool res = sphereAndBoxOverlapExists(center, radius, metadata.min, metadata.max); return res; } - -sensor_msgs::msg::PointCloud2 downsample( - const sensor_msgs::msg::PointCloud2 & msg_input, const float leaf_size) -{ - pcl::PointCloud::Ptr pcl_input(new pcl::PointCloud); - pcl::PointCloud::Ptr pcl_output(new pcl::PointCloud); - pcl::fromROSMsg(msg_input, *pcl_input); - pcl::VoxelGrid filter; - filter.setInputCloud(pcl_input); - filter.setLeafSize(leaf_size, leaf_size, leaf_size); - filter.filter(*pcl_output); - - sensor_msgs::msg::PointCloud2 msg_output; - pcl::toROSMsg(*pcl_output, msg_output); - msg_output.header = msg_input.header; - return msg_output; -} diff --git a/map/map_loader/src/pointcloud_map_loader/utils.hpp b/map/map_loader/src/pointcloud_map_loader/utils.hpp index 2ec5d1e2b8361..a0be6771ca9bd 100644 --- a/map/map_loader/src/pointcloud_map_loader/utils.hpp +++ b/map/map_loader/src/pointcloud_map_loader/utils.hpp @@ -17,11 +17,7 @@ #include #include - #include -#include -#include -#include struct PCDFileMetadata { @@ -34,7 +30,5 @@ bool sphereAndBoxOverlapExists( const pcl::PointXYZ position_max); bool isGridWithinQueriedArea( const autoware_map_msgs::msg::AreaInfo area, const PCDFileMetadata metadata); -sensor_msgs::msg::PointCloud2 downsample( - const sensor_msgs::msg::PointCloud2 & msg_input, const float leaf_size); #endif // POINTCLOUD_MAP_LOADER__UTILS_HPP_ From 3e9ab3fcc1fc26a90445ce3d7f246d53b3f16879 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 27 Mar 2023 04:03:40 +0000 Subject: [PATCH 4/7] style(pre-commit): autofix --- .../src/pointcloud_map_loader/pointcloud_map_loader_module.cpp | 1 + .../src/pointcloud_map_loader/pointcloud_map_loader_module.hpp | 2 ++ map/map_loader/src/pointcloud_map_loader/utils.hpp | 1 + 3 files changed, 4 insertions(+) diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp index fc775df719d4d..b4aa930831a04 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp @@ -17,6 +17,7 @@ #include "utils.hpp" #include + #include #include diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.hpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.hpp index 2e504cf3c48d4..6a87643b57bff 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.hpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.hpp @@ -20,9 +20,11 @@ #include #include + #include #include #include + #include #include diff --git a/map/map_loader/src/pointcloud_map_loader/utils.hpp b/map/map_loader/src/pointcloud_map_loader/utils.hpp index a0be6771ca9bd..0a0d2912bb8c5 100644 --- a/map/map_loader/src/pointcloud_map_loader/utils.hpp +++ b/map/map_loader/src/pointcloud_map_loader/utils.hpp @@ -17,6 +17,7 @@ #include #include + #include struct PCDFileMetadata From 7495ae2486e4c72dfa950eb9475feff3e9910fdb Mon Sep 17 00:00:00 2001 From: kminoda Date: Mon, 27 Mar 2023 14:26:51 +0900 Subject: [PATCH 5/7] added other tests too Signed-off-by: kminoda --- map/map_loader/CMakeLists.txt | 2 + .../test_differential_map_loader_module.cpp | 98 +++++++++++++++++++ .../test/test_partial_map_loader_module.cpp | 95 ++++++++++++++++++ 3 files changed, 195 insertions(+) create mode 100644 map/map_loader/test/test_differential_map_loader_module.cpp create mode 100644 map/map_loader/test/test_partial_map_loader_module.cpp diff --git a/map/map_loader/CMakeLists.txt b/map/map_loader/CMakeLists.txt index fe3dec6b9adf2..b8abb4da84e4a 100644 --- a/map/map_loader/CMakeLists.txt +++ b/map/map_loader/CMakeLists.txt @@ -66,6 +66,8 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() add_testcase(test/test_sphere_box_overlap.cpp) add_testcase(test/test_pointcloud_map_loader_module.cpp) + add_testcase(test/test_partial_map_loader_module.cpp) + add_testcase(test/test_differential_map_loader_module.cpp) endif() install(PROGRAMS diff --git a/map/map_loader/test/test_differential_map_loader_module.cpp b/map/map_loader/test/test_differential_map_loader_module.cpp new file mode 100644 index 0000000000000..85ead7548d131 --- /dev/null +++ b/map/map_loader/test/test_differential_map_loader_module.cpp @@ -0,0 +1,98 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 "../src/pointcloud_map_loader/differential_map_loader_module.hpp" +#include "autoware_map_msgs/srv/get_differential_point_cloud_map.hpp" + +using autoware_map_msgs::srv::GetDifferentialPointCloudMap; + +class TestDifferentialMapLoaderModule : public ::testing::Test +{ +protected: + void SetUp() override + { + // Initialize ROS node + rclcpp::init(0, nullptr); + node_ = std::make_shared("test_differential_map_loader_module"); + + // Generate a sample dummy pointcloud and save it to a file + pcl::PointCloud dummy_cloud; + dummy_cloud.width = 3; + dummy_cloud.height = 1; + dummy_cloud.points.resize(dummy_cloud.width * dummy_cloud.height); + dummy_cloud.points[0] = pcl::PointXYZ(-1.0, -1.0, -1.0); + dummy_cloud.points[1] = pcl::PointXYZ(0.0, 0.0, 0.0); + dummy_cloud.points[2] = pcl::PointXYZ(1.0, 1.0, 1.0); + pcl::io::savePCDFileASCII("/tmp/dummy.pcd", dummy_cloud); + + // Generate a sample dummy pointcloud metadata dictionary + std::map dummy_metadata_dict; + PCDFileMetadata dummy_metadata; + dummy_metadata.min = pcl::PointXYZ(-1.0, -1.0, -1.0); + dummy_metadata.max = pcl::PointXYZ(1.0, 1.0, 1.0); + dummy_metadata_dict["/tmp/dummy.pcd"] = dummy_metadata; + + // Initialize the DifferentialMapLoaderModule with the dummy metadata dictionary + module_ = std::make_shared(node_.get(), dummy_metadata_dict); + + // Create a client for the GetDifferentialPointCloudMap service + client_ = node_->create_client("service/get_differential_pcd_map"); + } + + void TearDown() override + { + rclcpp::shutdown(); + } + + rclcpp::Node::SharedPtr node_; + std::shared_ptr module_; + rclcpp::Client::SharedPtr client_; +}; + +TEST_F(TestDifferentialMapLoaderModule, LoadDifferentialPCDFiles) +{ + // Wait for the GetDifferentialPointCloudMap service to be available + ASSERT_TRUE(client_->wait_for_service(std::chrono::seconds(3))); + + // Prepare a request for the service + auto request = std::make_shared(); + request->area.center.x = 0; + request->area.center.y = 0; + request->area.center.z = 0; + request->area.radius = 2; + request->cached_ids.clear(); + + // Call the service + auto result_future = client_->async_send_request(request); + ASSERT_EQ( + rclcpp::spin_until_future_complete(node_, result_future), + rclcpp::FutureReturnCode::SUCCESS); + + // Check the result + auto result = result_future.get(); + ASSERT_EQ(static_cast(result->new_pointcloud_with_ids.size()), 1); + EXPECT_EQ(result->new_pointcloud_with_ids[0].cell_id, "/tmp/dummy.pcd"); + EXPECT_EQ(static_cast(result->ids_to_remove.size()), 0); +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} \ No newline at end of file diff --git a/map/map_loader/test/test_partial_map_loader_module.cpp b/map/map_loader/test/test_partial_map_loader_module.cpp new file mode 100644 index 0000000000000..194153067bb20 --- /dev/null +++ b/map/map_loader/test/test_partial_map_loader_module.cpp @@ -0,0 +1,95 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 "../src/pointcloud_map_loader/partial_map_loader_module.hpp" +#include "autoware_map_msgs/srv/get_partial_point_cloud_map.hpp" + +using autoware_map_msgs::srv::GetPartialPointCloudMap; + +class TestPartialMapLoaderModule : public ::testing::Test +{ +protected: + void SetUp() override + { + // Initialize ROS node + rclcpp::init(0, nullptr); + node_ = std::make_shared("test_partial_map_loader_module"); + + // Generate a sample dummy pointcloud and save it to a file + pcl::PointCloud dummy_cloud; + dummy_cloud.width = 3; + dummy_cloud.height = 1; + dummy_cloud.points.resize(dummy_cloud.width * dummy_cloud.height); + dummy_cloud.points[0] = pcl::PointXYZ(-1.0, -1.0, -1.0); + dummy_cloud.points[1] = pcl::PointXYZ(0.0, 0.0, 0.0); + dummy_cloud.points[2] = pcl::PointXYZ(1.0, 1.0, 1.0); + pcl::io::savePCDFileASCII("/tmp/dummy.pcd", dummy_cloud); + + // Generate a sample dummy pointcloud metadata dictionary + std::map dummy_metadata_dict; + PCDFileMetadata dummy_metadata; + dummy_metadata.min = pcl::PointXYZ(-1.0, -1.0, -1.0); + dummy_metadata.max = pcl::PointXYZ(1.0, 1.0, 1.0); + dummy_metadata_dict["/tmp/dummy.pcd"] = dummy_metadata; + + // Initialize the PartialMapLoaderModule with the dummy metadata dictionary + module_ = std::make_shared(node_.get(), dummy_metadata_dict); + + // Create a client for the GetPartialPointCloudMap service + client_ = node_->create_client("service/get_partial_pcd_map"); + } + + void TearDown() override + { + rclcpp::shutdown(); + } + + rclcpp::Node::SharedPtr node_; + std::shared_ptr module_; + rclcpp::Client::SharedPtr client_; +}; + +TEST_F(TestPartialMapLoaderModule, LoadPartialPCDFiles) +{ + // Wait for the GetPartialPointCloudMap service to be available + ASSERT_TRUE(client_->wait_for_service(std::chrono::seconds(3))); + + // Prepare a request for the service + auto request = std::make_shared(); + request->area.center.x = 0; + request->area.center.y = 0; + request->area.center.z = 0; + request->area.radius = 2; + + // Call the service + auto result_future = client_->async_send_request(request); + ASSERT_EQ( + rclcpp::spin_until_future_complete(node_, result_future), + rclcpp::FutureReturnCode::SUCCESS); + + // Check the result + auto result = result_future.get(); + ASSERT_EQ(static_cast(result->new_pointcloud_with_ids.size()), 1); + EXPECT_EQ(result->new_pointcloud_with_ids[0].cell_id, "/tmp/dummy.pcd"); +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} \ No newline at end of file From 12da6608f032e7a6e91f434c6dfc76254e52eb8e Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 27 Mar 2023 05:28:31 +0000 Subject: [PATCH 6/7] style(pre-commit): autofix --- .../test_differential_map_loader_module.cpp | 26 +++++++++---------- .../test/test_partial_map_loader_module.cpp | 21 +++++++-------- 2 files changed, 23 insertions(+), 24 deletions(-) diff --git a/map/map_loader/test/test_differential_map_loader_module.cpp b/map/map_loader/test/test_differential_map_loader_module.cpp index 85ead7548d131..5742dd851b078 100644 --- a/map/map_loader/test/test_differential_map_loader_module.cpp +++ b/map/map_loader/test/test_differential_map_loader_module.cpp @@ -12,14 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "../src/pointcloud_map_loader/differential_map_loader_module.hpp" + #include -#include -#include -#include "../src/pointcloud_map_loader/differential_map_loader_module.hpp" #include "autoware_map_msgs/srv/get_differential_point_cloud_map.hpp" +#include +#include + +#include + using autoware_map_msgs::srv::GetDifferentialPointCloudMap; class TestDifferentialMapLoaderModule : public ::testing::Test @@ -52,13 +55,11 @@ class TestDifferentialMapLoaderModule : public ::testing::Test module_ = std::make_shared(node_.get(), dummy_metadata_dict); // Create a client for the GetDifferentialPointCloudMap service - client_ = node_->create_client("service/get_differential_pcd_map"); + client_ = + node_->create_client("service/get_differential_pcd_map"); } - void TearDown() override - { - rclcpp::shutdown(); - } + void TearDown() override { rclcpp::shutdown(); } rclcpp::Node::SharedPtr node_; std::shared_ptr module_; @@ -81,8 +82,7 @@ TEST_F(TestDifferentialMapLoaderModule, LoadDifferentialPCDFiles) // Call the service auto result_future = client_->async_send_request(request); ASSERT_EQ( - rclcpp::spin_until_future_complete(node_, result_future), - rclcpp::FutureReturnCode::SUCCESS); + rclcpp::spin_until_future_complete(node_, result_future), rclcpp::FutureReturnCode::SUCCESS); // Check the result auto result = result_future.get(); @@ -91,8 +91,8 @@ TEST_F(TestDifferentialMapLoaderModule, LoadDifferentialPCDFiles) EXPECT_EQ(static_cast(result->ids_to_remove.size()), 0); } -int main(int argc, char **argv) +int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); -} \ No newline at end of file +} diff --git a/map/map_loader/test/test_partial_map_loader_module.cpp b/map/map_loader/test/test_partial_map_loader_module.cpp index 194153067bb20..5aa3a8910a8cb 100644 --- a/map/map_loader/test/test_partial_map_loader_module.cpp +++ b/map/map_loader/test/test_partial_map_loader_module.cpp @@ -12,13 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "../src/pointcloud_map_loader/partial_map_loader_module.hpp" + #include -#include -#include "../src/pointcloud_map_loader/partial_map_loader_module.hpp" #include "autoware_map_msgs/srv/get_partial_point_cloud_map.hpp" +#include + +#include + using autoware_map_msgs::srv::GetPartialPointCloudMap; class TestPartialMapLoaderModule : public ::testing::Test @@ -54,10 +57,7 @@ class TestPartialMapLoaderModule : public ::testing::Test client_ = node_->create_client("service/get_partial_pcd_map"); } - void TearDown() override - { - rclcpp::shutdown(); - } + void TearDown() override { rclcpp::shutdown(); } rclcpp::Node::SharedPtr node_; std::shared_ptr module_; @@ -79,8 +79,7 @@ TEST_F(TestPartialMapLoaderModule, LoadPartialPCDFiles) // Call the service auto result_future = client_->async_send_request(request); ASSERT_EQ( - rclcpp::spin_until_future_complete(node_, result_future), - rclcpp::FutureReturnCode::SUCCESS); + rclcpp::spin_until_future_complete(node_, result_future), rclcpp::FutureReturnCode::SUCCESS); // Check the result auto result = result_future.get(); @@ -88,8 +87,8 @@ TEST_F(TestPartialMapLoaderModule, LoadPartialPCDFiles) EXPECT_EQ(result->new_pointcloud_with_ids[0].cell_id, "/tmp/dummy.pcd"); } -int main(int argc, char **argv) +int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); -} \ No newline at end of file +} From 7fc8d324bbace7f33f23d51f5ed8b7acfb2b3e01 Mon Sep 17 00:00:00 2001 From: kminoda Date: Mon, 27 Mar 2023 15:10:48 +0900 Subject: [PATCH 7/7] fix pre-commit Signed-off-by: kminoda --- map/map_loader/test/test_pointcloud_map_loader_module.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/map/map_loader/test/test_pointcloud_map_loader_module.cpp b/map/map_loader/test/test_pointcloud_map_loader_module.cpp index c3bf8fdf5ddb4..dce7e564956f1 100644 --- a/map/map_loader/test/test_pointcloud_map_loader_module.cpp +++ b/map/map_loader/test/test_pointcloud_map_loader_module.cpp @@ -27,7 +27,7 @@ #include #include -using namespace std::chrono_literals; +using std::chrono_literals::operator""ms; class TestPointcloudMapLoaderModule : public ::testing::Test {