feat(map_loader): warn if some pcds from the metadata file are missing (autowarefoundation#7406)

* Examine if there are PCD segments found in the metadata file but are missing from the input pcd paths

Signed-off-by: Anh Nguyen <anh.nguyen.2@tier4.jp>

* style(pre-commit): autofix

* Fixing CI

Signed-off-by: Anh Nguyen <anh.nguyen.2@tier4.jp>

* Fixing CI

Signed-off-by: Anh Nguyen <anh.nguyen.2@tier4.jp>

* Fixing CI

Signed-off-by: Anh Nguyen <anh.nguyen.2@tier4.jp>

* Fix CI related to map_loader

Signed-off-by: Anh Nguyen <anh.nguyen.2@tier4.jp>

* Removed try{} block from getPCDMetadata and redundant std::endl at the end of error messages

Signed-off-by: Anh Nguyen <anh.nguyen.2@tier4.jp>

---------

Signed-off-by: Anh Nguyen <anh.nguyen.2@tier4.jp>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

Author: anhnv3991

map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp

@@ -67,12 +67,8 @@ PointCloudMapLoaderNode::PointCloudMapLoaderNode(const rclcpp::NodeOptions & opt
       std::make_unique<PointcloudMapLoaderModule>(this, pcd_paths, publisher_name, true);
   }
 
-  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());
-  }
+  // Parse the metadata file and get the map of (absolute pcd path, pcd file metadata)
+  auto pcd_metadata_dict = getPCDMetadata(pcd_metadata_path, pcd_paths);
 
   if (enable_partial_load) {
     partial_map_loader_ = std::make_unique<PartialMapLoaderModule>(this, pcd_metadata_dict);
@@ -89,8 +85,25 @@ std::map<std::string, PCDFileMetadata> PointCloudMapLoaderNode::getPCDMetadata(
   const std::string & pcd_metadata_path, const std::vector<std::string> & pcd_paths) const
 {
   if (fs::exists(pcd_metadata_path)) {
+    std::set<std::string> missing_pcd_names;
     auto pcd_metadata_dict = loadPCDMetadata(pcd_metadata_path);
-    pcd_metadata_dict = replaceWithAbsolutePath(pcd_metadata_dict, pcd_paths);
+
+    pcd_metadata_dict = replaceWithAbsolutePath(pcd_metadata_dict, pcd_paths, missing_pcd_names);
+
+    // Warning if some segments are missing
+    if (!missing_pcd_names.empty()) {
+      std::ostringstream oss;
+
+      oss << "The following segment(s) are missing from the input PCDs: ";
+
+      for (auto & fname : missing_pcd_names) {
+        oss << std::endl << fname;
+      }
+
+      RCLCPP_ERROR_STREAM(get_logger(), oss.str());
+      throw std::runtime_error("Missing PCD segments. Exiting map loader...");
+    }
+
     return pcd_metadata_dict;
   } else if (pcd_paths.size() == 1) {
     // An exception when using a single file PCD map so that the users do not have to provide

map/map_loader/src/pointcloud_map_loader/utils.cpp

@@ -50,16 +50,28 @@ std::map<std::string, PCDFileMetadata> loadPCDMetadata(const std::string & pcd_m
 
 std::map<std::string, PCDFileMetadata> replaceWithAbsolutePath(
   const std::map<std::string, PCDFileMetadata> & pcd_metadata_path,
-  const std::vector<std::string> & pcd_paths)
+  const std::vector<std::string> & pcd_paths, std::set<std::string> & missing_pcd_names)
 {
+  // Initially, assume all segments are missing
+  for (auto & it : pcd_metadata_path) {
+    missing_pcd_names.insert(it.first);
+  }
+
   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;
+
+      // If a segment was found from the pcd paths, remove
+      // it from the missing segments
+      missing_pcd_names.erase(filename);
     }
   }
+
+  // The remaining segments in the @missing_pcd are were not
+  // found from the pcd paths, which means they are missing
   return absolute_path_map;
 }
 

map/map_loader/src/pointcloud_map_loader/utils.hpp

@@ -22,6 +22,7 @@
 #include <yaml-cpp/yaml.h>
 
 #include <map>
+#include <set>
 #include <string>
 #include <vector>
 
@@ -39,7 +40,7 @@ struct PCDFileMetadata
 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);
+  const std::vector<std::string> & pcd_paths, std::set<std::string> & missing_pcd_names);
 
 bool cylinderAndBoxOverlapExists(
   const double center_x, const double center_y, const double radius,

map/map_loader/test/test_replace_with_absolute_path.cpp

@@ -36,7 +36,8 @@ TEST(ReplaceWithAbsolutePathTest, BasicFunctionality)
     {"/home/user/pcd/file2.pcd", {{-1, -2, -3}, {-4, -5, -6}}},
   };
 
-  auto result = replaceWithAbsolutePath(pcd_metadata_path, pcd_paths);
+  std::set<std::string> missing_pcd_names;
+  auto result = replaceWithAbsolutePath(pcd_metadata_path, pcd_paths, missing_pcd_names);
   ASSERT_THAT(result, ContainerEq(expected));
 }
 
@@ -53,8 +54,9 @@ TEST(ReplaceWithAbsolutePathTest, NoMatchingFiles)
   };
 
   std::map<std::string, PCDFileMetadata> expected = {};
+  std::set<std::string> missing_pcd_names;
 
-  auto result = replaceWithAbsolutePath(pcd_metadata_path, pcd_paths);
+  auto result = replaceWithAbsolutePath(pcd_metadata_path, pcd_paths, missing_pcd_names);
   ASSERT_THAT(result, ContainerEq(expected));
 }