fix: make small fixes (#65)

* Added pose_topic Signed-off-by: Shintaro Sakoda <[email protected]> * Added load_pcd_recursive and use it in regression_test Signed-off-by: Shintaro Sakoda <[email protected]> --------- Signed-off-by: Shintaro Sakoda <[email protected]>
tier4 · Sep 11, 2024 · cc0ff83 · cc0ff83
1 parent ebc71c7
commit cc0ff83
Show file tree

Hide file tree

Showing 3 changed files with 43 additions and 3 deletions.
diff --git a/apps/regression_test.cpp b/apps/regression_test.cpp
@@ -48,7 +48,7 @@ int main(int argc, char ** argv)
 
   // load target pcd
   const std::string target_pcd = input_dir + "/pointcloud_map.pcd";
-  const pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud = load_pcd(target_pcd);
+  const pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud = load_pcd_recursive(target_pcd);
 
   // prepare sensor_pcd
   const std::string source_pcd_dir = input_dir + "/sensor_pcd/";

diff --git a/apps/util.hpp b/apps/util.hpp
@@ -21,6 +21,7 @@
 #include <pcl/point_types.h>
 
 #include <algorithm>
+#include <filesystem>
 #include <string>
 #include <vector>
 
@@ -47,6 +48,43 @@ inline pcl::PointCloud<pcl::PointXYZ>::Ptr load_pcd(const std::string & path)
   return pcd;
 }
 
+inline pcl::PointCloud<pcl::PointXYZ>::Ptr load_pcd_recursive(const std::string & file_path)
+{
+  pcl::PointCloud<pcl::PointXYZ>::Ptr pcd(new pcl::PointCloud<pcl::PointXYZ>());
+  if (!std::filesystem::exists(file_path)) {
+    std::cerr << "No such path: " << file_path << std::endl;
+    std::exit(1);
+  }
+  if (!std::filesystem::is_directory(file_path)) {
+    return load_pcd(file_path);
+  }
+
+  int pcd_count = 0;
+  for (const auto & file : std::filesystem::recursive_directory_iterator(file_path)) {
+    const std::string filename = file.path().c_str();
+    const std::string extension = file.path().extension().string();
+    if (extension != ".pcd" && extension != ".PCD") {
+      std::cerr << "ignore files: " << extension.c_str() << std::endl;
+      continue;
+    }
+    if (file.is_directory()) {
+      std::cerr << "ignore directory: " << filename.c_str() << std::endl;
+      continue;
+    }
+
+    // check load pcd
+    auto partial_pcd = load_pcd(filename);
+
+    // concatenate pcd
+    *pcd += *partial_pcd;
+
+    pcd_count++;
+    std::cout << "PCD loaded:" << filename << " (" << pcd_count << " files processed)" << std::endl;
+  }
+
+  return pcd;
+}
+
 inline std::vector<Eigen::Matrix4f> load_pose_list(const std::string & path)
 {
   /*

diff --git a/script/convert_rosbag_to_test_data.py b/script/convert_rosbag_to_test_data.py
@@ -16,13 +16,15 @@ def parse_args():
     parser = argparse.ArgumentParser()
     parser.add_argument("rosbag_path", type=pathlib.Path)
     parser.add_argument("output_dir", type=pathlib.Path)
+    parser.add_argument("--pose_topic", type=str, default="/localization/kinematic_state")
     return parser.parse_args()
 
 
 if __name__ == "__main__":
     args = parse_args()
     rosbag_path = args.rosbag_path
     output_dir = args.output_dir
+    pose_topic = args.pose_topic
 
     serialization_format = "cdr"
     storage_options = rosbag2_py.StorageOptions(
@@ -42,7 +44,7 @@ def parse_args():
     }
 
     target_topics = [
-        "/localization/kinematic_state",
+        pose_topic,
         "/localization/util/downsample/pointcloud",
     ]
     storage_filter = rosbag2_py.StorageFilter(topics=target_topics)
@@ -58,7 +60,7 @@ def parse_args():
         timestamp_header = (
             int(msg.header.stamp.sec) + int(msg.header.stamp.nanosec) * 1e-9
         )
-        if topic == "/localization/kinematic_state":
+        if topic == pose_topic:
             pose = msg.pose.pose
             twist = msg.twist.twist
             kinematic_state_list.append(