refactor(map_loader): modularization

map/map_loader/CMakeLists.txt

@@ -8,6 +8,7 @@ find_package(PCL REQUIRED COMPONENTS io)
 
 ament_auto_add_library(pointcloud_map_loader_node SHARED
   src/pointcloud_map_loader/pointcloud_map_loader_node.cpp
+  src/pointcloud_map_loader/pointcloud_map_loader_module.cpp
 )
 target_link_libraries(pointcloud_map_loader_node ${PCL_LIBRARIES})
 

map/map_loader/package.xml

@@ -13,6 +13,7 @@
   <build_depend>autoware_cmake</build_depend>
 
   <depend>autoware_auto_mapping_msgs</depend>
+  <depend>fmt</depend>
   <depend>geometry_msgs</depend>
   <depend>lanelet2_extension</depend>
   <depend>libpcl-all-dev</depend>

map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp

@@ -0,0 +1,75 @@
+// Copyright 2022 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 "pointcloud_map_loader_module.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>
+
+PointcloudMapLoaderModule::PointcloudMapLoaderModule(
+  rclcpp::Node * node, const std::vector<std::string> & pcd_paths, const std::string publisher_name)
+: logger_(node->get_logger())
+{
+  rclcpp::QoS durable_qos{1};
+  durable_qos.transient_local();
+  pub_pointcloud_map_ =
+    node->create_publisher<sensor_msgs::msg::PointCloud2>(publisher_name, durable_qos);
+
+  sensor_msgs::msg::PointCloud2 pcd = loadPCDFiles(pcd_paths);
+
+  if (pcd.width == 0) {
+    RCLCPP_ERROR(logger_, "No PCD was loaded: pcd_paths.size() = %zu", pcd_paths.size());
+    return;
+  }
+
+  pcd.header.frame_id = "map";
+  pub_pointcloud_map_->publish(pcd);
+}
+
+sensor_msgs::msg::PointCloud2 PointcloudMapLoaderModule::loadPCDFiles(
+  const std::vector<std::string> & pcd_paths) const
+{
+  sensor_msgs::msg::PointCloud2 whole_pcd;
+  sensor_msgs::msg::PointCloud2 partial_pcd;
+
+  for (int i = 0; i < static_cast<int>(pcd_paths.size()); ++i) {
+    auto & path = pcd_paths[i];
+    if (i % 50 == 0) {
+      RCLCPP_INFO_STREAM(
+        logger_,
+        fmt::format("Load {} ({} out of {})", path, i + 1, static_cast<int>(pcd_paths.size())));
+    }
+
+    if (pcl::io::loadPCDFile(path, partial_pcd) == -1) {
+      RCLCPP_ERROR_STREAM(logger_, "PCD load failed: " << path);
+    }
+
+    if (whole_pcd.width == 0) {
+      whole_pcd = partial_pcd;
+    } else {
+      whole_pcd.width += partial_pcd.width;
+      whole_pcd.row_step += partial_pcd.row_step;
+      whole_pcd.data.insert(whole_pcd.data.end(), partial_pcd.data.begin(), partial_pcd.data.end());
+    }
+  }
+
+  whole_pcd.header.frame_id = "map";
+
+  return whole_pcd;
+}

map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.hpp

@@ -0,0 +1,41 @@
+// Copyright 2022 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.
+
+#ifndef POINTCLOUD_MAP_LOADER__POINTCLOUD_MAP_LOADER_MODULE_HPP_
+#define POINTCLOUD_MAP_LOADER__POINTCLOUD_MAP_LOADER_MODULE_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <sensor_msgs/msg/point_cloud2.hpp>
+
+#include <boost/optional.hpp>
+
+#include <string>
+#include <vector>
+
+class PointcloudMapLoaderModule
+{
+public:
+  explicit PointcloudMapLoaderModule(
+    rclcpp::Node * node, const std::vector<std::string> & pcd_paths,
+    const std::string publisher_name);
+
+private:
+  rclcpp::Logger logger_;
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_pointcloud_map_;
+
+  sensor_msgs::msg::PointCloud2 loadPCDFiles(const std::vector<std::string> & pcd_paths) const;
+};
+
+#endif  // POINTCLOUD_MAP_LOADER__POINTCLOUD_MAP_LOADER_MODULE_HPP_

map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp

@@ -1,4 +1,4 @@
-// Copyright 2020 Tier IV, Inc.
+// Copyright 2022 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.
@@ -12,29 +12,14 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-/*
- * Copyright 2015-2019 Autoware Foundation. All rights reserved.
- *
- * 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 "map_loader/pointcloud_map_loader_node.hpp"
+#include "pointcloud_map_loader_node.hpp"
 
 #include <glob.h>
 #include <pcl/io/pcd_io.h>
 #include <pcl_conversions/pcl_conversions.h>
 
 #include <filesystem>
+#include <memory>
 #include <string>
 #include <vector>
 
@@ -61,16 +46,17 @@ bool isPcdFile(const std::string & p)
 PointCloudMapLoaderNode::PointCloudMapLoaderNode(const rclcpp::NodeOptions & options)
 : Node("pointcloud_map_loader", options)
 {
-  rclcpp::QoS durable_qos{1};
-  durable_qos.transient_local();
-  pub_pointcloud_map_ =
-    this->create_publisher<sensor_msgs::msg::PointCloud2>("output/pointcloud_map", durable_qos);
-
-  const auto pcd_paths_or_directory =
-    declare_parameter("pcd_paths_or_directory", std::vector<std::string>({}));
+  const auto pcd_paths =
+    getPcdPaths(declare_parameter<std::vector<std::string>>("pcd_paths_or_directory"));
 
-  std::vector<std::string> pcd_paths{};
+  std::string publisher_name = "output/pointcloud_map";
+  pcd_map_loader_ = std::make_unique<PointcloudMapLoaderModule>(this, pcd_paths, publisher_name);
+}
 
+std::vector<std::string> PointCloudMapLoaderNode::getPcdPaths(
+  const std::vector<std::string> & pcd_paths_or_directory) const
+{
+  std::vector<std::string> pcd_paths;
   for (const auto & p : pcd_paths_or_directory) {
     if (!fs::exists(p)) {
       RCLCPP_ERROR_STREAM(get_logger(), "invalid path: " << p);
@@ -89,41 +75,7 @@ PointCloudMapLoaderNode::PointCloudMapLoaderNode(const rclcpp::NodeOptions & opt
       }
     }
   }
-
-  const auto pcd = loadPCDFiles(pcd_paths);
-
-  if (pcd.width == 0) {
-    RCLCPP_ERROR(get_logger(), "No PCD was loaded: pcd_paths.size() = %zu", pcd_paths.size());
-    return;
-  }
-
-  pub_pointcloud_map_->publish(pcd);
-}
-
-sensor_msgs::msg::PointCloud2 PointCloudMapLoaderNode::loadPCDFiles(
-  const std::vector<std::string> & pcd_paths)
-{
-  sensor_msgs::msg::PointCloud2 whole_pcd{};
-
-  sensor_msgs::msg::PointCloud2 partial_pcd;
-  for (const auto & path : pcd_paths) {
-    if (pcl::io::loadPCDFile(path, partial_pcd) == -1) {
-      RCLCPP_ERROR_STREAM(get_logger(), "PCD load failed: " << path);
-    }
-
-    if (whole_pcd.width == 0) {
-      whole_pcd = partial_pcd;
-    } else {
-      whole_pcd.width += partial_pcd.width;
-      whole_pcd.row_step += partial_pcd.row_step;
-      whole_pcd.data.reserve(whole_pcd.data.size() + partial_pcd.data.size());
-      whole_pcd.data.insert(whole_pcd.data.end(), partial_pcd.data.begin(), partial_pcd.data.end());
-    }
-  }
-
-  whole_pcd.header.frame_id = "map";
-
-  return whole_pcd;
+  return pcd_paths;
 }
 
 #include <rclcpp_components/register_node_macro.hpp>

map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp

@@ -0,0 +1,45 @@
+// Copyright 2022 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.
+
+#ifndef POINTCLOUD_MAP_LOADER__POINTCLOUD_MAP_LOADER_NODE_HPP_
+#define POINTCLOUD_MAP_LOADER__POINTCLOUD_MAP_LOADER_NODE_HPP_
+
+#include "pointcloud_map_loader_module.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <pcl/common/common.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+#include <map>
+#include <memory>
+#include <random>
+#include <set>
+#include <string>
+#include <vector>
+
+class PointCloudMapLoaderNode : public rclcpp::Node
+{
+public:
+  explicit PointCloudMapLoaderNode(const rclcpp::NodeOptions & options);
+
+private:
+  std::unique_ptr<PointcloudMapLoaderModule> pcd_map_loader_;
+
+  std::vector<std::string> getPcdPaths(
+    const std::vector<std::string> & pcd_paths_or_directory) const;
+};
+
+#endif  // POINTCLOUD_MAP_LOADER__POINTCLOUD_MAP_LOADER_NODE_HPP_