Skip to content

Commit ea41a5c

Browse files
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>
1 parent 00e12c4 commit ea41a5c

File tree

4 files changed

+39
-11
lines changed

4 files changed

+39
-11
lines changed

map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp

+20-7
Original file line numberDiff line numberDiff line change
@@ -67,12 +67,8 @@ PointCloudMapLoaderNode::PointCloudMapLoaderNode(const rclcpp::NodeOptions & opt
6767
std::make_unique<PointcloudMapLoaderModule>(this, pcd_paths, publisher_name, true);
6868
}
6969

70-
std::map<std::string, PCDFileMetadata> pcd_metadata_dict;
71-
try {
72-
pcd_metadata_dict = getPCDMetadata(pcd_metadata_path, pcd_paths);
73-
} catch (std::runtime_error & e) {
74-
RCLCPP_ERROR_STREAM(get_logger(), e.what());
75-
}
70+
// Parse the metadata file and get the map of (absolute pcd path, pcd file metadata)
71+
auto pcd_metadata_dict = getPCDMetadata(pcd_metadata_path, pcd_paths);
7672

7773
if (enable_partial_load) {
7874
partial_map_loader_ = std::make_unique<PartialMapLoaderModule>(this, pcd_metadata_dict);
@@ -89,8 +85,25 @@ std::map<std::string, PCDFileMetadata> PointCloudMapLoaderNode::getPCDMetadata(
8985
const std::string & pcd_metadata_path, const std::vector<std::string> & pcd_paths) const
9086
{
9187
if (fs::exists(pcd_metadata_path)) {
88+
std::set<std::string> missing_pcd_names;
9289
auto pcd_metadata_dict = loadPCDMetadata(pcd_metadata_path);
93-
pcd_metadata_dict = replaceWithAbsolutePath(pcd_metadata_dict, pcd_paths);
90+
91+
pcd_metadata_dict = replaceWithAbsolutePath(pcd_metadata_dict, pcd_paths, missing_pcd_names);
92+
93+
// Warning if some segments are missing
94+
if (!missing_pcd_names.empty()) {
95+
std::ostringstream oss;
96+
97+
oss << "The following segment(s) are missing from the input PCDs: ";
98+
99+
for (auto & fname : missing_pcd_names) {
100+
oss << std::endl << fname;
101+
}
102+
103+
RCLCPP_ERROR_STREAM(get_logger(), oss.str());
104+
throw std::runtime_error("Missing PCD segments. Exiting map loader...");
105+
}
106+
94107
return pcd_metadata_dict;
95108
} else if (pcd_paths.size() == 1) {
96109
// 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

+13-1
Original file line numberDiff line numberDiff line change
@@ -50,16 +50,28 @@ std::map<std::string, PCDFileMetadata> loadPCDMetadata(const std::string & pcd_m
5050

5151
std::map<std::string, PCDFileMetadata> replaceWithAbsolutePath(
5252
const std::map<std::string, PCDFileMetadata> & pcd_metadata_path,
53-
const std::vector<std::string> & pcd_paths)
53+
const std::vector<std::string> & pcd_paths, std::set<std::string> & missing_pcd_names)
5454
{
55+
// Initially, assume all segments are missing
56+
for (auto & it : pcd_metadata_path) {
57+
missing_pcd_names.insert(it.first);
58+
}
59+
5560
std::map<std::string, PCDFileMetadata> absolute_path_map;
5661
for (const auto & path : pcd_paths) {
5762
std::string filename = path.substr(path.find_last_of("/\\") + 1);
5863
auto it = pcd_metadata_path.find(filename);
5964
if (it != pcd_metadata_path.end()) {
6065
absolute_path_map[path] = it->second;
66+
67+
// If a segment was found from the pcd paths, remove
68+
// it from the missing segments
69+
missing_pcd_names.erase(filename);
6170
}
6271
}
72+
73+
// The remaining segments in the @missing_pcd are were not
74+
// found from the pcd paths, which means they are missing
6375
return absolute_path_map;
6476
}
6577

map/map_loader/src/pointcloud_map_loader/utils.hpp

+2-1
Original file line numberDiff line numberDiff line change
@@ -22,6 +22,7 @@
2222
#include <yaml-cpp/yaml.h>
2323

2424
#include <map>
25+
#include <set>
2526
#include <string>
2627
#include <vector>
2728

@@ -39,7 +40,7 @@ struct PCDFileMetadata
3940
std::map<std::string, PCDFileMetadata> loadPCDMetadata(const std::string & pcd_metadata_path);
4041
std::map<std::string, PCDFileMetadata> replaceWithAbsolutePath(
4142
const std::map<std::string, PCDFileMetadata> & pcd_metadata_path,
42-
const std::vector<std::string> & pcd_paths);
43+
const std::vector<std::string> & pcd_paths, std::set<std::string> & missing_pcd_names);
4344

4445
bool cylinderAndBoxOverlapExists(
4546
const double center_x, const double center_y, const double radius,

map/map_loader/test/test_replace_with_absolute_path.cpp

+4-2
Original file line numberDiff line numberDiff line change
@@ -36,7 +36,8 @@ TEST(ReplaceWithAbsolutePathTest, BasicFunctionality)
3636
{"/home/user/pcd/file2.pcd", {{-1, -2, -3}, {-4, -5, -6}}},
3737
};
3838

39-
auto result = replaceWithAbsolutePath(pcd_metadata_path, pcd_paths);
39+
std::set<std::string> missing_pcd_names;
40+
auto result = replaceWithAbsolutePath(pcd_metadata_path, pcd_paths, missing_pcd_names);
4041
ASSERT_THAT(result, ContainerEq(expected));
4142
}
4243

@@ -53,8 +54,9 @@ TEST(ReplaceWithAbsolutePathTest, NoMatchingFiles)
5354
};
5455

5556
std::map<std::string, PCDFileMetadata> expected = {};
57+
std::set<std::string> missing_pcd_names;
5658

57-
auto result = replaceWithAbsolutePath(pcd_metadata_path, pcd_paths);
59+
auto result = replaceWithAbsolutePath(pcd_metadata_path, pcd_paths, missing_pcd_names);
5860
ASSERT_THAT(result, ContainerEq(expected));
5961
}
6062

0 commit comments

Comments
 (0)