diff --git a/map/map_loader/CMakeLists.txt b/map/map_loader/CMakeLists.txt index 4c3b418239a5c..b8abb4da84e4a 100644 --- a/map/map_loader/CMakeLists.txt +++ b/map/map_loader/CMakeLists.txt @@ -65,6 +65,9 @@ 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) + 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/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp index 1c67d25f08f7a..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 @@ -14,10 +14,9 @@ #include "pointcloud_map_loader_module.hpp" +#include "utils.hpp" + #include -#include -#include -#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 aebc5a17454ab..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 @@ -21,6 +21,10 @@ #include +#include +#include +#include + #include #include 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..5742dd851b078 --- /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 "../src/pointcloud_map_loader/differential_map_loader_module.hpp" + +#include + +#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 +{ +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(); +} 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..5aa3a8910a8cb --- /dev/null +++ b/map/map_loader/test/test_partial_map_loader_module.cpp @@ -0,0 +1,94 @@ +// 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 "../src/pointcloud_map_loader/partial_map_loader_module.hpp" + +#include + +#include "autoware_map_msgs/srv/get_partial_point_cloud_map.hpp" + +#include + +#include + +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(); +} 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..dce7e564956f1 --- /dev/null +++ b/map/map_loader/test/test_pointcloud_map_loader_module.cpp @@ -0,0 +1,121 @@ +// 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 "../src/pointcloud_map_loader/pointcloud_map_loader_module.hpp" +#include "../src/pointcloud_map_loader/utils.hpp" + +#include + +#include + +#include +#include +#include + +#include +#include +#include + +using std::chrono_literals::operator""ms; + +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(); +}