feat: implement thread-safe image saving with mutex and update save logic

orbbec_camera/include/orbbec_camera/ob_camera_node.h

@@ -809,6 +809,7 @@ class OBCameraNode {
   std::unique_ptr<D2CViewer> d2c_viewer_ = nullptr;
   std::map<stream_index_pair, std::atomic_bool> save_images_;
   std::map<stream_index_pair, int> save_images_count_;
+  std::mutex save_images_mutex_;
   int max_save_images_count_ = 10;
   std::atomic_bool save_point_cloud_{false};
   std::atomic_bool save_colored_point_cloud_{false};

orbbec_camera/src/ob_camera_node.cpp

@@ -6995,18 +6995,34 @@ void OBCameraNode::saveImageToFile(const stream_index_pair &stream_index, const
                                    const cv::Mat &image_to_save,
                                    const sensor_msgs::msg::Image &image_msg,
                                    const std::shared_ptr<ob::Frame> &frame) {
-  if (save_images_[stream_index]) {
+  if (save_images_[stream_index].load(std::memory_order_acquire)) {
+    int index = 0;
+    {
+      std::lock_guard<std::mutex> lock(save_images_mutex_);
+      if (!save_images_[stream_index].load(std::memory_order_relaxed)) {
+        return;
+      }
+      index = save_images_count_[stream_index]++;
+      if (save_images_count_[stream_index] >= max_save_images_count_) {
+        save_images_[stream_index].store(false, std::memory_order_release);
+      }
+    }
+
     auto now = std::chrono::system_clock::now();
     auto in_time_t = std::chrono::system_clock::to_time_t(now);
     auto us =
         std::chrono::duration_cast<std::chrono::microseconds>(now.time_since_epoch()) % 1000000;
 
+    std::tm local_time{};
+    if (localtime_r(&in_time_t, &local_time) == nullptr) {
+      RCLCPP_ERROR_STREAM(logger_, "Failed to convert image save timestamp to local time");
+      return;
+    }
     std::stringstream ss;
-    ss << std::put_time(std::localtime(&in_time_t), "%Y%m%d_%H%M%S");
+    ss << std::put_time(&local_time, "%Y%m%d_%H%M%S");
     ss << "_" << std::setw(6) << std::setfill('0') << us.count();
     const auto output_directory = std::filesystem::current_path() / "image";
     auto fps = fps_[stream_index];
-    const int index = save_images_count_[stream_index];
     const std::string file_name = stream_name_[stream_index] + "_" +
                                   std::to_string(image_msg.width) + "x" +
                                   std::to_string(image_msg.height) + "_" + std::to_string(fps) +
@@ -7091,10 +7107,6 @@ void OBCameraNode::saveImageToFile(const stream_index_pair &stream_index, const
       }
       metadata_ofs.close();
     }
-
-    if (++save_images_count_[stream_index] >= max_save_images_count_) {
-      save_images_[stream_index] = false;
-    }
   }
 }
 

orbbec_camera/src/ros_service.cpp

@@ -1961,10 +1961,11 @@ void OBCameraNode::saveImageCallback(const std::shared_ptr<std_srvs::srv::Empty:
                                      std::shared_ptr<std_srvs::srv::Empty::Response>& response) {
   (void)request;
   (void)response;
+  std::lock_guard<std::mutex> lock(save_images_mutex_);
   for (const auto& stream_index : IMAGE_STREAMS) {
     if (enable_stream_[stream_index]) {
-      save_images_[stream_index] = true;
       save_images_count_[stream_index] = 0;
+      save_images_[stream_index].store(true, std::memory_order_release);
     }
   }
 }