feat(autoware_pointcloud_preprocessor): redesign concatenate and time sync node

sensing/autoware_pointcloud_preprocessor/CMakeLists.txt

@@ -64,7 +64,9 @@ 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/concatenate_pointclouds.cpp
   src/crop_box_filter/crop_box_filter_node.cpp
   src/time_synchronizer/time_synchronizer_node.cpp
@@ -111,7 +113,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 +245,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
+    lidar_timestamp_offsets: [0.0, 0.015, 0.016]
+    lidar_timestamp_noise_window: [0.01, 0.01, 0.01]

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

@@ -0,0 +1,62 @@
+// 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;
+
+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);
+
+  void set_reference_timestamp(double timestamp, double noise_window);
+  std::tuple<double, double> get_reference_timestamp_boundary();
+  void 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();
+
+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_;
+  double reference_timestamp_min_{0.0};
+  double reference_timestamp_max_{0.0};
+  bool debug_mode_;
+};
+
+}  // namespace autoware::pointcloud_preprocessor

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

@@ -0,0 +1,110 @@
+// 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 & input);
+  void process_odometry(const nav_msgs::msg::Odometry::ConstSharedPtr & input);
+
+  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

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

@@ -0,0 +1,125 @@
+// Copyright 2023 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 <list>
+#include <memory>
+#include <mutex>
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+// ROS includes
+#include "cloud_collector.hpp"
+#include "combine_cloud_handler.hpp"
+
+#include <autoware/universe_utils/ros/debug_publisher.hpp>
+#include <autoware/universe_utils/system/stop_watch.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
+{
+class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node
+{
+public:
+  explicit PointCloudConcatenateDataSynchronizerComponent(const rclcpp::NodeOptions & node_options);
+  ~PointCloudConcatenateDataSynchronizerComponent() override = default;
+  void publish_clouds(
+    ConcatenatedCloudResult && concatenated_cloud_result, double reference_timestamp_min,
+    double reference_timestamp_max);
+  void delete_collector(CloudCollector & cloud_collector);
+  std::list<std::shared_ptr<CloudCollector>> get_cloud_collectors();
+  void add_cloud_collector(const std::shared_ptr<CloudCollector> & collector);
+
+private:
+  struct Parameters
+  {
+    bool debug_mode;
+    bool has_static_tf_only;
+    double rosbag_length;
+    int maximum_queue_size;
+    double timeout_sec;
+    bool is_motion_compensated;
+    bool publish_synchronized_pointcloud;
+    bool keep_input_frame_in_synchronized_pointcloud;
+    bool publish_previous_but_late_pointcloud;
+    std::string synchronized_pointcloud_postfix;
+    std::string input_twist_topic_type;
+    std::vector<std::string> input_topics;
+    std::string output_frame;
+    std::vector<double> lidar_timestamp_offsets;
+    std::vector<double> lidar_timestamp_noise_window;
+  } params_;
+
+  double current_concatenate_cloud_timestamp_{0.0};
+  double latest_concatenate_cloud_timestamp_{0.0};
+  bool drop_previous_but_late_pointcloud_{false};
+  bool publish_pointcloud_{false};
+  bool is_concatenated_cloud_empty_{false};
+  double diagnostic_reference_timestamp_min_{0.0};
+  double diagnostic_reference_timestamp_max_{0.0};
+  std::unordered_map<std::string, double> diagnostic_topic_to_original_stamp_map_;
+
+  std::shared_ptr<CombineCloudHandler> combine_cloud_handler_;
+  std::list<std::shared_ptr<CloudCollector>> cloud_collectors_;
+  std::mutex cloud_collectors_mutex_;
+  std::unordered_map<std::string, double> topic_to_offset_map_;
+  std::unordered_map<std::string, double> topic_to_noise_window_map_;
+
+  // default postfix name for synchronized pointcloud
+  static constexpr const char * default_sync_topic_postfix = "_synchronized";
+
+  // subscribers
+  std::vector<rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr> pointcloud_subs_;
+  rclcpp::Subscription<geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr twist_sub_;
+  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
+
+  // publishers
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr concatenated_cloud_publisher_;
+  std::unordered_map<std::string, rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr>
+    topic_to_transformed_cloud_publisher_map_;
+  std::unique_ptr<autoware::universe_utils::DebugPublisher> debug_publisher_;
+
+  std::unique_ptr<autoware::universe_utils::StopWatch<std::chrono::milliseconds>> stop_watch_ptr_;
+  diagnostic_updater::Updater diagnostic_updater_{this};
+
+  void cloud_callback(
+    const sensor_msgs::msg::PointCloud2::SharedPtr & input_ptr, const std::string & topic_name);
+  void twist_callback(const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr input);
+  void odom_callback(const nav_msgs::msg::Odometry::ConstSharedPtr input);
+
+  static std::string format_timestamp(double timestamp);
+  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);
+};
+
+}  // namespace autoware::pointcloud_preprocessor