feat(autoware_pointcloud_preprocessor): redesign concatenate and time sync node

sensing/autoware_pointcloud_preprocessor/CMakeLists.txt

@@ -64,7 +64,10 @@ ament_target_dependencies(faster_voxel_grid_downsample_filter
 )
 
 ament_auto_add_library(pointcloud_preprocessor_filter SHARED
-  src/concatenate_data/concatenate_and_time_sync_nodelet.cpp
+  src/concatenate_data/concatenate_and_time_sync_node.cpp
+  src/concatenate_data/combine_cloud_handler.cpp
+  src/concatenate_data/cloud_collector.cpp
+  src/concatenate_data/collector_matching_strategy.cpp
   src/concatenate_data/concatenate_pointclouds.cpp
   src/crop_box_filter/crop_box_filter_node.cpp
   src/time_synchronizer/time_synchronizer_node.cpp
@@ -111,7 +114,7 @@ rclcpp_components_register_node(pointcloud_preprocessor_filter
 # ========== Concatenate and Sync data ==========
 rclcpp_components_register_node(pointcloud_preprocessor_filter
   PLUGIN "autoware::pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent"
-  EXECUTABLE concatenate_data_node)
+  EXECUTABLE concatenate_and_time_sync_node)
 
 # ========== CropBox ==========
 rclcpp_components_register_node(pointcloud_preprocessor_filter
@@ -243,8 +246,17 @@ if(BUILD_TESTING)
     test/test_distortion_corrector_node.cpp
   )
 
+  ament_add_gtest(test_concatenate_node_unit
+    test/test_concatenate_node_unit.cpp
+  )
+
   target_link_libraries(test_utilities pointcloud_preprocessor_filter)
   target_link_libraries(test_distortion_corrector_node pointcloud_preprocessor_filter)
+  target_link_libraries(test_concatenate_node_unit pointcloud_preprocessor_filter)
 
+  add_ros_test(
+    test/test_concatenate_node_component.py
+    TIMEOUT "50"
+  )
 
 endif()

sensing/autoware_pointcloud_preprocessor/config/concatenate_and_time_sync_node.param.yaml

@@ -0,0 +1,21 @@
+/**:
+  ros__parameters:
+    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/right/pointcloud_before_sync",
+                    "/sensing/lidar/top/pointcloud_before_sync",
+                    "/sensing/lidar/left/pointcloud_before_sync",
+                ]
+    output_frame: base_link
+    matching_strategy:
+      type: naive

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

@@ -0,0 +1,96 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#pragma once
+
+#include "combine_cloud_handler.hpp"
+
+#include <memory>
+#include <string>
+#include <tuple>
+#include <unordered_map>
+
+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};
+
+  [[nodiscard]] CollectorStrategyType getStrategyType() const override
+  {
+    return CollectorStrategyType::Naive;
+  }
+};
+
+struct AdvancedCollectorInfo : public CollectorInfoBase
+{
+  double timestamp{0.0};
+  double noise_window{0.0};
+
+  [[nodiscard]] CollectorStrategyType getStrategyType() const override
+  {
+    return CollectorStrategyType::Advanced;
+  }
+};
+
+class CloudCollector
+{
+public:
+  CloudCollector(
+    std::shared_ptr<PointCloudConcatenateDataSynchronizerComponent> && ros2_parent_node,
+    std::shared_ptr<CombineCloudHandler> & combine_cloud_handler, int num_of_clouds,
+    double timeout_sec, bool debug_mode);
+  bool topic_exists(const std::string & topic_name);
+  bool process_pointcloud(
+    const std::string & topic_name, sensor_msgs::msg::PointCloud2::SharedPtr cloud);
+  void concatenate_callback();
+
+  ConcatenatedCloudResult concatenate_pointclouds(
+    std::unordered_map<std::string, sensor_msgs::msg::PointCloud2::SharedPtr> topic_to_cloud_map);
+
+  std::unordered_map<std::string, sensor_msgs::msg::PointCloud2::SharedPtr>
+  get_topic_to_cloud_map();
+
+  [[nodiscard]] bool concatenate_finished() const;
+
+  void set_info(std::shared_ptr<CollectorInfoBase> collector_info);
+  [[nodiscard]] std::shared_ptr<CollectorInfoBase> get_info() const;
+  void show_debug_message();
+
+private:
+  std::shared_ptr<PointCloudConcatenateDataSynchronizerComponent> ros2_parent_node_;
+  std::shared_ptr<CombineCloudHandler> combine_cloud_handler_;
+  rclcpp::TimerBase::SharedPtr timer_;
+  std::unordered_map<std::string, sensor_msgs::msg::PointCloud2::SharedPtr> topic_to_cloud_map_;
+  uint64_t num_of_clouds_;
+  double timeout_sec_;
+  bool debug_mode_;
+  bool concatenate_finished_{false};
+  std::mutex concatenate_mutex_;
+  std::shared_ptr<CollectorInfoBase> collector_info_;
+};
+
+}  // namespace autoware::pointcloud_preprocessor

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

@@ -0,0 +1,85 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#pragma once
+
+#include "cloud_collector.hpp"
+
+#include <rclcpp/node.hpp>
+
+#include <sensor_msgs/msg/point_cloud2.hpp>
+
+#include <list>
+#include <memory>
+#include <optional>
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+namespace autoware::pointcloud_preprocessor
+{
+
+struct MatchingParams
+{
+  std::string topic_name;
+  double cloud_timestamp;
+  double cloud_arrival_time;
+};
+
+class CollectorMatchingStrategy
+{
+public:
+  virtual ~CollectorMatchingStrategy() = default;
+
+  [[nodiscard]] virtual std::optional<std::shared_ptr<CloudCollector>> match_cloud_to_collector(
+    const std::list<std::shared_ptr<CloudCollector>> & cloud_collectors,
+    const MatchingParams & params) const = 0;
+  virtual void set_collector_info(
+    std::shared_ptr<CloudCollector> & collector, const MatchingParams & matching_params) = 0;
+
+protected:
+  CollectorStrategyType strategy_type_;
+};
+
+class NaiveMatchingStrategy : public CollectorMatchingStrategy
+{
+public:
+  explicit NaiveMatchingStrategy(rclcpp::Node & node);
+  [[nodiscard]] std::optional<std::shared_ptr<CloudCollector>> match_cloud_to_collector(
+    const std::list<std::shared_ptr<CloudCollector>> & cloud_collectors,
+    const MatchingParams & params) const override;
+  void set_collector_info(
+    std::shared_ptr<CloudCollector> & collector, const MatchingParams & matching_params) override;
+};
+
+class AdvancedMatchingStrategy : public CollectorMatchingStrategy
+{
+public:
+  explicit AdvancedMatchingStrategy(rclcpp::Node & node, std::vector<std::string> input_topics);
+
+  [[nodiscard]] std::optional<std::shared_ptr<CloudCollector>> match_cloud_to_collector(
+    const std::list<std::shared_ptr<CloudCollector>> & cloud_collectors,
+    const MatchingParams & params) const override;
+  void set_collector_info(
+    std::shared_ptr<CloudCollector> & collector, const MatchingParams & matching_params) override;
+
+private:
+  std::vector<std::string> input_topics_;
+  std::unordered_map<std::string, double> topic_to_offset_map_;
+  std::unordered_map<std::string, double> topic_to_noise_window_map_;
+};
+
+std::shared_ptr<CollectorMatchingStrategy> parse_matching_strategy(rclcpp::Node & node);
+
+}  // namespace autoware::pointcloud_preprocessor

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

@@ -0,0 +1,111 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#pragma once
+
+#include <deque>
+#include <memory>
+#include <optional>
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+// ROS includes
+#include "autoware/point_types/types.hpp"
+
+#include <autoware/universe_utils/ros/managed_transform_buffer.hpp>
+#include <diagnostic_updater/diagnostic_updater.hpp>
+#include <point_cloud_msg_wrapper/point_cloud_msg_wrapper.hpp>
+
+#include <diagnostic_msgs/msg/diagnostic_status.hpp>
+#include <geometry_msgs/msg/twist_stamped.hpp>
+#include <geometry_msgs/msg/twist_with_covariance_stamped.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <sensor_msgs/point_cloud2_iterator.hpp>
+#include <tier4_debug_msgs/msg/int32_stamped.hpp>
+#include <tier4_debug_msgs/msg/string_stamped.hpp>
+
+#include <message_filters/pass_through.h>
+#include <message_filters/subscriber.h>
+#include <message_filters/sync_policies/approximate_time.h>
+#include <message_filters/sync_policies/exact_time.h>
+#include <message_filters/synchronizer.h>
+
+namespace autoware::pointcloud_preprocessor
+{
+using autoware::point_types::PointXYZIRC;
+using point_cloud_msg_wrapper::PointCloud2Modifier;
+
+struct ConcatenatedCloudResult
+{
+  sensor_msgs::msg::PointCloud2::SharedPtr concatenate_cloud_ptr{nullptr};
+  std::optional<std::unordered_map<std::string, sensor_msgs::msg::PointCloud2::SharedPtr>>
+    topic_to_transformed_cloud_map;
+  std::unordered_map<std::string, double> topic_to_original_stamp_map;
+};
+
+class CombineCloudHandler
+{
+private:
+  rclcpp::Node & node_;
+
+  std::vector<std::string> input_topics_;
+  std::string output_frame_;
+  bool is_motion_compensated_;
+  bool publish_synchronized_pointcloud_;
+  bool keep_input_frame_in_synchronized_pointcloud_;
+  std::unique_ptr<autoware::universe_utils::ManagedTransformBuffer> managed_tf_buffer_{nullptr};
+
+  std::deque<geometry_msgs::msg::TwistStamped> twist_queue_;
+
+  /// @brief RclcppTimeHash structure defines a custom hash function for the rclcpp::Time type by
+  /// using its nanoseconds representation as the hash value.
+  struct RclcppTimeHash
+  {
+    std::size_t operator()(const rclcpp::Time & t) const
+    {
+      return std::hash<int64_t>()(t.nanoseconds());
+    }
+  };
+
+  static void convert_to_xyzirc_cloud(
+    const sensor_msgs::msg::PointCloud2::SharedPtr & input_cloud,
+    sensor_msgs::msg::PointCloud2::SharedPtr & xyzirc_cloud);
+
+  void correct_pointcloud_motion(
+    const std::shared_ptr<sensor_msgs::msg::PointCloud2> & transformed_cloud_ptr,
+    const std::vector<rclcpp::Time> & pc_stamps,
+    std::unordered_map<rclcpp::Time, Eigen::Matrix4f, RclcppTimeHash> & transform_memo,
+    std::shared_ptr<sensor_msgs::msg::PointCloud2> transformed_delay_compensated_cloud_ptr);
+
+public:
+  CombineCloudHandler(
+    rclcpp::Node & node, std::vector<std::string> input_topics, std::string output_frame,
+    bool is_motion_compensated, bool publish_synchronized_pointcloud,
+    bool keep_input_frame_in_synchronized_pointcloud, bool has_static_tf_only);
+  void process_twist(
+    const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr & twist_msg);
+  void process_odometry(const nav_msgs::msg::Odometry::ConstSharedPtr & odometry_msg);
+
+  ConcatenatedCloudResult combine_pointclouds(
+    std::unordered_map<std::string, sensor_msgs::msg::PointCloud2::SharedPtr> & topic_to_cloud_map);
+
+  Eigen::Matrix4f compute_transform_to_adjust_for_old_timestamp(
+    const rclcpp::Time & old_stamp, const rclcpp::Time & new_stamp);
+
+  std::deque<geometry_msgs::msg::TwistStamped> get_twist_queue();
+};
+
+}  // namespace autoware::pointcloud_preprocessor