revert mutex related changes

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

Author: technolojin

perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp

@@ -76,7 +76,6 @@ struct Det2dStatus
   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 Msg3D, class Msg2D, class ExportObj>
@@ -108,26 +107,13 @@ class FusionNode : public rclcpp::Node
   // 2d detection management methods
   bool checkAllDet2dFused()
   {
-    std::lock_guard<std::mutex> lock_det2d_flags(mutex_det2d_flags_);
     for (const auto & det2d : det2d_list_) {
       if (!det2d.is_fused) {
         return false;
       }
     }
     return true;
   }
-  void setDet2dFused(Det2dStatus<Msg2D> & det2d)
-  {
-    std::lock_guard<std::mutex> lock_det2d_flags(mutex_det2d_flags_);
-    det2d.is_fused = true;
-  }
-  void clearAllDet2dFlags()
-  {
-    std::lock_guard<std::mutex> lock_det2d_flags(mutex_det2d_flags_);
-    for (auto & det2d : det2d_list_) {
-      det2d.is_fused = false;
-    }
-  }
 
   // Custom process methods
   virtual void preprocess(Msg3D & output_msg);
@@ -144,7 +130,6 @@ class FusionNode : public rclcpp::Node
 
   // 2d detection management
   std::vector<Det2dStatus<Msg2D>> det2d_list_;
-  std::mutex mutex_det2d_flags_;
 
   // camera projection
   float approx_grid_cell_w_size_;
@@ -163,7 +148,7 @@ class FusionNode : public rclcpp::Node
   // cache for fusion
   int64_t cached_det3d_msg_timestamp_;
   typename Msg3D::SharedPtr cached_det3d_msg_ptr_;
-  std::mutex mutex_det3d_msg_;
+  std::mutex mutex_cached_msgs_;
 
   // parameters for out_of_scope filter
   float filter_scope_min_x_;

perception/autoware_image_projection_based_fusion/src/fusion_node.cpp

@@ -148,7 +148,6 @@ FusionNode<Msg3D, Msg2D, ExportObj>::FusionNode(
   // 2d detection status initialization
   det2d_list_.resize(rois_number_);
   for (std::size_t roi_i = 0; roi_i < rois_number_; ++roi_i) {
-    det2d_list_.at(roi_i).mtx_ptr = std::make_unique<std::mutex>();
     det2d_list_.at(roi_i).id = roi_i;
     det2d_list_.at(roi_i).project_to_unrectified_image =
       point_project_to_unrectified_image.at(roi_i);
@@ -261,13 +260,16 @@ void FusionNode<Msg3D, Msg2D, ExportObj>::subCallback(
     // PROCESS: if the main message is remained (and roi is not collected all) publish the main
     // message may processed partially with arrived 2d rois
     stop_watch_ptr_->toc("processing_time", true);
-    std::lock_guard<std::mutex> lock_det3d(mutex_det3d_msg_);
     exportProcess();
 
     // reset flags
-    clearAllDet2dFlags();
+    for (auto & det2d : det2d_list_) {
+      det2d.is_fused = false;
+    }
   }
 
+  std::lock_guard<std::mutex> lock(mutex_cached_msgs_);
+
   // TIMING: reset timer to the timeout time
   auto period = std::chrono::duration_cast<std::chrono::nanoseconds>(
     std::chrono::duration<double, std::milli>(timeout_ms_));
@@ -291,7 +293,6 @@ void FusionNode<Msg3D, Msg2D, ExportObj>::subCallback(
   // for loop for each roi
   for (auto & det2d : det2d_list_) {
     const auto roi_i = det2d.id;
-    std::lock_guard<std::mutex> lock_det2d(*(det2d.mtx_ptr));
 
     // check camera info
     if (det2d.camera_projector_ptr == nullptr) {
@@ -333,7 +334,7 @@ void FusionNode<Msg3D, Msg2D, ExportObj>::subCallback(
 
       fuseOnSingleImage(*det3d_msg, det2d, *(det2d_msgs[matched_stamp]), *output_msg);
       det2d_msgs.erase(matched_stamp);
-      setDet2dFused(det2d);
+      det2d.is_fused = true;
 
       // add timestamp interval for debug
       if (debug_publisher_) {
@@ -348,15 +349,16 @@ void FusionNode<Msg3D, Msg2D, ExportObj>::subCallback(
   }
 
   // PROCESS: check if the fused message is ready to publish
-  std::lock_guard<std::mutex> lock_det3d(mutex_det3d_msg_);
   cached_det3d_msg_timestamp_ = timestamp_nsec;
   cached_det3d_msg_ptr_ = output_msg;
   if (checkAllDet2dFused()) {
     // if all camera fused, postprocess and publish the main message
     exportProcess();
 
     // reset flags
-    clearAllDet2dFlags();
+    for (auto & det2d : det2d_list_) {
+      det2d.is_fused = false;
+    }
   } else {
     // if all of rois are not collected, publish the old Msg(if exists) and cache the
     // current Msg
@@ -374,14 +376,11 @@ void FusionNode<Msg3D, Msg2D, ExportObj>::roiCallback(
   stop_watch_ptr_->toc("processing_time", true);
 
   auto & det2d = det2d_list_.at(roi_i);
-  std::lock_guard<std::mutex> lock_det2d(*(det2d.mtx_ptr));
 
   int64_t timestamp_nsec =
     (*det2d_msg).header.stamp.sec * static_cast<int64_t>(1e9) + (*det2d_msg).header.stamp.nanosec;
   // if cached Msg exist, try to match
   if (cached_det3d_msg_ptr_ != nullptr) {
-    std::lock_guard<std::mutex> lock_det3d(mutex_det3d_msg_);
-
     int64_t new_stamp =
       cached_det3d_msg_timestamp_ + det2d.input_offset_ms * static_cast<int64_t>(1e6);
     int64_t interval = abs(timestamp_nsec - new_stamp);
@@ -401,7 +400,7 @@ void FusionNode<Msg3D, Msg2D, ExportObj>::roiCallback(
       }
       // PROCESS: fuse the main message with the roi message
       fuseOnSingleImage(*(cached_det3d_msg_ptr_), det2d, *det2d_msg, *(cached_det3d_msg_ptr_));
-      setDet2dFused(det2d);
+      det2d.is_fused = true;
 
       if (debug_publisher_) {
         double timestamp_interval_ms = (timestamp_nsec - cached_det3d_msg_timestamp_) / 1e6;
@@ -416,7 +415,9 @@ void FusionNode<Msg3D, Msg2D, ExportObj>::roiCallback(
       if (checkAllDet2dFused()) {
         exportProcess();
         // reset flags
-        clearAllDet2dFlags();
+        for (auto & det2d : det2d_list_) {
+          det2d.is_fused = false;
+        }
       }
       processing_time_ms = processing_time_ms + stop_watch_ptr_->toc("processing_time", true);
       return;
@@ -440,17 +441,19 @@ void FusionNode<Msg3D, Msg2D, ExportObj>::timer_callback()
 
   using std::chrono_literals::operator""ms;
   timer_->cancel();
-  if (mutex_det3d_msg_.try_lock()) {
+  if (mutex_cached_msgs_.try_lock()) {
     // PROCESS: if timeout, postprocess cached msg
     if (cached_det3d_msg_ptr_ != nullptr) {
       stop_watch_ptr_->toc("processing_time", true);
       exportProcess();
     }
 
     // reset flags whether the message is fused or not
-    clearAllDet2dFlags();
+    for (auto & det2d : det2d_list_) {
+      det2d.is_fused = false;
+    }
 
-    mutex_det3d_msg_.unlock();
+    mutex_cached_msgs_.unlock();
   } else {
     // TIMING: retry the process after 10ms
     try {