From 36a57fba1e33547692d392f3c83e873d6b1f1b94 Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Thu, 26 Dec 2024 13:30:33 +0900
Subject: [PATCH] Refactor fusion_node.hpp and fusion_node.cpp for improved
 code organization and readability

---
 .../fusion_node.hpp                           | 42 ++++++++++++-------
 .../src/fusion_node.cpp                       | 26 ++++++------
 2 files changed, 40 insertions(+), 28 deletions(-)

diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp
index a73ebd459139d..04e3066f812c0 100644
--- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp
+++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp
@@ -62,34 +62,21 @@ using autoware_perception_msgs::msg::ObjectClassification;
 template <class Msg2D>
 struct Det2dManager
 {
+  // camera index
   std::size_t id = 0;
   // camera projection
   std::unique_ptr<CameraProjection> camera_projector_ptr;
   bool project_to_unrectified_image = false;
   bool approximate_camera_projection = false;
-
   // process flags
   bool is_fused = false;
-
   // timing
   double input_offset_ms = 0.0;
-
   // cache
   std::map<int64_t, typename Msg2D::ConstSharedPtr> cached_det2d_msgs;
   std::unique_ptr<std::mutex> mtx_ptr;
 };
 
-template <class Msg2D>
-bool checkAllDet2dFused(const std::vector<Det2dManager<Msg2D>> & det2d_list)
-{
-  for (const auto & det2d : det2d_list) {
-    if (!det2d.is_fused) {
-      return false;
-    }
-  }
-  return true;
-}
-
 template <class TargetMsg3D, class ObjType, class Msg2D>
 class FusionNode : public rclcpp::Node
 {
@@ -116,6 +103,30 @@ class FusionNode : public rclcpp::Node
   void setPeriod(const int64_t new_period);
   void exportProcess();
 
+  // 2d detection management methods
+  bool checkAllDet2dFused()
+  {
+    std::lock_guard<std::mutex> lock(mutex_det2d_flags_);
+    for (const auto & det2d : det2d_list_) {
+      if (!det2d.is_fused) {
+        return false;
+      }
+    }
+    return true;
+  }
+  void setDet2dFused(Det2dManager<Msg2D> & det2d)
+  {
+    std::lock_guard<std::mutex> lock(mutex_det2d_flags_);
+    det2d.is_fused = true;
+  }
+  void clearAllDet2dFlags()
+  {
+    std::lock_guard<std::mutex> lock(mutex_det2d_flags_);
+    for (auto & det2d : det2d_list_) {
+      det2d.is_fused = false;
+    }
+  }
+
   // Custom process methods
   virtual void preprocess(TargetMsg3D & output_msg);
   virtual void fuseOnSingleImage(
@@ -129,8 +140,9 @@ class FusionNode : public rclcpp::Node
   tf2_ros::Buffer tf_buffer_;
   tf2_ros::TransformListener tf_listener_;
 
-  // camera_info
+  // 2d detection management
   std::vector<Det2dManager<Msg2D>> det2d_list_;
+  std::mutex mutex_det2d_flags_;
 
   // camera projection
   float approx_grid_cell_w_size_;
diff --git a/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp b/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp
index 93e7e60e22d3f..620fe1a48a873 100644
--- a/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp
+++ b/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp
@@ -251,11 +251,6 @@ void FusionNode<TargetMsg3D, Obj, Msg2D>::exportProcess()
       "debug/pipeline_latency_ms", pipeline_latency_ms);
     processing_time_ms = 0;
   }
-
-  // reset flags
-  for (auto & det2d : det2d_list_) {
-    det2d.is_fused = false;
-  }
   cached_det3d_msg_ptr_ = nullptr;
 }
 
@@ -272,6 +267,9 @@ void FusionNode<TargetMsg3D, Obj, Msg2D>::subCallback(
     stop_watch_ptr_->toc("processing_time", true);
     std::lock_guard<std::mutex> lock(mutex_det3d_msg_);
     exportProcess();
+
+    // reset flags
+    clearAllDet2dFlags();
   }
 
   // TIMING: reset timer to the timeout time
@@ -339,7 +337,7 @@ void FusionNode<TargetMsg3D, Obj, Msg2D>::subCallback(
 
       fuseOnSingleImage(*det3d_msg, roi_i, *(roi_msgs[matched_stamp]), *output_msg);
       roi_msgs.erase(matched_stamp);
-      det2d.is_fused = true;
+      setDet2dFused(det2d);
 
       // add timestamp interval for debug
       if (debug_publisher_) {
@@ -357,9 +355,12 @@ void FusionNode<TargetMsg3D, Obj, Msg2D>::subCallback(
   std::lock_guard<std::mutex> lock(mutex_det3d_msg_);
   cached_det3d_msg_timestamp_ = timestamp_nsec;
   cached_det3d_msg_ptr_ = output_msg;
-  if (checkAllDet2dFused(det2d_list_)) {
+  if (checkAllDet2dFused()) {
     // if all camera fused, postprocess and publish the main message
     exportProcess();
+
+    // reset flags
+    clearAllDet2dFlags();
   } else {
     // if all of rois are not collected, publish the old Msg(if exists) and cache the
     // current Msg
@@ -404,7 +405,7 @@ void FusionNode<TargetMsg3D, Obj, Msg2D>::roiCallback(
       }
       // PROCESS: fuse the main message with the roi message
       fuseOnSingleImage(*(cached_det3d_msg_ptr_), roi_i, *det2d_msg, *(cached_det3d_msg_ptr_));
-      det2d.is_fused = true;
+      setDet2dFused(det2d);
 
       if (debug_publisher_) {
         double timestamp_interval_ms = (timestamp_nsec - cached_det3d_msg_timestamp_) / 1e6;
@@ -416,8 +417,10 @@ void FusionNode<TargetMsg3D, Obj, Msg2D>::roiCallback(
       }
 
       // PROCESS: if all camera fused, postprocess and publish the main message
-      if (checkAllDet2dFused(det2d_list_)) {
+      if (checkAllDet2dFused()) {
         exportProcess();
+        // reset flags
+        clearAllDet2dFlags();
       }
       processing_time_ms = processing_time_ms + stop_watch_ptr_->toc("processing_time", true);
       return;
@@ -450,10 +453,7 @@ void FusionNode<TargetMsg3D, Obj, Msg2D>::timer_callback()
     }
 
     // reset flags whether the message is fused or not
-    for (auto & det2d : det2d_list_) {
-      det2d.is_fused = false;
-    }
-    cached_det3d_msg_ptr_ = nullptr;
+    clearAllDet2dFlags();
 
     mutex_det3d_msg_.unlock();
   } else {