refactor(image_projection_based_fusion): rename cache-related variables' name (#5709)

* refactor(image_projection_based_fusion): rename sub_std_map_ to cached_msg_

Signed-off-by: kminoda <koji.minoda@tier4.jp>

* style(pre-commit): autofix

* rename other cache-related variables

Signed-off-by: kminoda <koji.minoda@tier4.jp>

* style(pre-commit): autofix

---------

Signed-off-by: kminoda <koji.minoda@tier4.jp>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

Author: kminoda

perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp

@@ -118,9 +118,10 @@ class FusionNode : public rclcpp::Node
 
   // cache for fusion
   std::vector<bool> is_fused_;
-  std::pair<int64_t, typename Msg::SharedPtr> sub_std_pair_;
-  std::vector<std::map<int64_t, DetectedObjectsWithFeature::ConstSharedPtr>> roi_stdmap_;
-  std::mutex mutex_;
+  std::pair<int64_t, typename Msg::SharedPtr>
+    cached_msg_;  // first element is the timestamp in nanoseconds, second element is the message
+  std::vector<std::map<int64_t, DetectedObjectsWithFeature::ConstSharedPtr>> cached_roi_msgs_;
+  std::mutex mutex_cached_msgs_;
 
   // output publisher
   typename rclcpp::Publisher<Msg>::SharedPtr pub_ptr_;

perception/image_projection_based_fusion/src/fusion_node.cpp

@@ -96,7 +96,7 @@ FusionNode<Msg, ObjType>::FusionNode(
 
   // sub rois
   rois_subs_.resize(rois_number_);
-  roi_stdmap_.resize(rois_number_);
+  cached_roi_msgs_.resize(rois_number_);
   is_fused_.resize(rois_number_, false);
   for (std::size_t roi_i = 0; roi_i < rois_number_; ++roi_i) {
     std::function<void(const DetectedObjectsWithFeature::ConstSharedPtr msg)> roi_callback =
@@ -163,12 +163,12 @@ void FusionNode<Msg, Obj>::preprocess(Msg & ouput_msg __attribute__((unused)))
 template <class Msg, class Obj>
 void FusionNode<Msg, Obj>::subCallback(const typename Msg::ConstSharedPtr input_msg)
 {
-  if (sub_std_pair_.second != nullptr) {
+  if (cached_msg_.second != nullptr) {
     stop_watch_ptr_->toc("processing_time", true);
     timer_->cancel();
-    postprocess(*(sub_std_pair_.second));
-    publish(*(sub_std_pair_.second));
-    sub_std_pair_.second = nullptr;
+    postprocess(*(cached_msg_.second));
+    publish(*(cached_msg_.second));
+    cached_msg_.second = nullptr;
     std::fill(is_fused_.begin(), is_fused_.end(), false);
 
     // add processing time for debug
@@ -183,7 +183,7 @@ void FusionNode<Msg, Obj>::subCallback(const typename Msg::ConstSharedPtr input_
     }
   }
 
-  std::lock_guard<std::mutex> lock(mutex_);
+  std::lock_guard<std::mutex> lock(mutex_cached_msgs_);
   auto period = std::chrono::duration_cast<std::chrono::nanoseconds>(
     std::chrono::duration<double, std::milli>(timeout_ms_));
   try {
@@ -211,12 +211,12 @@ void FusionNode<Msg, Obj>::subCallback(const typename Msg::ConstSharedPtr input_
       continue;
     }
 
-    if ((roi_stdmap_.at(roi_i)).size() > 0) {
+    if ((cached_roi_msgs_.at(roi_i)).size() > 0) {
       int64_t min_interval = 1e9;
       int64_t matched_stamp = -1;
       std::list<int64_t> outdate_stamps;
 
-      for (const auto & [k, v] : roi_stdmap_.at(roi_i)) {
+      for (const auto & [k, v] : cached_roi_msgs_.at(roi_i)) {
         int64_t new_stamp = timestamp_nsec + input_offset_ms_.at(roi_i) * (int64_t)1e6;
         int64_t interval = abs(int64_t(k) - new_stamp);
 
@@ -230,7 +230,7 @@ void FusionNode<Msg, Obj>::subCallback(const typename Msg::ConstSharedPtr input_
 
       // remove outdated stamps
       for (auto stamp : outdate_stamps) {
-        (roi_stdmap_.at(roi_i)).erase(stamp);
+        (cached_roi_msgs_.at(roi_i)).erase(stamp);
       }
 
       // fuseOnSingle
@@ -240,9 +240,9 @@ void FusionNode<Msg, Obj>::subCallback(const typename Msg::ConstSharedPtr input_
         }
 
         fuseOnSingleImage(
-          *input_msg, roi_i, *((roi_stdmap_.at(roi_i))[matched_stamp]), camera_info_map_.at(roi_i),
-          *output_msg);
-        (roi_stdmap_.at(roi_i)).erase(matched_stamp);
+          *input_msg, roi_i, *((cached_roi_msgs_.at(roi_i))[matched_stamp]),
+          camera_info_map_.at(roi_i), *output_msg);
+        (cached_roi_msgs_.at(roi_i)).erase(matched_stamp);
         is_fused_.at(roi_i) = true;
 
         // add timestamp interval for debug
@@ -265,7 +265,7 @@ void FusionNode<Msg, Obj>::subCallback(const typename Msg::ConstSharedPtr input_
     postprocess(*output_msg);
     publish(*output_msg);
     std::fill(is_fused_.begin(), is_fused_.end(), false);
-    sub_std_pair_.second = nullptr;
+    cached_msg_.second = nullptr;
 
     // add processing time for debug
     if (debug_publisher_) {
@@ -278,8 +278,8 @@ void FusionNode<Msg, Obj>::subCallback(const typename Msg::ConstSharedPtr input_
       processing_time_ms = 0;
     }
   } else {
-    sub_std_pair_.first = int64_t(timestamp_nsec);
-    sub_std_pair_.second = output_msg;
+    cached_msg_.first = int64_t(timestamp_nsec);
+    cached_msg_.second = output_msg;
     processing_time_ms = stop_watch_ptr_->toc("processing_time", true);
   }
 }
@@ -294,28 +294,28 @@ void FusionNode<Msg, Obj>::roiCallback(
     (*input_roi_msg).header.stamp.sec * (int64_t)1e9 + (*input_roi_msg).header.stamp.nanosec;
 
   // if cached Msg exist, try to match
-  if (sub_std_pair_.second != nullptr) {
-    int64_t new_stamp = sub_std_pair_.first + input_offset_ms_.at(roi_i) * (int64_t)1e6;
+  if (cached_msg_.second != nullptr) {
+    int64_t new_stamp = cached_msg_.first + input_offset_ms_.at(roi_i) * (int64_t)1e6;
     int64_t interval = abs(timestamp_nsec - new_stamp);
 
     if (interval < match_threshold_ms_ * (int64_t)1e6 && is_fused_.at(roi_i) == false) {
       if (camera_info_map_.find(roi_i) == camera_info_map_.end()) {
         RCLCPP_WARN_THROTTLE(
           this->get_logger(), *this->get_clock(), 5000, "no camera info. id is %zu", roi_i);
-        (roi_stdmap_.at(roi_i))[timestamp_nsec] = input_roi_msg;
+        (cached_roi_msgs_.at(roi_i))[timestamp_nsec] = input_roi_msg;
         return;
       }
       if (debugger_) {
         debugger_->clear();
       }
 
       fuseOnSingleImage(
-        *(sub_std_pair_.second), roi_i, *input_roi_msg, camera_info_map_.at(roi_i),
-        *(sub_std_pair_.second));
+        *(cached_msg_.second), roi_i, *input_roi_msg, camera_info_map_.at(roi_i),
+        *(cached_msg_.second));
       is_fused_.at(roi_i) = true;
 
       if (debug_publisher_) {
-        double timestamp_interval_ms = (timestamp_nsec - sub_std_pair_.first) / 1e6;
+        double timestamp_interval_ms = (timestamp_nsec - cached_msg_.first) / 1e6;
         debug_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
           "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_ms", timestamp_interval_ms);
         debug_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
@@ -325,10 +325,10 @@ void FusionNode<Msg, Obj>::roiCallback(
 
       if (std::count(is_fused_.begin(), is_fused_.end(), true) == static_cast<int>(rois_number_)) {
         timer_->cancel();
-        postprocess(*(sub_std_pair_.second));
-        publish(*(sub_std_pair_.second));
+        postprocess(*(cached_msg_.second));
+        publish(*(cached_msg_.second));
         std::fill(is_fused_.begin(), is_fused_.end(), false);
-        sub_std_pair_.second = nullptr;
+        cached_msg_.second = nullptr;
 
         // add processing time for debug
         if (debug_publisher_) {
@@ -346,7 +346,7 @@ void FusionNode<Msg, Obj>::roiCallback(
     }
   }
   // store roi msg if not matched
-  (roi_stdmap_.at(roi_i))[timestamp_nsec] = input_roi_msg;
+  (cached_roi_msgs_.at(roi_i))[timestamp_nsec] = input_roi_msg;
 }
 
 template <class Msg, class Obj>
@@ -360,13 +360,13 @@ void FusionNode<Msg, Obj>::timer_callback()
 {
   using std::chrono_literals::operator""ms;
   timer_->cancel();
-  if (mutex_.try_lock()) {
+  if (mutex_cached_msgs_.try_lock()) {
     // timeout, postprocess cached msg
-    if (sub_std_pair_.second != nullptr) {
+    if (cached_msg_.second != nullptr) {
       stop_watch_ptr_->toc("processing_time", true);
 
-      postprocess(*(sub_std_pair_.second));
-      publish(*(sub_std_pair_.second));
+      postprocess(*(cached_msg_.second));
+      publish(*(cached_msg_.second));
 
       // add processing time for debug
       if (debug_publisher_) {
@@ -380,9 +380,9 @@ void FusionNode<Msg, Obj>::timer_callback()
       }
     }
     std::fill(is_fused_.begin(), is_fused_.end(), false);
-    sub_std_pair_.second = nullptr;
+    cached_msg_.second = nullptr;
 
-    mutex_.unlock();
+    mutex_cached_msgs_.unlock();
   } else {
     try {
       std::chrono::nanoseconds period = 10ms;