Examine if there are PCD segments found in the metadata file but are …

…missing from the input pcd paths Signed-off-by: Anh Nguyen <[email protected]>
anhnv3991 · Jun 10, 2024 · b465464 · b465464
1 parent e800d54
commit b465464
Show file tree

Hide file tree

Showing 4 changed files with 40 additions and 5 deletions.
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
@@ -89,8 +89,26 @@ 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_pcds;
     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_pcds);
+
+    // Warning if some pcds are missing
+    if (!missing_pcds.empty())
+    {
+      std::ostringstream oss;
+
+      oss << "The following segment(s) are missing from the input PCDs: " << std::endl;
+
+      for (auto & fname : missing_pcds)
+      {
+        oss << fname << std::endl;
+      }
+
+      RCLCPP_WARN_STREAM(get_logger(), oss.str());
+    }
+
     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

diff --git a/map/map_loader/src/pointcloud_map_loader/utils.cpp b/map/map_loader/src/pointcloud_map_loader/utils.cpp
@@ -50,16 +50,30 @@ 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)
 {
+  // Initially, assume all segments are missing
+  for (auto & it : pcd_metadata_path)
+  {
+    missing_pcd.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.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;
 }
 

diff --git a/map/map_loader/src/pointcloud_map_loader/utils.hpp b/map/map_loader/src/pointcloud_map_loader/utils.hpp
@@ -39,7 +39,8 @@ 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_pcds);
 
 bool cylinderAndBoxOverlapExists(
   const double center_x, const double center_y, const double radius,

diff --git a/map/map_loader/test/test_replace_with_absolute_path.cpp b/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_pcds;
+  auto result = replaceWithAbsolutePath(pcd_metadata_path, pcd_paths, missing_pcds);
   ASSERT_THAT(result, ContainerEq(expected));
 }
 
@@ -53,8 +54,9 @@ TEST(ReplaceWithAbsolutePathTest, NoMatchingFiles)
   };
 
   std::map<std::string, PCDFileMetadata> expected = {};
+  std::set<std::string> missing_pcds;
 
-  auto result = replaceWithAbsolutePath(pcd_metadata_path, pcd_paths);
+  auto result = replaceWithAbsolutePath(pcd_metadata_path, pcd_paths, missing_pcds);
   ASSERT_THAT(result, ContainerEq(expected));
 }