From d01eba81f0b51cb3103538ee302877228e51ee1b Mon Sep 17 00:00:00 2001 From: vividf Date: Tue, 7 Jan 2025 00:45:44 +0900 Subject: [PATCH] chroe: refactor collectorinfo structure Signed-off-by: vividf --- .../config/concatenate_pointclouds.param.yaml | 23 +++++++++++++------ .../concatenate_data/cloud_collector.hpp | 19 +++++---------- .../collector_matching_strategy.hpp | 3 --- .../src/concatenate_data/cloud_collector.cpp | 20 +++++++--------- .../collector_matching_strategy.cpp | 12 ++++------ 5 files changed, 34 insertions(+), 43 deletions(-) diff --git a/sensing/autoware_pointcloud_preprocessor/config/concatenate_pointclouds.param.yaml b/sensing/autoware_pointcloud_preprocessor/config/concatenate_pointclouds.param.yaml index 449795c328402..56fe6643b9973 100644 --- a/sensing/autoware_pointcloud_preprocessor/config/concatenate_pointclouds.param.yaml +++ b/sensing/autoware_pointcloud_preprocessor/config/concatenate_pointclouds.param.yaml @@ -1,12 +1,21 @@ /**: ros__parameters: - output_frame: base_link - has_static_tf_only: true + debug_mode: false + has_static_tf_only: false + rosbag_length: 10.0 + maximum_queue_size: 5 + timeout_sec: 0.2 + is_motion_compensated: true + publish_synchronized_pointcloud: true + keep_input_frame_in_synchronized_pointcloud: true + publish_previous_but_late_pointcloud: false + synchronized_pointcloud_postfix: pointcloud + input_twist_topic_type: twist input_topics: [ - "/sensing/lidar/left/pointcloud_before_sync", "/sensing/lidar/right/pointcloud_before_sync", - "/sensing/lidar/top/pointcloud_before_sync" + "/sensing/lidar/top/pointcloud_before_sync", + "/sensing/lidar/left/pointcloud_before_sync", ] - max_queue_size: 5 - timeout_sec: 0.033 - input_offset: [0.0 ,0.0 ,0.0] + output_frame: base_link + matching_strategy: + type: naive 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 ae97752b2f6d7..13604569df9a8 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 @@ -18,7 +18,6 @@ #include #include -#include #include namespace autoware::pointcloud_preprocessor @@ -27,32 +26,26 @@ namespace autoware::pointcloud_preprocessor class PointCloudConcatenateDataSynchronizerComponent; class CombineCloudHandler; -enum class CollectorStrategyType { Naive, Advanced }; - struct CollectorInfoBase { virtual ~CollectorInfoBase() = default; - [[nodiscard]] virtual CollectorStrategyType getStrategyType() const = 0; }; struct NaiveCollectorInfo : public CollectorInfoBase { - double timestamp{0.0}; + double timestamp; - [[nodiscard]] CollectorStrategyType getStrategyType() const override - { - return CollectorStrategyType::Naive; - } + explicit NaiveCollectorInfo(double timestamp = 0.0) : timestamp(timestamp) {} }; struct AdvancedCollectorInfo : public CollectorInfoBase { - double timestamp{0.0}; - double noise_window{0.0}; + double timestamp; + double noise_window; - [[nodiscard]] CollectorStrategyType getStrategyType() const override + explicit AdvancedCollectorInfo(double timestamp = 0.0, double noise_window = 0.0) + : timestamp(timestamp), noise_window(noise_window) { - return CollectorStrategyType::Advanced; } }; diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/collector_matching_strategy.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/collector_matching_strategy.hpp index a6aa39ef7ab74..8502d3f89bf42 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/collector_matching_strategy.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/collector_matching_strategy.hpp @@ -47,9 +47,6 @@ class CollectorMatchingStrategy const MatchingParams & params) const = 0; virtual void set_collector_info( std::shared_ptr & collector, const MatchingParams & matching_params) = 0; - -protected: - CollectorStrategyType strategy_type_; }; class NaiveMatchingStrategy : public CollectorMatchingStrategy 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 c3e2d7eedff25..63ee23d204788 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/cloud_collector.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/cloud_collector.cpp @@ -128,18 +128,14 @@ void CloudCollector::show_debug_message() log_stream << "Collector's concatenate callback time: " << ros2_parent_node_->get_clock()->now().seconds() << " seconds\n"; - if (collector_info_) { - if (collector_info_->getStrategyType() == CollectorStrategyType::Advanced) { - auto advanced_info = std::dynamic_pointer_cast(collector_info_); - log_stream << "Advanced strategy:\n Collector's reference time min: " - << advanced_info->timestamp - advanced_info->noise_window - << " to max: " << advanced_info->timestamp + advanced_info->noise_window - << " seconds\n"; - } else if (collector_info_->getStrategyType() == CollectorStrategyType::Naive) { - auto naive_info = std::dynamic_pointer_cast(collector_info_); - log_stream << "Naive strategy:\n Collector's timestamp: " << naive_info->timestamp - << " seconds\n"; - } + if (auto advanced_info = std::dynamic_pointer_cast(collector_info_)) { + log_stream << "Advanced strategy:\n Collector's reference time min: " + << advanced_info->timestamp - advanced_info->noise_window + << " to max: " << advanced_info->timestamp + advanced_info->noise_window + << " seconds\n"; + } else if (auto naive_info = std::dynamic_pointer_cast(collector_info_)) { + log_stream << "Naive strategy:\n Collector's timestamp: " << naive_info->timestamp + << " seconds\n"; } log_stream << "Time until trigger: " << (time_until_trigger.count() / 1e9) << " seconds\n"; diff --git a/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/collector_matching_strategy.cpp b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/collector_matching_strategy.cpp index fa233994c34e7..5addd7044e579 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/collector_matching_strategy.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/collector_matching_strategy.cpp @@ -28,7 +28,6 @@ namespace autoware::pointcloud_preprocessor NaiveMatchingStrategy::NaiveMatchingStrategy(rclcpp::Node & node) { - strategy_type_ = CollectorStrategyType::Naive; RCLCPP_INFO(node.get_logger(), "Utilize naive matching strategy"); } @@ -58,15 +57,13 @@ std::optional> NaiveMatchingStrategy::match_clou void NaiveMatchingStrategy::set_collector_info( std::shared_ptr & collector, const MatchingParams & matching_params) { - auto info = std::make_shared(); - info->timestamp = matching_params.cloud_arrival_time; + auto info = std::make_shared(matching_params.cloud_arrival_time); collector->set_info(info); } AdvancedMatchingStrategy::AdvancedMatchingStrategy( rclcpp::Node & node, std::vector input_topics) { - strategy_type_ = CollectorStrategyType::Advanced; auto lidar_timestamp_offsets = node.declare_parameter>("matching_strategy.lidar_timestamp_offsets"); auto lidar_timestamp_noise_window = @@ -114,10 +111,9 @@ std::optional> AdvancedMatchingStrategy::match_c void AdvancedMatchingStrategy::set_collector_info( std::shared_ptr & collector, const MatchingParams & matching_params) { - auto info = std::make_shared(); - info->timestamp = - matching_params.cloud_timestamp - topic_to_offset_map_[matching_params.topic_name]; - info->noise_window = topic_to_noise_window_map_[matching_params.topic_name]; + auto info = std::make_shared( + matching_params.cloud_timestamp - topic_to_offset_map_[matching_params.topic_name], + topic_to_noise_window_map_[matching_params.topic_name]); collector->set_info(info); }