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 1a85c9dd36341..57ce5c400188a 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/cloud_collector.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/cloud_collector.cpp @@ -39,9 +39,8 @@ CloudCollector::CloudCollector( { 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(temp_timeout_sec)); + std::chrono::duration(timeout_sec_)); timer_ = rclcpp::create_timer(ros2_parent_node_, ros2_parent_node_->get_clock(), period_ns, [this]() { @@ -49,6 +48,8 @@ CloudCollector::CloudCollector( if (status_ == CollectorStatus::Finished) return; concatenate_callback(); }); + + timer_->cancel(); } void CloudCollector::set_info(std::shared_ptr collector_info) @@ -73,6 +74,7 @@ bool CloudCollector::process_pointcloud( if (status_ == CollectorStatus::Finished) { return false; } + if (status_ == CollectorStatus::Idle) { // Add first pointcloud to the collector, restart the timer status_ = CollectorStatus::Processing; @@ -105,8 +107,6 @@ CollectorStatus CloudCollector::get_status() void CloudCollector::concatenate_callback() { - if (topic_to_cloud_map_.size() == 0) return; - if (debug_mode_) { show_debug_message(); } 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 c1a1d19782ada..8574e3e8a35ae 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 @@ -189,6 +189,8 @@ ConcatenatedCloudResult CombineCloudHandler::combine_pointclouds( { ConcatenatedCloudResult concatenate_cloud_result; + if (topic_to_cloud_map.empty()) return concatenate_cloud_result; + std::vector pc_stamps; pc_stamps.reserve(topic_to_cloud_map.size()); for (const auto & [topic, cloud] : topic_to_cloud_map) {