From ea41a5ca1b1697154ecb43181eb92fef5aaf7079 Mon Sep 17 00:00:00 2001 From: Anh Nguyen Date: Sat, 15 Jun 2024 16:20:55 +0700 Subject: [PATCH] feat(map_loader): warn if some pcds from the metadata file are missing (#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 * style(pre-commit): autofix * Fixing CI Signed-off-by: Anh Nguyen * Fixing CI Signed-off-by: Anh Nguyen * Fixing CI Signed-off-by: Anh Nguyen * Fix CI related to map_loader Signed-off-by: Anh Nguyen * Removed try{} block from getPCDMetadata and redundant std::endl at the end of error messages Signed-off-by: Anh Nguyen --------- Signed-off-by: Anh Nguyen Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../pointcloud_map_loader_node.cpp | 27 ++++++++++++++----- .../src/pointcloud_map_loader/utils.cpp | 14 +++++++++- .../src/pointcloud_map_loader/utils.hpp | 3 ++- .../test/test_replace_with_absolute_path.cpp | 6 +++-- 4 files changed, 39 insertions(+), 11 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 index bacbabe60a719..349fc2954fe28 100644 --- 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 @@ -67,12 +67,8 @@ PointCloudMapLoaderNode::PointCloudMapLoaderNode(const rclcpp::NodeOptions & opt std::make_unique(this, pcd_paths, publisher_name, true); } - std::map 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(this, pcd_metadata_dict); @@ -89,8 +85,25 @@ std::map PointCloudMapLoaderNode::getPCDMetadata( const std::string & pcd_metadata_path, const std::vector & pcd_paths) const { if (fs::exists(pcd_metadata_path)) { + std::set 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 diff --git a/map/map_loader/src/pointcloud_map_loader/utils.cpp b/map/map_loader/src/pointcloud_map_loader/utils.cpp index 2430ce74e3e8b..96f9d114ed265 100644 --- a/map/map_loader/src/pointcloud_map_loader/utils.cpp +++ b/map/map_loader/src/pointcloud_map_loader/utils.cpp @@ -50,16 +50,28 @@ std::map loadPCDMetadata(const std::string & pcd_m std::map replaceWithAbsolutePath( const std::map & pcd_metadata_path, - const std::vector & pcd_paths) + const std::vector & pcd_paths, std::set & missing_pcd_names) { + // Initially, assume all segments are missing + for (auto & it : pcd_metadata_path) { + missing_pcd_names.insert(it.first); + } + std::map 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; } diff --git a/map/map_loader/src/pointcloud_map_loader/utils.hpp b/map/map_loader/src/pointcloud_map_loader/utils.hpp index 62d0b34e516e3..29d9a24d7b0e7 100644 --- a/map/map_loader/src/pointcloud_map_loader/utils.hpp +++ b/map/map_loader/src/pointcloud_map_loader/utils.hpp @@ -22,6 +22,7 @@ #include #include +#include #include #include @@ -39,7 +40,7 @@ struct PCDFileMetadata std::map loadPCDMetadata(const std::string & pcd_metadata_path); std::map replaceWithAbsolutePath( const std::map & pcd_metadata_path, - const std::vector & pcd_paths); + const std::vector & pcd_paths, std::set & missing_pcd_names); 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 index 10680e05103ce..f61dd188f0679 100644 --- 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 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 expected = {}; + std::set 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)); }