feat(image_projection_based_fusion): substitute message filter with custom synchronizer

perception/image_projection_based_fusion/config/roi_sync.param.yaml

@@ -0,0 +1,5 @@
+/**:
+  ros__parameters:
+    input_offset_ms: [61.67, 111.67, 45.0, 28.33, 78.33, 95.0]
+    timeout_ms: 70.0
+    match_threshold_ms: 50.0

perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp

@@ -17,6 +17,8 @@
 
 #include <image_projection_based_fusion/debugger.hpp>
 #include <rclcpp/rclcpp.hpp>
+#include <tier4_autoware_utils/ros/debug_publisher.hpp>
+#include <tier4_autoware_utils/system/stop_watch.hpp>
 
 #include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
 #include <sensor_msgs/msg/camera_info.hpp>
@@ -32,40 +34,46 @@
 
 #include <map>
 #include <memory>
+#include <mutex>
+#include <set>
 #include <string>
+#include <utility>
 #include <vector>
 
 namespace image_projection_based_fusion
 {
 using autoware_auto_perception_msgs::msg::DetectedObject;
 using autoware_auto_perception_msgs::msg::DetectedObjects;
+using sensor_msgs::msg::PointCloud2;
 using tier4_perception_msgs::msg::DetectedObjectsWithFeature;
 using tier4_perception_msgs::msg::DetectedObjectWithFeature;
 
 template <class Msg, class ObjType>
 class FusionNode : public rclcpp::Node
 {
 public:
+  /** \brief constructor. */
   explicit FusionNode(const std::string & node_name, const rclcpp::NodeOptions & options);
+  /** \brief constructor.
+   * \param queue_size the maximum queue size
+   */
+  explicit FusionNode(
+    const std::string & node_name, const rclcpp::NodeOptions & options, int queue_size);
 
 protected:
   void cameraInfoCallback(
     const sensor_msgs::msg::CameraInfo::ConstSharedPtr input_camera_info_msg,
     const std::size_t camera_id);
 
-  void fusionCallback(
-    typename Msg::ConstSharedPtr input_msg,
-    DetectedObjectsWithFeature::ConstSharedPtr input_roi0_msg,
-    DetectedObjectsWithFeature::ConstSharedPtr input_roi1_msg,
-    DetectedObjectsWithFeature::ConstSharedPtr input_roi2_msg,
-    DetectedObjectsWithFeature::ConstSharedPtr input_roi3_msg,
-    DetectedObjectsWithFeature::ConstSharedPtr input_roi4_msg,
-    DetectedObjectsWithFeature::ConstSharedPtr input_roi5_msg,
-    DetectedObjectsWithFeature::ConstSharedPtr input_roi6_msg,
-    DetectedObjectsWithFeature::ConstSharedPtr input_roi7_msg);
-
   virtual void preprocess(Msg & output_msg);
 
+  // callback for Msg subscription
+  virtual void subCallback(const typename Msg::ConstSharedPtr input_msg);
+
+  // callback for roi subscription
+  virtual void roiCallback(
+    const DetectedObjectsWithFeature::ConstSharedPtr input_roi_msg, const std::size_t roi_i);
+
   virtual void fuseOnSingleImage(
     const Msg & input_msg, const std::size_t image_id,
     const DetectedObjectsWithFeature & input_roi_msg,
@@ -76,6 +84,9 @@ class FusionNode : public rclcpp::Node
 
   void publish(const Msg & output_msg);
 
+  void timer_callback();
+  void setPeriod(const int64_t new_period);
+
   std::size_t rois_number_{1};
   tf2_ros::Buffer tf_buffer_;
   tf2_ros::TransformListener tf_listener_;
@@ -84,22 +95,25 @@ class FusionNode : public rclcpp::Node
   std::map<std::size_t, sensor_msgs::msg::CameraInfo> camera_info_map_;
   std::vector<rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr> camera_info_subs_;
 
-  // fusion
-  typename message_filters::Subscriber<Msg> sub_;
-  message_filters::PassThrough<DetectedObjectsWithFeature> passthrough_;
-  std::vector<std::shared_ptr<message_filters::Subscriber<DetectedObjectsWithFeature>>> rois_subs_;
-  inline void dummyCallback(DetectedObjectsWithFeature::ConstSharedPtr input)
-  {
-    passthrough_.add(input);
-  }
-  using SyncPolicy = message_filters::sync_policies::ApproximateTime<
-    Msg, DetectedObjectsWithFeature, DetectedObjectsWithFeature, DetectedObjectsWithFeature,
-    DetectedObjectsWithFeature, DetectedObjectsWithFeature, DetectedObjectsWithFeature,
-    DetectedObjectsWithFeature, DetectedObjectsWithFeature>;
-  using Sync = message_filters::Synchronizer<SyncPolicy>;
-  typename std::shared_ptr<Sync> sync_ptr_;
-
-  // output
+  rclcpp::TimerBase::SharedPtr timer_;
+  double timeout_ms_{};
+  double match_threshold_ms_{};
+
+  /** \brief A vector of subscriber. */
+  typename rclcpp::Subscription<Msg>::SharedPtr sub_;
+  std::vector<rclcpp::Subscription<DetectedObjectsWithFeature>::SharedPtr> rois_subs_;
+
+  /** \brief Input point cloud topics. */
+  std::vector<std::string> input_topics_;
+  std::vector<double> input_offset_ms_;
+
+  // cache for fusion
+  std::vector<bool> is_fused_;
+  std::pair<int64_t, typename Msg::SharedPtr> sub_stdpair_;
+  std::vector<std::map<int64_t, DetectedObjectsWithFeature::ConstSharedPtr>> roi_stdmap_;
+  std::mutex mutex_;
+
+  // output publisher
   typename rclcpp::Publisher<Msg>::SharedPtr pub_ptr_;
 
   // debugger
@@ -111,6 +125,10 @@ class FusionNode : public rclcpp::Node
   float filter_scope_maxy_;
   float filter_scope_minz_;
   float filter_scope_maxz_;
+
+  /** \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 image_projection_based_fusion

perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp

@@ -38,7 +38,7 @@ class PointPaintingFusionNode : public FusionNode<sensor_msgs::msg::PointCloud2,
   explicit PointPaintingFusionNode(const rclcpp::NodeOptions & options);
 
 protected:
-  void preprocess(sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg) override;
+  void preprocess(sensor_msgs::msg::PointCloud2 & pointcloud_msg) override;
 
   void fuseOnSingleImage(
     const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const std::size_t image_id,
@@ -50,6 +50,8 @@ class PointPaintingFusionNode : public FusionNode<sensor_msgs::msg::PointCloud2,
 
   rclcpp::Publisher<DetectedObjects>::SharedPtr obj_pub_ptr_;
 
+  std::vector<double> tan_h_;  // horizontal field of view
+
   float score_threshold_{0.0};
   std::vector<std::string> class_names_;
   std::vector<double> pointcloud_range;
@@ -61,7 +63,5 @@ class PointPaintingFusionNode : public FusionNode<sensor_msgs::msg::PointCloud2,
 
   bool out_of_scope(const DetectedObjects & obj);
 };
-
 }  // namespace image_projection_based_fusion
-
 #endif  // IMAGE_PROJECTION_BASED_FUSION__POINTPAINTING_FUSION__NODE_HPP_

perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml

@@ -22,6 +22,7 @@
   <arg name="model_path" default="$(find-pkg-share image_projection_based_fusion)/data"/>
   <arg name="model_param_path" default="$(find-pkg-share image_projection_based_fusion)/config/$(var model_name).param.yaml"/>
   <arg name="class_remapper_param_path" default="$(find-pkg-share lidar_centerpoint)/config/detection_class_remapper.param.yaml"/>
+  <arg name="sync_param_path" default="$(find-pkg-share image_projection_based_fusion)/config/roi_sync.param.yaml"/>
 
   <!-- for eval variable-->
   <arg name="input_rois_number" default="$(var input/rois_number)"/>
@@ -75,6 +76,7 @@
       <param name="head_onnx_path" value="$(var model_path)/pts_backbone_neck_head_$(var model_name).onnx"/>
       <param name="head_engine_path" value="$(var model_path)/pts_backbone_neck_head_$(var model_name).engine"/>
       <param from="$(var model_param_path)"/>
+      <param from="$(var sync_param_path)"/>
       <param from="$(var class_remapper_param_path)"/>
       <param name="rois_number" value="$(var input/rois_number)"/>
 

perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml

@@ -18,6 +18,7 @@
   <arg name="input/camera_info7" default="/camera_info7"/>
   <arg name="input/clusters" default="clusters"/>
   <arg name="output/clusters" default="labeled_clusters"/>
+  <arg name="sync_param_path" default="$(find-pkg-share image_projection_based_fusion)/config/roi_sync.param.yaml"/>
 
   <!-- for eval variable-->
   <arg name="input_rois_number" default="$(var input/rois_number)"/>
@@ -72,6 +73,7 @@
       <param name="use_iou_y" value="false"/>
       <param name="iou_threshold" value="0.35"/>
       <param name="rois_number" value="$(var input/rois_number)"/>
+      <param from="$(var sync_param_path)"/>
       <remap from="input" to="$(var input/clusters)"/>
       <remap from="output" to="$(var output/clusters)"/>
 

perception/image_projection_based_fusion/launch/roi_detected_object_fusion.launch.xml

@@ -18,6 +18,7 @@
   <arg name="input/camera_info7" default="/camera_info7"/>
   <arg name="input/objects" default="objects"/>
   <arg name="output/objects" default="fused_objects"/>
+  <arg name="sync_param_path" default="$(find-pkg-share image_projection_based_fusion)/config/roi_sync.param.yaml"/>
 
   <!-- for eval variable-->
   <arg name="input_rois_number" default="$(var input/rois_number)"/>
@@ -66,6 +67,7 @@
       <param name="use_iou_y" value="false"/>
       <param name="iou_threshold" value="0.35"/>
       <param name="rois_number" value="$(var input/rois_number)"/>
+      <param from="$(var sync_param_path)"/>
       <remap from="input" to="$(var input/objects)"/>
       <remap from="output" to="$(var output/objects)"/>