feat: initialize required number of collectors when the node start

Signed-off-by: vividf <yihsiang.fang@tier4.jp>

sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/cloud_collector.hpp

@@ -69,7 +69,7 @@ class CloudCollector
   std::unordered_map<std::string, sensor_msgs::msg::PointCloud2::SharedPtr>
   get_topic_to_cloud_map();
 
-  [[nodiscard]] CollectorStatus get_status() const;
+  [[nodiscard]] CollectorStatus get_status();
 
   void set_info(std::shared_ptr<CollectorInfoBase> collector_info);
   [[nodiscard]] std::shared_ptr<CollectorInfoBase> get_info() const;

sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_node.hpp

@@ -92,7 +92,8 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node
   std::list<std::shared_ptr<CloudCollector>> cloud_collectors_;
   std::unique_ptr<CollectorMatchingStrategy> collector_matching_strategy_;
   std::mutex cloud_collectors_mutex_;
-  static constexpr const int collectors_threshold = 10;
+  bool init_collector_list_ = false;
+  static constexpr const int num_of_collectors = 3;
 
   // default postfix name for synchronized pointcloud
   static constexpr const char * default_sync_topic_postfix = "_synchronized";
@@ -120,6 +121,7 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node
   void check_concat_status(diagnostic_updater::DiagnosticStatusWrapper & stat);
   std::string replace_sync_topic_name_postfix(
     const std::string & original_topic_name, const std::string & postfix);
+  void initialize_collector_list();
 };
 
 }  // namespace autoware::pointcloud_preprocessor

sensing/autoware_pointcloud_preprocessor/src/concatenate_data/cloud_collector.cpp

@@ -37,9 +37,11 @@ CloudCollector::CloudCollector(
   timeout_sec_(timeout_sec),
   debug_mode_(debug_mode)
 {
-  status_ = CollectorStatus::Processing;
+  status_ = CollectorStatus::Idle;
+
+  double temp_timeout_sec = 1.0;  // This will be overwritten when the first cloud comes
   const auto period_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
-    std::chrono::duration<double>(timeout_sec_));
+    std::chrono::duration<double>(temp_timeout_sec));
 
   timer_ =
     rclcpp::create_timer(ros2_parent_node_, ros2_parent_node_->get_clock(), period_ns, [this]() {
@@ -95,13 +97,16 @@ bool CloudCollector::process_pointcloud(
   return true;
 }
 
-CollectorStatus CloudCollector::get_status() const
+CollectorStatus CloudCollector::get_status()
 {
+  std::lock_guard<std::mutex> concatenate_lock(concatenate_mutex_);
   return status_;
 }
 
 void CloudCollector::concatenate_callback()
 {
+  if (topic_to_cloud_map_.size() == 0) return;
+
   if (debug_mode_) {
     show_debug_message();
   }
@@ -111,7 +116,6 @@ void CloudCollector::concatenate_callback()
   timer_->cancel();
 
   auto concatenated_cloud_result = concatenate_pointclouds(topic_to_cloud_map_);
-
   ros2_parent_node_->publish_clouds(std::move(concatenated_cloud_result), collector_info_);
 
   status_ = CollectorStatus::Finished;

sensing/autoware_pointcloud_preprocessor/src/concatenate_data/combine_cloud_handler.cpp

@@ -190,6 +190,7 @@ ConcatenatedCloudResult CombineCloudHandler::combine_pointclouds(
   ConcatenatedCloudResult concatenate_cloud_result;
 
   std::vector<rclcpp::Time> pc_stamps;
+  pc_stamps.reserve(topic_to_cloud_map.size());
   for (const auto & [topic, cloud] : topic_to_cloud_map) {
     pc_stamps.emplace_back(cloud->header.stamp);
   }

sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_node.cpp

@@ -179,10 +179,26 @@ std::string PointCloudConcatenateDataSynchronizerComponent::replace_sync_topic_n
   return replaced_topic_name;
 }
 
+void PointCloudConcatenateDataSynchronizerComponent::initialize_collector_list()
+{
+  // Initialize collector list
+  for (size_t i = 0; i < num_of_collectors; ++i) {
+    cloud_collectors_.emplace_back(std::make_shared<CloudCollector>(
+      std::dynamic_pointer_cast<PointCloudConcatenateDataSynchronizerComponent>(shared_from_this()),
+      combine_cloud_handler_, params_.input_topics.size(), params_.timeout_sec,
+      params_.debug_mode));
+  }
+  init_collector_list_ = true;
+}
+
 void PointCloudConcatenateDataSynchronizerComponent::cloud_callback(
   const sensor_msgs::msg::PointCloud2::SharedPtr & input_ptr, const std::string & topic_name)
 {
   stop_watch_ptr_->toc("processing_time", true);
+  if (!init_collector_list_) {
+    initialize_collector_list();
+  }
+
   double cloud_arrival_time = this->get_clock()->now().seconds();
   manage_collector_list();
 
@@ -235,7 +251,7 @@ void PointCloudConcatenateDataSynchronizerComponent::cloud_callback(
     }
   }
 
-  // Didn't find matched collector, create a new collector
+  // Didn't find matched collector
   if (!process_success) {
     // Reuse the collector if the status is IDLE
     std::shared_ptr<CloudCollector> selected_collector = nullptr;
@@ -247,21 +263,14 @@ void PointCloudConcatenateDataSynchronizerComponent::cloud_callback(
     if (it != cloud_collectors_.end()) {
       selected_collector = *it;
     }
-
-    if (!selected_collector) {
-      // If no idle collector exists, create a new collector
-      selected_collector = std::make_shared<CloudCollector>(
-        std::dynamic_pointer_cast<PointCloudConcatenateDataSynchronizerComponent>(
-          shared_from_this()),
-        combine_cloud_handler_, params_.input_topics.size(), params_.timeout_sec,
-        params_.debug_mode);
-
-      cloud_collectors_.push_back(selected_collector);
-    }
-
     cloud_collectors_lock.unlock();
-    collector_matching_strategy_->set_collector_info(selected_collector, matching_params);
-    (void)selected_collector->process_pointcloud(topic_name, input_ptr);
+    if (selected_collector) {
+      collector_matching_strategy_->set_collector_info(selected_collector, matching_params);
+      (void)selected_collector->process_pointcloud(topic_name, input_ptr);
+    } else {
+      // Handle case where no suitable collector is found
+      RCLCPP_WARN(get_logger(), "No available CloudCollector in IDLE state.");
+    }
   }
 }
 
@@ -376,7 +385,7 @@ void PointCloudConcatenateDataSynchronizerComponent::manage_collector_list()
     }
   }
 
-  if (num_processing_collectors >= collectors_threshold) {
+  if (num_processing_collectors == num_of_collectors) {
     auto min_it = cloud_collectors_.end();
     constexpr double k_max_timestamp = std::numeric_limits<double>::max();
     double min_timestamp = k_max_timestamp;
@@ -405,8 +414,8 @@ void PointCloudConcatenateDataSynchronizerComponent::manage_collector_list()
     if (min_it != cloud_collectors_.end()) {
       RCLCPP_WARN_STREAM_THROTTLE(
         this->get_logger(), *this->get_clock(), 1000,
-        "Reset the oldest collector because the number of collectors ("
-          << num_processing_collectors << ") exceeds the limit (" << collectors_threshold << ").");
+        "Reset the oldest collector because the number of processing collectors ("
+          << num_processing_collectors << ") equal to the limit (" << num_of_collectors << ").");
       (*min_it)->reset();
     }
   }