test(map_loader): add a ROS 2 test (#3170)

* chore(map_loader): add a ROS 2 test

Signed-off-by: kminoda <koji.minoda@tier4.jp>

* style(pre-commit): autofix

* debug

Signed-off-by: kminoda <koji.minoda@tier4.jp>

* style(pre-commit): autofix

* added other tests too

Signed-off-by: kminoda <koji.minoda@tier4.jp>

* style(pre-commit): autofix

* fix pre-commit

Signed-off-by: kminoda <koji.minoda@tier4.jp>

---------

Signed-off-by: kminoda <koji.minoda@tier4.jp>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

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

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 <fmt/format.h>
-#include <pcl/filters/voxel_grid.h>
-#include <pcl/io/pcd_io.h>
-#include <pcl_conversions/pcl_conversions.h>
 
 #include <string>
 #include <vector>

map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.hpp

@@ -21,6 +21,10 @@
 
 #include <boost/optional.hpp>
 
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl_conversions/pcl_conversions.h>
+
 #include <string>
 #include <vector>
 

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 <rclcpp/rclcpp.hpp>
+
+#include "autoware_map_msgs/srv/get_differential_point_cloud_map.hpp"
+
+#include <gtest/gtest.h>
+#include <pcl/io/pcd_io.h>
+
+#include <memory>
+
+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<rclcpp::Node>("test_differential_map_loader_module");
+
+    // Generate a sample dummy pointcloud and save it to a file
+    pcl::PointCloud<pcl::PointXYZ> 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<std::string, PCDFileMetadata> 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<DifferentialMapLoaderModule>(node_.get(), dummy_metadata_dict);
+
+    // Create a client for the GetDifferentialPointCloudMap service
+    client_ =
+      node_->create_client<GetDifferentialPointCloudMap>("service/get_differential_pcd_map");
+  }
+
+  void TearDown() override { rclcpp::shutdown(); }
+
+  rclcpp::Node::SharedPtr node_;
+  std::shared_ptr<DifferentialMapLoaderModule> module_;
+  rclcpp::Client<GetDifferentialPointCloudMap>::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<GetDifferentialPointCloudMap::Request>();
+  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<int>(result->new_pointcloud_with_ids.size()), 1);
+  EXPECT_EQ(result->new_pointcloud_with_ids[0].cell_id, "/tmp/dummy.pcd");
+  EXPECT_EQ(static_cast<int>(result->ids_to_remove.size()), 0);
+}
+
+int main(int argc, char ** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

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 <rclcpp/rclcpp.hpp>
+
+#include "autoware_map_msgs/srv/get_partial_point_cloud_map.hpp"
+
+#include <gtest/gtest.h>
+
+#include <memory>
+
+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<rclcpp::Node>("test_partial_map_loader_module");
+
+    // Generate a sample dummy pointcloud and save it to a file
+    pcl::PointCloud<pcl::PointXYZ> 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<std::string, PCDFileMetadata> 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<PartialMapLoaderModule>(node_.get(), dummy_metadata_dict);
+
+    // Create a client for the GetPartialPointCloudMap service
+    client_ = node_->create_client<GetPartialPointCloudMap>("service/get_partial_pcd_map");
+  }
+
+  void TearDown() override { rclcpp::shutdown(); }
+
+  rclcpp::Node::SharedPtr node_;
+  std::shared_ptr<PartialMapLoaderModule> module_;
+  rclcpp::Client<GetPartialPointCloudMap>::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<GetPartialPointCloudMap::Request>();
+  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<int>(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();
+}

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 <rclcpp/rclcpp.hpp>
+
+#include <sensor_msgs/msg/point_cloud2.hpp>
+
+#include <gtest/gtest.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/point_types.h>
+
+#include <chrono>
+#include <filesystem>
+#include <memory>
+
+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<pcl::PointXYZ> 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<float>(i);
+      cloud.points[i].y = static_cast<float>(i * 2);
+      cloud.points[i].z = static_cast<float>(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<std::string> 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<bool>(false);
+  auto pointcloud_msg = std::make_shared<sensor_msgs::msg::PointCloud2>();
+
+  rclcpp::QoS durable_qos{10};
+  durable_qos.transient_local();  // Match publisher's Durability setting
+
+  auto pointcloud_sub = node->create_subscription<sensor_msgs::msg::PointCloud2>(
+    "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<pcl::PointXYZ> 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<int>(received_cloud.width), 5);
+  ASSERT_EQ(static_cast<int>(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<float>(i));
+    EXPECT_FLOAT_EQ(received_cloud.points[i].y, static_cast<float>(i * 2));
+    EXPECT_FLOAT_EQ(received_cloud.points[i].z, static_cast<float>(i * 3));
+  }
+}
+
+int main(int argc, char ** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}