feat: enable TP manager to handle tiny resolution

map/autoware_pointcloud_divider/CMakeLists.txt

@@ -12,14 +12,9 @@ else ()
   set(CMAKE_CXX_STANDARD 17)
 endif ()
 
-# Find packages
-find_package(yaml-cpp REQUIRED)
-find_package(PCL REQUIRED COMPONENTS common io filters)
-
-include_directories(include)
-
 # Add divider library
 ament_auto_add_library(${PROJECT_NAME} SHARED src/pointcloud_divider_node.cpp src/voxel_grid_filter.cpp src/pcd_divider.cpp)
+target_include_directories(${PROJECT_NAME} PUBLIC include)
 target_link_libraries(${PROJECT_NAME} yaml-cpp ${PCL_LIBRARIES})
 rclcpp_components_register_node(${PROJECT_NAME}
   PLUGIN "autoware::pointcloud_divider::PointCloudDivider"

map/autoware_pointcloud_divider/include/autoware/pointcloud_divider/pcd_divider.hpp

@@ -81,9 +81,16 @@ class PCDDivider
 
 public:
   explicit PCDDivider(const rclcpp::Logger & logger) : logger_(logger) {}
+  explicit PCDDivider(const std::string & logger_name) : logger_(rclcpp::get_logger(logger_name)) {}
 
   // Functions to set input parameters
-  void setInput(const std::string & input_pcd_or_dir) { input_pcd_or_dir_ = input_pcd_or_dir; }
+  void setInput(const std::string & input_pcd_or_dir)
+  {
+    // Reset the PCD reader
+    reader_.clear();
+
+    input_pcd_or_dir_ = input_pcd_or_dir;
+  }
 
   void setOutputDir(const std::string & output_dir)
   {
@@ -121,7 +128,8 @@ class PCDDivider
   }
 
   void run();
-  void run(const std::vector<std::string> & pcd_names);
+  void run(const std::vector<std::string> & pcd_names, bool meta_gen = false);
+  void meta_generator();
 
 private:
   std::string input_pcd_or_dir_, output_dir_, file_prefix_, config_file_;
@@ -161,7 +169,7 @@ class PCDDivider
 
   PclCloudPtr loadPCD(const std::string & pcd_name);
   void savePCD(const std::string & pcd_name, const pcl::PointCloud<PointT> & cloud);
-  void dividePointCloud(const PclCloudPtr & cloud_ptr);
+  void dividePointCloud(const PclCloudPtr & cloud_ptr, bool meta_gen);
   void paramInitialize();
   void saveGridInfoToYAML(const std::string & yaml_file_path);
   void checkOutputDirectoryValidity();

map/autoware_pointcloud_divider/include/autoware/pointcloud_divider/pcd_io_reader.hpp

@@ -105,15 +105,6 @@ class CustomPCDReader
     return 0;
   }
 
-  ~CustomPCDReader() { clear(); }
-
-private:
-  void readHeader(std::ifstream & input);
-  size_t readABlock(std::ifstream & input, PclCloudType & output);
-
-  size_t readABlockBinary(std::ifstream & input, PclCloudType & output);
-  size_t readABlockASCII(std::ifstream & input, PclCloudType & output);
-
   void clear()
   {
     version_.clear();
@@ -141,8 +132,21 @@ class CustomPCDReader
 
     buffer_ = nullptr;
     block_size_ = 30000000;
+
+    pcd_path_.clear();
+    read_loc_.clear();
+    read_sizes_.clear();
   }
 
+  ~CustomPCDReader() { clear(); }
+
+private:
+  void readHeader(std::ifstream & input);
+  size_t readABlock(std::ifstream & input, PclCloudType & output);
+
+  size_t readABlockBinary(std::ifstream & input, PclCloudType & output);
+  size_t readABlockASCII(std::ifstream & input, PclCloudType & output);
+
   // Metadata
   std::string version_;
   size_t width_, height_;

map/autoware_pointcloud_divider/package.xml

@@ -13,6 +13,7 @@
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
   <depend>libpcl-all-dev</depend>
+  <depend>pcl_conversions</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
   <depend>yaml-cpp</depend>

map/autoware_pointcloud_divider/src/pcd_divider.cpp

@@ -52,6 +52,7 @@
 #include <list>
 #include <memory>
 #include <string>
+#include <unordered_set>
 #include <utility>
 #include <vector>
 
@@ -107,38 +108,43 @@ void PCDDivider<PointT>::run()
   auto pcd_list = discoverPCDs(input_pcd_or_dir_);
 
   // Process pcd files
-  run(pcd_list);
+  run(pcd_list, false);
 }
 
 template <class PointT>
-void PCDDivider<PointT>::run(const std::vector<std::string> & pcd_names)
+void PCDDivider<PointT>::run(const std::vector<std::string> & pcd_names, bool meta_gen)
 {
   checkOutputDirectoryValidity();
 
   grid_set_.clear();
 
   for (const std::string & pcd_name : pcd_names) {
     if (!rclcpp::ok()) {
+      RCLCPP_INFO(logger_, "Quit");
+
       return;
     }
 
-    if (debug_mode_) {
-      RCLCPP_INFO(logger_, "Dividing file %s", pcd_name.c_str());
-    }
+    // if (debug_mode_) {
+    RCLCPP_INFO(logger_, "Dividing file %s", pcd_name.c_str());
+    // }
 
     do {
       auto cloud_ptr = loadPCD(pcd_name);
 
-      dividePointCloud(cloud_ptr);
+      dividePointCloud(cloud_ptr, meta_gen);
+      RCLCPP_ERROR(logger_, "Checking a segment");
     } while (reader_.good() && rclcpp::ok());
   }
 
-  saveTheRest();
+  if (!meta_gen) {
+    saveTheRest();
 
-  RCLCPP_INFO(logger_, "Merge and downsampling... ");
+    RCLCPP_INFO(logger_, "Merge and downsampling... ");
 
-  // Now merge and downsample
-  mergeAndDownsample();
+    // Now merge and downsample
+    mergeAndDownsample();
+  }
 
   std::string yaml_file_path = output_dir_ + "/pointcloud_map_metadata.yaml";
   saveGridInfoToYAML(yaml_file_path);
@@ -188,10 +194,13 @@ void PCDDivider<PointT>::savePCD(const std::string & path, const pcl::PointCloud
 }
 
 template <class PointT>
-void PCDDivider<PointT>::dividePointCloud(const PclCloudPtr & cloud_ptr)
+void PCDDivider<PointT>::dividePointCloud(const PclCloudPtr & cloud_ptr, bool meta_gen)
 {
   if (!cloud_ptr || cloud_ptr->size() <= 0) {
+    RCLCPP_ERROR(logger_, "Empty cloud input");
     return;
+  } else {
+    RCLCPP_ERROR(logger_, "Size of the input cloud = %lu", cloud_ptr->size());
   }
 
   for (const PointT p : *cloud_ptr) {
@@ -200,12 +209,21 @@ void PCDDivider<PointT>::dividePointCloud(const PclCloudPtr & cloud_ptr)
       exit(EXIT_SUCCESS);
     }
 
-    auto tmp = pointToGrid2(p, grid_size_x_, grid_size_y_);
-    auto it = grid_to_cloud_.find(tmp);
+    auto grid_key = pointToGrid2(p, grid_size_x_, grid_size_y_);
+
+    grid_set_.insert(grid_key);
+
+    // If only want to generate metadata file, move to the next point
+    if (meta_gen) {
+      continue;
+    }
+
+    // Otherwise, distribute the point to the appropriate segment
+    auto it = grid_to_cloud_.find(grid_key);
 
     // If the grid has not existed yet, create a new one
     if (it == grid_to_cloud_.end()) {
-      auto & new_grid = grid_to_cloud_[tmp];
+      auto & new_grid = grid_to_cloud_[grid_key];
 
       std::get<0>(new_grid).reserve(max_block_size_);
 
@@ -227,11 +245,11 @@ void PCDDivider<PointT>::dividePointCloud(const PclCloudPtr & cloud_ptr)
         // Otherwise, update the seg_by_size_ if the change of size is significant
         if (cloud.size() - prev_size >= 10000) {
           prev_size = cloud.size();
-          auto seg_to_size_it = seg_to_size_itr_map_.find(tmp);
+          auto seg_to_size_it = seg_to_size_itr_map_.find(grid_key);
 
           if (seg_to_size_it == seg_to_size_itr_map_.end()) {
             auto size_it = seg_by_size_.insert(std::make_pair(prev_size, it));
-            seg_to_size_itr_map_[tmp] = size_it;
+            seg_to_size_itr_map_[grid_key] = size_it;
           } else {
             seg_by_size_.erase(seg_to_size_it->second);
             auto new_size_it = seg_by_size_.insert(std::make_pair(prev_size, it));
@@ -473,6 +491,17 @@ void PCDDivider<PointT>::saveGridInfoToYAML(const std::string & yaml_file_path)
   }
 
   yaml_file.close();
+
+  RCLCPP_INFO(logger_, "Save a yaml file at %s", yaml_file_path.c_str());
+}
+
+template <class PointT>
+void PCDDivider<PointT>::meta_generator()
+{
+  // Discover PCD files
+  auto pcd_list = discoverPCDs(input_pcd_or_dir_);
+
+  run(pcd_list, true);
 }
 
 template class PCDDivider<pcl::PointXYZ>;

map/autoware_pointcloud_divider/src/pointcloud_divider_node.cpp

@@ -87,7 +87,7 @@ PointCloudDivider::PointCloudDivider(const rclcpp::NodeOptions & node_options)
     pcd_divider_exe.run();
   }
 
-  rclcpp::shutdown();
+  // rclcpp::shutdown();
 }
 
 }  // namespace autoware::pointcloud_divider

map/autoware_tp_manager/CMakeLists.txt

@@ -3,10 +3,25 @@ project(autoware_tp_manager)
 
 find_package(autoware_cmake REQUIRED)
 autoware_package()
+ament_auto_find_build_dependencies()
 
-# Find packages
-find_package(yaml-cpp REQUIRED)
-find_package(PCL REQUIRED)
+find_package(Python3 COMPONENTS Development REQUIRED)
+
+# Using pybind11_add_module caused lots of trouble in linking to pointcloud divider
+# So I just used ament_auto_add_library
+ament_auto_add_library(pcd_divider_wrapper MODULE src/python_bindings.cpp)
+# This remove the prefix 'lib' of pcd_divider_wrapper
+# It cannot be import by the Python code
+set_target_properties(pcd_divider_wrapper PROPERTIES PREFIX "")
+# This is to fix "Python.h not found"
+target_include_directories(pcd_divider_wrapper PRIVATE ${Python3_INCLUDE_DIRS})
+# Linking to pointcloud divider
+ament_target_dependencies(pcd_divider_wrapper autoware_pointcloud_divider)
+
+# Copy the pcd_divider_wrapper.so to the install/${PROJECT_NAME}/lib/
+install(TARGETS pcd_divider_wrapper
+  LIBRARY DESTINATION lib/${PROJECT_NAME}
+)
 
 install(PROGRAMS
   scripts/tp_collector.py

map/autoware_tp_manager/package.xml

@@ -19,6 +19,7 @@
   <buildtool_depend>autoware_internal_debug_msgs</buildtool_depend>
 
   <depend>autoware_internal_debug_msgs</depend>
+  <depend>autoware_pointcloud_divider</depend>
   <depend>libpcl-all-dev</depend>
   <depend>yaml-cpp</depend>
 

map/autoware_tp_manager/scripts/tp_checker.py

@@ -52,12 +52,34 @@ def __initialize(self, score_dir: str):
             print("Error: {0} does not exist! Abort!".format(score_dir))
             exit()
 
-        self.pcd_path = os.path.join(score_dir, "pointcloud_map.pcd")
+        map_path_file = os.path.join(score_dir, "map_path.txt")
+
+        if not os.path.exists(map_path_file):
+            print(
+                "Error: the file containing the path to the PCD folder does not exist at {0}! Abort!".format(
+                    map_path_file
+                )
+            )
+            exit()
+
+        with open(map_path_file, "r") as f:
+            self.pcd_path = f.read()
 
         if not os.path.exists(self.pcd_path):
             print("Error: {0} does not exist! Abort!".format(self.pcd_path))
             exit()
 
+        # Look for the PCD map files
+        self.map_list = []
+        for fname in os.listdir(self.pcd_path):
+            full_name = os.path.join(self.pcd_path, fname)
+
+            if os.path.isfile(full_name):
+                name, ext = os.path.splitext(fname)
+
+                if ext == ".PCD" or ext == ".pcd":
+                    self.map_list.append(full_name)
+
         self.yaml_path = os.path.join(score_dir, "pointcloud_map_metadata.yaml")
 
         if not os.path.exists(self.yaml_path):
@@ -79,14 +101,14 @@ def __initialize(self, score_dir: str):
     # Read the input map directory and setup the segment dictionary
     def __get_pcd_segments_and_scores(self):
         # Read the metadata file and get the list of segments
-        print("Loading the PCD maps...")
+        print("Loading the segments...")
         self.segment_df = []
 
         with open(self.yaml_path, "r") as f:
             for key, value in yaml.safe_load(f).items():
                 if key != "x_resolution" and key != "y_resolution":
-                    self.segment_df.append([key, 0, 0])
                     seg_key = str(value[0]) + "_" + str(value[1])
+                    self.segment_df.append([seg_key, 0, 0])
                     self.segment_dict[seg_key] = len(self.segment_df) - 1
                 elif key == "x_resolution":
                     self.resolution = value
@@ -127,17 +149,26 @@ def __show(self):
         points = []
         pc2_width = 0
 
-        progress_bar = tqdm.tqdm(total=len(self.segment_df))
+        progress_bar = tqdm.tqdm(total=len(self.map_list))
         origin = None
 
-        for i in range(self.segment_df.shape[0]):
+        # for i in range(self.segment_df.shape[0]):
+        for seg_path in self.map_list:
             progress_bar.update(1)
             # Load the current segment
-            pcd = o3d.io.read_point_cloud(os.path.join(self.pcd_path, self.segment_df[i, 0]))
+            pcd = o3d.io.read_point_cloud(seg_path)
             np_pcd = np.asarray(pcd.points)
-            rgba = self.__set_color_based_on_mark(i)
 
             for p in np_pcd:
+                # Find the TP segments that contain the point @p
+                sx = int(p[0] / self.resolution) * int(self.resolution)
+                sy = int(p[1] / self.resolution) * int(self.resolution)
+
+                # Create a key to look for the segment
+                key = str(sx) + "_" + str(sy)
+                # Jump to the index of the segment and set the color for the point
+                rgba = self.__set_color_based_on_mark(self.segment_dict[key])
+
                 if origin is None:
                     origin = [p[0], p[1], p[2]]
                 pt = [p[0] - origin[0], p[1] - origin[1], p[2] - origin[2], rgba]