feat(map_loader): add differential map loading interface

launch/tier4_map_launch/config/pointcloud_map_loader.param.yaml

@@ -1,4 +1,5 @@
 /**:
   ros__parameters:
     enable_whole_load: true
-    enable_partial_load: true
+    enable_partial_load: false
+    enable_differential_load: false

map/map_loader/CMakeLists.txt

@@ -10,6 +10,7 @@ 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
   src/pointcloud_map_loader/partial_map_loader_module.cpp
+  src/pointcloud_map_loader/differential_map_loader_module.cpp
   src/pointcloud_map_loader/utils.cpp
 )
 target_link_libraries(pointcloud_map_loader_node ${PCL_LIBRARIES})

map/map_loader/README.md

@@ -11,6 +11,7 @@ Currently, it supports the following two types:
 
 - Publish raw pointcloud map
 - Send partial pointcloud map loading via ROS 2 service
+- Send differential pointcloud map loading via ROS 2 service
 
 #### Publish raw pointcloud map (ROS 2 topic)
 
@@ -23,17 +24,26 @@ Here, we assume that the pointcloud maps are divided into grids.
 Given a query from a client node, the node sends a set of pointcloud maps that overlaps with the queried area.
 Please see [the description of `GetPartialPointCloudMap.srv`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_map_msgs#getpartialpointcloudmapsrv) for details.
 
+#### Send differential pointcloud map (ROS 2 service)
+
+Here, we assume that the pointcloud maps are divided into grids.
+
+Given a query and set of map IDs, the node sends a set of pointcloud maps that overlap with the queried area and are not included in the set of map IDs.
+Please see [the description of `GetDifferentialPointCloudMap.srv`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_map_msgs#getdifferentialpointcloudmapsrv) for details.
+
 ### Parameters
 
-| Name                | Type | Description                                    | Default value |
-| :------------------ | :--- | :--------------------------------------------- | :------------ |
-| enable_whole_load   | bool | A flag to enable raw pointcloud map publishing | true          |
-| enable_partial_load | bool | A flag to enable partial pointcloud map server | true          |
+| Name                     | Type | Description                                         | Default value |
+| :----------------------- | :--- | :-------------------------------------------------- | :------------ |
+| enable_whole_load        | bool | A flag to enable raw pointcloud map publishing      | true          |
+| enable_partial_load      | bool | A flag to enable partial pointcloud map server      | true          |
+| enable_differential_load | bool | A flag to enable differential pointcloud map server | true          |
 
 ### Interfaces
 
 - `output/pointcloud_map` (sensor_msgs/msg/PointCloud2) : Raw pointcloud map
 - `service/get_partial_pcd_map` (autoware_map_msgs/srv/GetPartialPointCloudMap) : Partial pointcloud map
+- `service/get_differential_pcd_map` (autoware_map_msgs/srv/GetDifferentialPointCloudMap) : Differential pointcloud map
 
 ---
 

map/map_loader/config/pointcloud_map_loader.param.yaml

@@ -1,4 +1,5 @@
 /**:
   ros__parameters:
     enable_whole_load: true
-    enable_partial_load: true
+    enable_partial_load: false
+    enable_differential_load: false

map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp

@@ -0,0 +1,85 @@
+// 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 "differential_map_loader_module.hpp"
+
+DifferentialMapLoaderModule::DifferentialMapLoaderModule(
+  rclcpp::Node * node, const std::map<std::string, PCDFileMetadata> & pcd_file_metadata_dict)
+: logger_(node->get_logger()), all_pcd_file_metadata_dict_(pcd_file_metadata_dict)
+{
+  get_differential_pcd_maps_service_ = node->create_service<GetDifferentialPointCloudMap>(
+    "service/get_differential_pcd_map",
+    std::bind(
+      &DifferentialMapLoaderModule::onServiceGetDifferentialPointCloudMap, this,
+      std::placeholders::_1, std::placeholders::_2));
+}
+
+void DifferentialMapLoaderModule::differentialAreaLoad(
+  const autoware_map_msgs::msg::AreaInfo area, const std::vector<std::string> & cached_ids,
+  GetDifferentialPointCloudMap::Response::SharedPtr & response) const
+{
+  // iterate over all the available pcd map grids
+  std::vector<bool> should_remove(static_cast<int>(cached_ids.size()), true);
+  for (const auto & ele : all_pcd_file_metadata_dict_) {
+    std::string path = ele.first;
+    PCDFileMetadata metadata = ele.second;
+
+    // assume that the map ID = map path (for now)
+    std::string map_id = path;
+
+    // skip if the pcd file is not within the queried area
+    if (!isGridWithinQueriedArea(area, metadata)) continue;
+
+    auto id_in_cached_list = std::find(cached_ids.begin(), cached_ids.end(), map_id);
+    if (id_in_cached_list != cached_ids.end()) {
+      int index = id_in_cached_list - cached_ids.begin();
+      should_remove[index] = false;
+    } else {
+      autoware_map_msgs::msg::PointCloudMapCellWithID pointcloud_map_cell_with_id =
+        loadPointCloudMapCellWithID(path, map_id);
+      response->new_pointcloud_with_ids.push_back(pointcloud_map_cell_with_id);
+    }
+  }
+
+  for (int i = 0; i < static_cast<int>(cached_ids.size()); ++i) {
+    if (should_remove[i]) {
+      response->ids_to_remove.push_back(cached_ids[i]);
+    }
+  }
+}
+
+bool DifferentialMapLoaderModule::onServiceGetDifferentialPointCloudMap(
+  GetDifferentialPointCloudMap::Request::SharedPtr req,
+  GetDifferentialPointCloudMap::Response::SharedPtr res)
+{
+  auto area = req->area;
+  std::vector<std::string> cached_ids = req->cached_ids;
+  differentialAreaLoad(area, cached_ids, res);
+  res->header.frame_id = "map";
+  return true;
+}
+
+autoware_map_msgs::msg::PointCloudMapCellWithID
+DifferentialMapLoaderModule::loadPointCloudMapCellWithID(
+  const std::string path, const std::string map_id) const
+{
+  sensor_msgs::msg::PointCloud2 pcd;
+  if (pcl::io::loadPCDFile(path, pcd) == -1) {
+    RCLCPP_ERROR_STREAM(logger_, "PCD load failed: " << path);
+  }
+  autoware_map_msgs::msg::PointCloudMapCellWithID pointcloud_map_cell_with_id;
+  pointcloud_map_cell_with_id.pointcloud = pcd;
+  pointcloud_map_cell_with_id.cell_id = map_id;
+  return pointcloud_map_cell_with_id;
+}

map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp

@@ -0,0 +1,59 @@
+// 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__DIFFERENTIAL_MAP_LOADER_MODULE_HPP_
+#define POINTCLOUD_MAP_LOADER__DIFFERENTIAL_MAP_LOADER_MODULE_HPP_
+
+#include "utils.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+#include "autoware_map_msgs/srv/get_differential_point_cloud_map.hpp"
+
+#include <pcl/common/common.h>
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl_conversions/pcl_conversions.h>
+
+#include <map>
+#include <string>
+#include <vector>
+
+class DifferentialMapLoaderModule
+{
+  using GetDifferentialPointCloudMap = autoware_map_msgs::srv::GetDifferentialPointCloudMap;
+
+public:
+  explicit DifferentialMapLoaderModule(
+    rclcpp::Node * node, const std::map<std::string, PCDFileMetadata> & pcd_file_metadata_dict);
+
+private:
+  rclcpp::Logger logger_;
+
+  std::map<std::string, PCDFileMetadata> all_pcd_file_metadata_dict_;
+  rclcpp::Service<GetDifferentialPointCloudMap>::SharedPtr get_differential_pcd_maps_service_;
+
+  bool onServiceGetDifferentialPointCloudMap(
+    GetDifferentialPointCloudMap::Request::SharedPtr req,
+    GetDifferentialPointCloudMap::Response::SharedPtr res);
+  void differentialAreaLoad(
+    const autoware_map_msgs::msg::AreaInfo area_info, const std::vector<std::string> & cached_ids,
+    GetDifferentialPointCloudMap::Response::SharedPtr & response) const;
+  autoware_map_msgs::msg::PointCloudMapCellWithID loadPointCloudMapCellWithID(
+    const std::string path, const std::string map_id) const;
+};
+
+#endif  // POINTCLOUD_MAP_LOADER__DIFFERENTIAL_MAP_LOADER_MODULE_HPP_

map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp

@@ -51,16 +51,25 @@ PointCloudMapLoaderNode::PointCloudMapLoaderNode(const rclcpp::NodeOptions & opt
     getPcdPaths(declare_parameter<std::vector<std::string>>("pcd_paths_or_directory"));
   bool enable_whole_load = declare_parameter<bool>("enable_whole_load");
   bool enable_partial_load = declare_parameter<bool>("enable_partial_load");
+  bool enable_differential_load = declare_parameter<bool>("enable_differential_load");
 
   if (enable_whole_load) {
     std::string publisher_name = "output/pointcloud_map";
     pcd_map_loader_ = std::make_unique<PointcloudMapLoaderModule>(this, pcd_paths, publisher_name);
   }
 
-  if (enable_partial_load) {
+  if (enable_partial_load | enable_differential_load) {
     pcd_metadata_dict_ = generatePCDMetadata(pcd_paths);
+  }
+
+  if (enable_partial_load) {
     partial_map_loader_ = std::make_unique<PartialMapLoaderModule>(this, pcd_metadata_dict_);
   }
+
+  if (enable_differential_load) {
+    differential_map_loader_ =
+      std::make_unique<DifferentialMapLoaderModule>(this, pcd_metadata_dict_);
+  }
 }
 
 std::vector<std::string> PointCloudMapLoaderNode::getPcdPaths(

map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp

@@ -15,6 +15,7 @@
 #ifndef POINTCLOUD_MAP_LOADER__POINTCLOUD_MAP_LOADER_NODE_HPP_
 #define POINTCLOUD_MAP_LOADER__POINTCLOUD_MAP_LOADER_NODE_HPP_
 
+#include "differential_map_loader_module.hpp"
 #include "partial_map_loader_module.hpp"
 #include "pointcloud_map_loader_module.hpp"
 
@@ -42,6 +43,7 @@ class PointCloudMapLoaderNode : public rclcpp::Node
 
   std::unique_ptr<PointcloudMapLoaderModule> pcd_map_loader_;
   std::unique_ptr<PartialMapLoaderModule> partial_map_loader_;
+  std::unique_ptr<DifferentialMapLoaderModule> differential_map_loader_;
 
   std::vector<std::string> getPcdPaths(
     const std::vector<std::string> & pcd_paths_or_directory) const;