diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/cloud_collector.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/cloud_collector.hpp index 55e106cc3f32e..3e63c7ba2264d 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/cloud_collector.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/cloud_collector.hpp @@ -69,7 +69,7 @@ class CloudCollector std::unordered_map get_topic_to_cloud_map(); - [[nodiscard]] CollectorStatus get_status() const; + [[nodiscard]] CollectorStatus get_status(); void set_info(std::shared_ptr collector_info); [[nodiscard]] std::shared_ptr get_info() const; diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_node.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_node.hpp index 699db3535af23..fc422c7d2e54e 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_node.hpp +++ b/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> cloud_collectors_; std::unique_ptr 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 diff --git a/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/cloud_collector.cpp b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/cloud_collector.cpp index 63db05294c1c2..1a85c9dd36341 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/cloud_collector.cpp +++ b/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::duration(timeout_sec_)); + std::chrono::duration(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 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; diff --git a/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/combine_cloud_handler.cpp b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/combine_cloud_handler.cpp index d68218314830b..c1a1d19782ada 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/combine_cloud_handler.cpp +++ b/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 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); } diff --git a/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_node.cpp b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_node.cpp index a5f5328017468..826466b917d9c 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_node.cpp +++ b/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( + std::dynamic_pointer_cast(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 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( - std::dynamic_pointer_cast( - 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::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(); } }