feat(map_loader): add grid coordinates for partial/differential map l…

…oad (autowarefoundation#3205) * feat(map_loader): add grid coordinates for partial/differential map load Signed-off-by: kminoda <koji.minoda@tier4.jp> * style(pre-commit): autofix * update readme Signed-off-by: kminoda <koji.minoda@tier4.jp> * remove unnecessary line * update arguments in readme Signed-off-by: kminoda <koji.minoda@tier4.jp> * slightly updated directory structure in readme Signed-off-by: kminoda <koji.minoda@tier4.jp> * update readme 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>
badai-nguyen · Apr 3, 2023 · 92dd94f · 92dd94f
1 parent 2ce23f2
commit 92dd94f
Show file tree

Hide file tree

Showing 14 changed files with 304 additions and 42 deletions.
diff --git a/launch/tier4_map_launch/launch/map.launch.py b/launch/tier4_map_launch/launch/map.launch.py
@@ -89,6 +89,7 @@ def launch_setup(context, *args, **kwargs):
         ],
         parameters=[
             {"pcd_paths_or_directory": ["[", LaunchConfiguration("pointcloud_map_path"), "]"]},
+            {"pcd_metadata_path": [LaunchConfiguration("pointcloud_map_metadata_path")]},
             pointcloud_map_loader_param,
         ],
         extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
@@ -151,6 +152,11 @@ def add_launch_arg(name: str, default_value=None, description=None):
         [LaunchConfiguration("map_path"), "/pointcloud_map.pcd"],
         "path to pointcloud map file",
     ),
+    add_launch_arg(
+        "pointcloud_map_metadata_path",
+        [LaunchConfiguration("map_path"), "/pointcloud_map_metadata.yaml"],
+        "path to pointcloud map metadata file",
+    ),
     add_launch_arg(
         "lanelet2_map_loader_param_path",
         [

diff --git a/map/map_loader/CMakeLists.txt b/map/map_loader/CMakeLists.txt
@@ -14,6 +14,7 @@ ament_auto_add_library(pointcloud_map_loader_node SHARED
   src/pointcloud_map_loader/utils.cpp
 )
 target_link_libraries(pointcloud_map_loader_node ${PCL_LIBRARIES})
+target_link_libraries(pointcloud_map_loader_node yaml-cpp)
 
 target_include_directories(pointcloud_map_loader_node
   SYSTEM PUBLIC
@@ -59,12 +60,18 @@ if(BUILD_TESTING)
     ament_add_gmock(${test_name} ${filepath})
     target_include_directories(${test_name} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include)
     target_link_libraries(${test_name} ${${PROJECT_NAME}_LIBRARIES})
+    target_link_libraries(${test_name} yaml-cpp)
     ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS})
   endfunction()
 
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()
   add_testcase(test/test_sphere_box_overlap.cpp)
+  add_testcase(test/test_replace_with_absolute_path.cpp)
+  add_testcase(test/test_load_pcd_metadata.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/README.md b/map/map_loader/README.md
@@ -38,20 +38,64 @@ Please see [the description of `GetDifferentialPointCloudMap.srv`](https://githu
 
 ### Parameters
 
-| Name                          | Type  | Description                                                                       | Default value |
-| :---------------------------- | :---- | :-------------------------------------------------------------------------------- | :------------ |
-| enable_whole_load             | bool  | A flag to enable raw pointcloud map publishing                                    | true          |
-| enable_downsampled_whole_load | bool  | A flag to enable downsampled pointcloud map publishing                            | false         |
-| enable_partial_load           | bool  | A flag to enable partial pointcloud map server                                    | false         |
-| enable_differential_load      | bool  | A flag to enable differential pointcloud map server                               | false         |
-| leaf_size                     | float | Downsampling leaf size (only used when enable_downsampled_whole_load is set true) | 3.0           |
+| Name                          | Type        | Description                                                                       | Default value |
+| :---------------------------- | :---------- | :-------------------------------------------------------------------------------- | :------------ |
+| enable_whole_load             | bool        | A flag to enable raw pointcloud map publishing                                    | true          |
+| enable_downsampled_whole_load | bool        | A flag to enable downsampled pointcloud map publishing                            | false         |
+| enable_partial_load           | bool        | A flag to enable partial pointcloud map server                                    | false         |
+| enable_differential_load      | bool        | A flag to enable differential pointcloud map server                               | false         |
+| leaf_size                     | float       | Downsampling leaf size (only used when enable_downsampled_whole_load is set true) | 3.0           |
+| pcd_paths_or_directory        | std::string | Path(s) to pointcloud map file or directory                                       |               |
+| pcd_metadata_path             | std::string | Path to pointcloud metadata file                                                  |               |
 
 ### Interfaces
 
 - `output/pointcloud_map` (sensor_msgs/msg/PointCloud2) : Raw pointcloud map
 - `output/debug/downsampled_pointcloud_map` (sensor_msgs/msg/PointCloud2) : Downsampled 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
+- pointcloud map file(s) (.pcd)
+- metadata of pointcloud map(s) (.yaml)
+
+### Metadata
+
+You must provide metadata in YAML format as well as pointcloud map files. Pointcloud map should be divided into one or more files with x-y grid.
+
+Metadata should look like this:
+
+```yaml
+x_resolution: 100.0
+y_resolution: 150.0
+A.pcd: [1200, 2500] # -> 1200 < x < 1300, 2500 < y < 2650
+B.pcd: [1300, 2500] # -> 1300 < x < 1400, 2500 < y < 2650
+C.pcd: [1200, 2650] # -> 1200 < x < 1300, 2650 < y < 2800
+D.pcd: [1400, 2650] # -> 1400 < x < 1500, 2650 < y < 2800
+```
+
+You may use [pointcloud_divider](https://github.com/MapIV/pointcloud_divider) from MAP IV for dividing pointcloud map as well as generating the compatible metadata.yaml.
+
+### How to store map-related files
+
+If you only have one pointcloud map, Autoware will assume the following directory structure by default.
+
+```bash
+sample-map-rosbag
+├── lanelet2_map.osm
+├── pointcloud_map.pcd
+```
+
+If you have multiple rosbags, an example directory structure would be as follows. Note that you need to have a metadata when you have multiple pointcloud map files.
+
+```bash
+sample-map-rosbag
+├── lanelet2_map.osm
+├── pointcloud_map
+├── pcd_00.pcd
+├── pcd_01.pcd
+├── pcd_02.pcd
+├── ...
+└── pointcloud_map_metadata.yaml
+```
 
 ---
 

diff --git a/map/map_loader/launch/pointcloud_map_loader.launch.xml b/map/map_loader/launch/pointcloud_map_loader.launch.xml
@@ -1,11 +1,13 @@
 <launch>
   <arg name="pointcloud_map_path"/>
+  <arg name="pointcloud_map_metadata_path"/>
   <arg name="pointcloud_map_loader_param_path" default="$(find-pkg-share map_loader)/config/pointcloud_map_loader.param.yaml"/>
 
   <node pkg="map_loader" exec="pointcloud_map_loader" name="pointcloud_map_loader" output="screen">
     <remap from="output/pointcloud_map" to="/map/pointcloud_map"/>
     <remap from="service/get_partial_pcd_map" to="/map/get_partial_pointcloud_map"/>
     <param name="pcd_paths_or_directory" value="[$(var pointcloud_map_path)]"/>
+    <param name="pcd_metadata_path" value="$(var pcd_metadata_path)"/>
     <param from="$(var pointcloud_map_loader_param_path)"/>
   </node>
 </launch>
diff --git a/map/map_loader/package.xml b/map/map_loader/package.xml
@@ -29,6 +29,7 @@
   <depend>tf2_ros</depend>
   <depend>tier4_autoware_utils</depend>
   <depend>visualization_msgs</depend>
+  <depend>yaml-cpp</depend>
 
   <test_depend>ament_cmake_gmock</test_depend>
   <test_depend>ament_lint_auto</test_depend>

diff --git a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp
@@ -48,6 +48,10 @@ void DifferentialMapLoaderModule::differentialAreaLoad(
     } else {
       autoware_map_msgs::msg::PointCloudMapCellWithID pointcloud_map_cell_with_id =
         loadPointCloudMapCellWithID(path, map_id);
+      pointcloud_map_cell_with_id.min_x = metadata.min.x;
+      pointcloud_map_cell_with_id.min_y = metadata.min.y;
+      pointcloud_map_cell_with_id.max_x = metadata.max.x;
+      pointcloud_map_cell_with_id.max_y = metadata.max.y;
       response->new_pointcloud_with_ids.push_back(pointcloud_map_cell_with_id);
     }
   }

diff --git a/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp
@@ -29,7 +29,6 @@ void PartialMapLoaderModule::partialAreaLoad(
   GetPartialPointCloudMap::Response::SharedPtr & response) const
 {
   // iterate over all the available pcd map grids
-
   for (const auto & ele : all_pcd_file_metadata_dict_) {
     std::string path = ele.first;
     PCDFileMetadata metadata = ele.second;
@@ -42,6 +41,11 @@ void PartialMapLoaderModule::partialAreaLoad(
 
     autoware_map_msgs::msg::PointCloudMapCellWithID pointcloud_map_cell_with_id =
       loadPointCloudMapCellWithID(path, map_id);
+    pointcloud_map_cell_with_id.min_x = metadata.min.x;
+    pointcloud_map_cell_with_id.min_y = metadata.min.y;
+    pointcloud_map_cell_with_id.max_x = metadata.max.x;
+    pointcloud_map_cell_with_id.max_y = metadata.max.y;
+
     response->new_pointcloud_with_ids.push_back(pointcloud_map_cell_with_id);
   }
 }

diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp
@@ -49,6 +49,7 @@ PointCloudMapLoaderNode::PointCloudMapLoaderNode(const rclcpp::NodeOptions & opt
 {
   const auto pcd_paths =
     getPcdPaths(declare_parameter<std::vector<std::string>>("pcd_paths_or_directory"));
+  std::string pcd_metadata_path = declare_parameter<std::string>("pcd_metadata_path");
   bool enable_whole_load = declare_parameter<bool>("enable_whole_load");
   bool enable_downsample_whole_load = declare_parameter<bool>("enable_downsampled_whole_load");
   bool enable_partial_load = declare_parameter<bool>("enable_partial_load");
@@ -66,18 +67,52 @@ PointCloudMapLoaderNode::PointCloudMapLoaderNode(const rclcpp::NodeOptions & opt
       std::make_unique<PointcloudMapLoaderModule>(this, pcd_paths, publisher_name, true);
   }
 
-  if (enable_partial_load | enable_differential_load) {
-    pcd_metadata_dict_ = generatePCDMetadata(pcd_paths);
-  }
+  if (enable_partial_load || enable_differential_load) {
+    std::map<std::string, PCDFileMetadata> pcd_metadata_dict;
+    try {
+      pcd_metadata_dict = getPCDMetadata(pcd_metadata_path, pcd_paths);
+    } catch (std::runtime_error & e) {
+      RCLCPP_ERROR_STREAM(get_logger(), e.what());
+    }
 
-  if (enable_partial_load) {
-    partial_map_loader_ = std::make_unique<PartialMapLoaderModule>(this, pcd_metadata_dict_);
+    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::map<std::string, PCDFileMetadata> PointCloudMapLoaderNode::getPCDMetadata(
+  const std::string & pcd_metadata_path, const std::vector<std::string> & pcd_paths) const
+{
+  std::map<std::string, PCDFileMetadata> pcd_metadata_dict;
+  if (pcd_paths.size() != 1) {
+    if (!fs::exists(pcd_metadata_path)) {
+      throw std::runtime_error("PCD metadata file not found: " + pcd_metadata_path);
+    }
 
-  if (enable_differential_load) {
-    differential_map_loader_ =
-      std::make_unique<DifferentialMapLoaderModule>(this, pcd_metadata_dict_);
+    pcd_metadata_dict = loadPCDMetadata(pcd_metadata_path);
+    pcd_metadata_dict = replaceWithAbsolutePath(pcd_metadata_dict_, pcd_paths);
+    RCLCPP_INFO_STREAM(get_logger(), "Loaded PCD metadata: " << pcd_metadata_path);
+  } else {
+    // An exception when using a single file PCD map so that the users do not have to provide
+    // a metadata file.
+    // Note that this should ideally be avoided and thus eventually be removed by someone, until
+    // Autoware users get used to handling the PCD file(s) with metadata.
+    RCLCPP_INFO_STREAM(get_logger(), "Create PCD metadata, as the pointcloud is a single file.");
+    pcl::PointCloud<pcl::PointXYZ> single_pcd;
+    if (pcl::io::loadPCDFile(pcd_paths[0], single_pcd) == -1) {
+      throw std::runtime_error("PCD load failed: " + pcd_paths[0]);
+    }
+    PCDFileMetadata metadata = {};
+    pcl::getMinMax3D(single_pcd, metadata.min, metadata.max);
+    pcd_metadata_dict[pcd_paths[0]] = metadata;
   }
+  return pcd_metadata_dict;
 }
 
 std::vector<std::string> PointCloudMapLoaderNode::getPcdPaths(
@@ -105,21 +140,5 @@ std::vector<std::string> PointCloudMapLoaderNode::getPcdPaths(
   return pcd_paths;
 }
 
-std::map<std::string, PCDFileMetadata> PointCloudMapLoaderNode::generatePCDMetadata(
-  const std::vector<std::string> & pcd_paths) const
-{
-  pcl::PointCloud<pcl::PointXYZ> partial_pcd;
-  std::map<std::string, PCDFileMetadata> all_pcd_file_metadata_dict;
-  for (const auto & path : pcd_paths) {
-    if (pcl::io::loadPCDFile(path, partial_pcd) == -1) {
-      RCLCPP_ERROR_STREAM(get_logger(), "PCD load failed: " << path);
-    }
-    PCDFileMetadata metadata = {};
-    pcl::getMinMax3D(partial_pcd, metadata.min, metadata.max);
-    all_pcd_file_metadata_dict[path] = metadata;
-  }
-  return all_pcd_file_metadata_dict;
-}
-
 #include <rclcpp_components/register_node_macro.hpp>
 RCLCPP_COMPONENTS_REGISTER_NODE(PointCloudMapLoaderNode)
diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp
@@ -48,8 +48,8 @@ class PointCloudMapLoaderNode : public rclcpp::Node
 
   std::vector<std::string> getPcdPaths(
     const std::vector<std::string> & pcd_paths_or_directory) const;
-  std::map<std::string, PCDFileMetadata> generatePCDMetadata(
-    const std::vector<std::string> & pcd_paths) const;
+  std::map<std::string, PCDFileMetadata> getPCDMetadata(
+    const std::string & pcd_metadata_path, const std::vector<std::string> & pcd_paths) const;
 };
 
 #endif  // POINTCLOUD_MAP_LOADER__POINTCLOUD_MAP_LOADER_NODE_HPP_
diff --git a/map/map_loader/src/pointcloud_map_loader/utils.cpp b/map/map_loader/src/pointcloud_map_loader/utils.cpp
@@ -16,6 +16,53 @@
 
 #include <fmt/format.h>
 
+#include <map>
+#include <string>
+#include <vector>
+
+std::map<std::string, PCDFileMetadata> loadPCDMetadata(const std::string & pcd_metadata_path)
+{
+  YAML::Node config = YAML::LoadFile(pcd_metadata_path);
+
+  std::map<std::string, PCDFileMetadata> metadata;
+
+  for (const auto & node : config) {
+    if (
+      node.first.as<std::string>() == "x_resolution" ||
+      node.first.as<std::string>() == "y_resolution") {
+      continue;
+    }
+
+    std::string key = node.first.as<std::string>();
+    std::vector<int> values = node.second.as<std::vector<int>>();
+
+    PCDFileMetadata fileMetadata;
+    fileMetadata.min.x = values[0];
+    fileMetadata.min.y = values[1];
+    fileMetadata.max.x = values[0] + config["x_resolution"].as<float>();
+    fileMetadata.max.y = values[1] + config["y_resolution"].as<float>();
+
+    metadata[key] = fileMetadata;
+  }
+
+  return metadata;
+}
+
+std::map<std::string, PCDFileMetadata> replaceWithAbsolutePath(
+  const std::map<std::string, PCDFileMetadata> & pcd_metadata_path,
+  const std::vector<std::string> & pcd_paths)
+{
+  std::map<std::string, PCDFileMetadata> absolute_path_map;
+  for (const auto & path : pcd_paths) {
+    std::string filename = path.substr(path.find_last_of("/\\") + 1);
+    auto it = pcd_metadata_path.find(filename);
+    if (it != pcd_metadata_path.end()) {
+      absolute_path_map[path] = it->second;
+    }
+  }
+  return absolute_path_map;
+}
+
 bool sphereAndBoxOverlapExists(
   const geometry_msgs::msg::Point center, const double radius, const pcl::PointXYZ box_min_point,
   const pcl::PointXYZ box_max_point)

diff --git a/map/map_loader/src/pointcloud_map_loader/utils.hpp b/map/map_loader/src/pointcloud_map_loader/utils.hpp
@@ -19,13 +19,28 @@
 #include <geometry_msgs/msg/point.hpp>
 
 #include <pcl/common/common.h>
+#include <yaml-cpp/yaml.h>
+
+#include <map>
+#include <string>
+#include <vector>
 
 struct PCDFileMetadata
 {
   pcl::PointXYZ min;
   pcl::PointXYZ max;
+  bool operator==(const PCDFileMetadata & other) const
+  {
+    return min.x == other.min.x && min.y == other.min.y && min.z == other.min.z &&
+           max.x == other.max.x && max.y == other.max.y && max.z == other.max.z;
+  }
 };
 
+std::map<std::string, PCDFileMetadata> loadPCDMetadata(const std::string & pcd_metadata_path);
+std::map<std::string, PCDFileMetadata> replaceWithAbsolutePath(
+  const std::map<std::string, PCDFileMetadata> & pcd_metadata_path,
+  const std::vector<std::string> & pcd_paths);
+
 bool sphereAndBoxOverlapExists(
   const geometry_msgs::msg::Point position, const double radius, const pcl::PointXYZ position_min,
   const pcl::PointXYZ position_max);