feat(pointcloud_preprocessor): separate concatenate filter node and output synchronized pointcloud

sensing/pointcloud_preprocessor/CMakeLists.txt

@@ -43,7 +43,9 @@ ament_target_dependencies(pointcloud_preprocessor_filter_base
 
 ament_auto_add_library(pointcloud_preprocessor_filter SHARED
   src/utility/utilities.cpp
-  src/concatenate_data/concatenate_data_nodelet.cpp
+  src/concatenate_data/concatenate_and_time_sync_nodelet.cpp
+  src/concatenate_data/concatenate_pointclouds.cpp
+  src/time_synchronizer/time_synchronizer_nodelet.cpp
   src/crop_box_filter/crop_box_filter_nodelet.cpp
   src/downsample_filter/voxel_grid_downsample_filter_nodelet.cpp
   src/downsample_filter/random_downsample_filter_nodelet.cpp
@@ -70,7 +72,17 @@ target_link_libraries(pointcloud_preprocessor_filter
   ${PCL_LIBRARIES}
 )
 
+# ========== Time synchronizer ==========
+rclcpp_components_register_node(pointcloud_preprocessor_filter
+  PLUGIN "pointcloud_preprocessor::PointCloudDataSynchronizerComponent"
+  EXECUTABLE time_synchronizer_node)
+
 # ========== Concatenate data ==========
+rclcpp_components_register_node(pointcloud_preprocessor_filter
+  PLUGIN "pointcloud_preprocessor::PointCloudConcatenationComponent"
+  EXECUTABLE concatenate_pointclouds_node)
+
+# ========== Concatenate and Sync data ==========
 rclcpp_components_register_node(pointcloud_preprocessor_filter
   PLUGIN "pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent"
   EXECUTABLE concatenate_data_node)

sensing/pointcloud_preprocessor/docs/concatenate-data.md

@@ -37,9 +37,36 @@ The figure below represents the reception time of each sensor data and how it is
 
 ### Core Parameters
 
-| Name          | Type   | Default Value | Description                                                                                                                                                                              |
-| ------------- | ------ | ------------- | ---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
-| `timeout_sec` | double | 0.1           | tolerance of time to publish next pointcloud [s]<br>When this time limit is exceeded, the filter concatenates and publishes pointcloud, even if not all the point clouds are subscribed. |
+| Name                              | Type             | Default Value | Description                                                                                                                                                                                                                                                |
+| --------------------------------- | ---------------- | ------------- | ---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
+| `timeout_sec`                     | double           | 0.1           | tolerance of time to publish next pointcloud [s]<br>When this time limit is exceeded, the filter concatenates and publishes pointcloud, even if not all the point clouds are subscribed.                                                                   |
+| `input_offset`                    | vector of double | []            | This parameter can control waiting time for each input sensor pointcloud [s]. You must to set the same length of offsets with input pointclouds numbers. <br> For its tuning, please see [actual usage page](#how-to-tuning-timeout_sec-and-input_offset). |
+| `publish_synchronized_pointcloud` | bool             | false         | If true, publish the time synchronized pointclouds. All input pointclouds are transformed and then re-published as message named `<original_msg_name>_synchronized`.                                                                                       |
+
+## Actual Usage
+
+For the example of actual usage of this node, please refer to the [preprocessor.launch.py](../launch/preprocessor.launch.py) file.
+
+### How to tuning timeout_sec and input_offset
+
+The values in `timeout_sec` and `input_offset` are used in the timercallback to control concatenation timings.
+
+- Assumptions
+  - when the timer runs out, we concatenate the pointclouds in the buffer
+  - when the first pointcloud comes to buffer, we reset the timer to `timeout_sec`
+  - when the second and later pointclouds comes to buffer, we reset the timer to `timeout_sec` - `input_offset`
+  - we assume all lidar has same frequency
+
+| Name           | Description                                          | How to tune                                                                                                                                                          |
+| -------------- | ---------------------------------------------------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
+| `timeout_sec`  | timeout sec for default timer                        | To avoid mis-concatenation, at least this value must be shorter than sampling time.                                                                                  |
+| `input_offset` | timeout extension when a pointcloud comes to buffer. | The amount of waiting time will be `timeout_sec` - `input_offset`. So, you will need to set larger value for the last-coming pointcloud and smaller for fore-coming. |
+
+### Node separation options for future
+
+Since the pointcloud concatenation has two process, "time synchronization" and "pointcloud concatenation", it is possible to separate these processes.
+
+In the future, Nodes will be completely separated in order to achieve node loosely coupled nature, but currently both nodes can be selected for backward compatibility ([See this PR](https://github.com/autowarefoundation/autoware.universe/pull/3312)).
 
 ## Assumptions / Known limits
 

sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_data_nodelet.hpp → sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp

@@ -1,4 +1,4 @@
-// Copyright 2020 Tier IV, Inc.
+// 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.
@@ -49,8 +49,8 @@
  *
  */
 
-#ifndef POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__CONCATENATE_DATA_NODELET_HPP_
-#define POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__CONCATENATE_DATA_NODELET_HPP_
+#ifndef POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__CONCATENATE_AND_TIME_SYNC_NODELET_HPP_
+#define POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__CONCATENATE_AND_TIME_SYNC_NODELET_HPP_
 
 #include <deque>
 #include <map>
@@ -114,12 +114,17 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node
 private:
   /** \brief The output PointCloud publisher. */
   rclcpp::Publisher<PointCloud2>::SharedPtr pub_output_;
+  /** \brief Delay Compensated PointCloud publisher*/
+  std::map<std::string, rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr>
+    transformed_raw_pc_publisher_map_;
 
   /** \brief The maximum number of messages that we can store in the queue. */
   int maximum_queue_size_ = 3;
 
   double timeout_sec_ = 0.1;
 
+  bool publish_synchronized_pointcloud_;
+
   std::set<std::string> not_subscribed_topic_names_;
 
   /** \brief A vector of subscriber. */
@@ -151,9 +156,10 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node
   std::map<std::string, double> offset_map_;
 
   void transformPointCloud(const PointCloud2::ConstSharedPtr & in, PointCloud2::SharedPtr & out);
-  void combineClouds(
-    const PointCloud2::ConstSharedPtr & in1, const PointCloud2::ConstSharedPtr & in2,
-    PointCloud2::SharedPtr & out);
+  Eigen::Matrix4f computeTransformToAdjustForOldTimestamp(
+    const rclcpp::Time & old_stamp, const rclcpp::Time & new_stamp);
+  std::map<std::string, sensor_msgs::msg::PointCloud2::SharedPtr> combineClouds(
+    sensor_msgs::msg::PointCloud2::SharedPtr & concat_cloud_ptr);
   void publish();
 
   void convertToXYZICloud(
@@ -175,4 +181,4 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node
 
 }  // namespace pointcloud_preprocessor
 
-#endif  // POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__CONCATENATE_DATA_NODELET_HPP_
+#endif  // POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__CONCATENATE_AND_TIME_SYNC_NODELET_HPP_

sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp

@@ -0,0 +1,181 @@
+// 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.
+
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id: concatenate_data.cpp 35231 2011-01-14 05:33:20Z rusu $
+ *
+ */
+
+#ifndef POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__CONCATENATE_POINTCLOUDS_HPP_
+#define POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__CONCATENATE_POINTCLOUDS_HPP_
+
+#include <deque>
+#include <map>
+#include <memory>
+#include <mutex>
+#include <set>
+#include <string>
+#include <vector>
+
+// ROS includes
+#include "autoware_point_types/types.hpp"
+
+#include <diagnostic_updater/diagnostic_updater.hpp>
+#include <point_cloud_msg_wrapper/point_cloud_msg_wrapper.hpp>
+#include <tier4_autoware_utils/ros/debug_publisher.hpp>
+#include <tier4_autoware_utils/system/stop_watch.hpp>
+
+#include <autoware_auto_vehicle_msgs/msg/velocity_report.hpp>
+#include <diagnostic_msgs/msg/diagnostic_status.hpp>
+#include <geometry_msgs/msg/twist_stamped.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>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+
+namespace pointcloud_preprocessor
+{
+using autoware_point_types::PointXYZI;
+using point_cloud_msg_wrapper::PointCloud2Modifier;
+/** \brief @b PointCloudConcatenationComponent is a special form of data
+ * synchronizer: it listens for a set of input PointCloud messages on the same topic,
+ * checks their timestamps, and concatenates their fields together into a single
+ * PointCloud output message.
+ * \author Radu Bogdan Rusu
+ */
+class PointCloudConcatenationComponent : public rclcpp::Node
+{
+public:
+  typedef sensor_msgs::msg::PointCloud2 PointCloud2;
+
+  /** \brief constructor. */
+  explicit PointCloudConcatenationComponent(const rclcpp::NodeOptions & node_options);
+
+  /** \brief constructor.
+   * \param queue_size the maximum queue size
+   */
+  PointCloudConcatenationComponent(const rclcpp::NodeOptions & node_options, int queue_size);
+
+  /** \brief Empty destructor. */
+  virtual ~PointCloudConcatenationComponent() {}
+
+private:
+  /** \brief The output PointCloud publisher. */
+  rclcpp::Publisher<PointCloud2>::SharedPtr pub_output_;
+  /** \brief Delay Compensated PointCloud publisher*/
+  std::map<std::string, rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr>
+    transformed_raw_pc_publisher_map_;
+
+  /** \brief The maximum number of messages that we can store in the queue. */
+  int maximum_queue_size_ = 3;
+
+  double timeout_sec_ = 0.1;
+
+  std::set<std::string> not_subscribed_topic_names_;
+
+  /** \brief A vector of subscriber. */
+  std::vector<rclcpp::Subscription<PointCloud2>::SharedPtr> filters_;
+
+  rclcpp::Subscription<autoware_auto_vehicle_msgs::msg::VelocityReport>::SharedPtr sub_twist_;
+
+  rclcpp::TimerBase::SharedPtr timer_;
+  diagnostic_updater::Updater updater_{this};
+
+  /** \brief Output TF frame the concatenated points should be transformed to. */
+  std::string output_frame_;
+
+  /** \brief Input point cloud topics. */
+  // XmlRpc::XmlRpcValue input_topics_;
+  std::vector<std::string> input_topics_;
+
+  /** \brief TF listener object. */
+  std::shared_ptr<tf2_ros::Buffer> tf2_buffer_;
+  std::shared_ptr<tf2_ros::TransformListener> tf2_listener_;
+
+  std::deque<geometry_msgs::msg::TwistStamped::ConstSharedPtr> twist_ptr_queue_;
+
+  std::map<std::string, sensor_msgs::msg::PointCloud2::ConstSharedPtr> cloud_stdmap_;
+  std::map<std::string, sensor_msgs::msg::PointCloud2::ConstSharedPtr> cloud_stdmap_tmp_;
+  std::mutex mutex_;
+
+  std::vector<double> input_offset_;
+  std::map<std::string, double> offset_map_;
+
+  void transformPointCloud(const PointCloud2::ConstSharedPtr & in, PointCloud2::SharedPtr & out);
+  void transformPointCloud(
+    const PointCloud2::ConstSharedPtr & in, PointCloud2::SharedPtr & out,
+    const std::string & target_frame);
+  void checkSyncStatus();
+  void combineClouds(sensor_msgs::msg::PointCloud2::SharedPtr & concat_cloud_ptr);
+  void publish();
+
+  void convertToXYZICloud(
+    const sensor_msgs::msg::PointCloud2::SharedPtr & input_ptr,
+    sensor_msgs::msg::PointCloud2::SharedPtr & output_ptr);
+  void setPeriod(const int64_t new_period);
+  void cloud_callback(
+    const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_ptr,
+    const std::string & topic_name);
+  void timer_callback();
+
+  void checkConcatStatus(diagnostic_updater::DiagnosticStatusWrapper & stat);
+
+  /** \brief processing time publisher. **/
+  std::unique_ptr<tier4_autoware_utils::StopWatch<std::chrono::milliseconds>> stop_watch_ptr_;
+  std::unique_ptr<tier4_autoware_utils::DebugPublisher> debug_publisher_;
+};
+
+}  // namespace pointcloud_preprocessor
+
+#endif  // POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__CONCATENATE_POINTCLOUDS_HPP_