diff --git a/orbbec_camera/CMakeLists.txt b/orbbec_camera/CMakeLists.txt
index 79c53699..2d5ad886 100644
--- a/orbbec_camera/CMakeLists.txt
+++ b/orbbec_camera/CMakeLists.txt
@@ -142,8 +142,40 @@ if(USE_NV_HW_DECODER)
     set(TEGRA_ARMABI /usr/lib/aarch64-linux-gnu/)
     add_definitions(-DUSE_NV_HW_DECODER)
     add_compile_options(-Wno-missing-field-initializers -Wno-unused-parameter)
-    set(NV_LIBRARIES -lnvjpeg -lnvbufsurface -lnvbufsurftransform -lyuv -lv4l2)
-    list(APPEND NV_LIBRARIES -L${TEGRA_ARMABI} -L${TEGRA_ARMABI}/tegra)
+    # Search Jetson Multimedia API libraries, excluding CUDA library directories.
+    find_library(JETSON_JPEG_LIBRARY
+        NAMES nvmm_jpeg nvjpeg
+        PATHS
+            "${TEGRA_ARMABI}/nvidia"
+            "${TEGRA_ARMABI}/tegra"
+        NO_DEFAULT_PATH
+    )
+    if(NOT JETSON_JPEG_LIBRARY)
+        message(FATAL_ERROR
+            "Jetson Multimedia API JPEG library not found. "
+            "Install the Multimedia API matching the system L4T version."
+        )
+    endif()
+    message(STATUS "Jetson JPEG library: ${JETSON_JPEG_LIBRARY}")
+
+    set(NV_LIBRARIES
+        "${JETSON_JPEG_LIBRARY}"
+        -lnvbufsurface -lnvbufsurftransform -lyuv -lv4l2
+    )
+    list(APPEND NV_LIBRARIES
+        "-L${TEGRA_ARMABI}"
+        "-L${TEGRA_ARMABI}/nvidia"
+        "-L${TEGRA_ARMABI}/tegra"
+    )
+
+    # The nvmm_jpeg Multimedia API classes use the CUDA driver API.
+    if(JETSON_JPEG_LIBRARY MATCHES "/libnvmm_jpeg\\.so")
+        if(CMAKE_VERSION VERSION_LESS "3.17")
+            message(FATAL_ERROR "nvmm_jpeg support requires CMake 3.17 or newer to find CUDAToolkit.")
+        endif()
+        find_package(CUDAToolkit REQUIRED)
+        list(APPEND NV_LIBRARIES CUDA::cuda_driver)
+    endif()
 endif()
 
 set(COMMON_INCLUDE_DIRS
diff --git a/orbbec_camera/config/tools/multisavergbir/multi_save_rgbir_params.json b/orbbec_camera/config/tools/multisavergbir/multi_save_rgbir_params.json
index d557155a..dd1d3703 100644
--- a/orbbec_camera/config/tools/multisavergbir/multi_save_rgbir_params.json
+++ b/orbbec_camera/config/tools/multisavergbir/multi_save_rgbir_params.json
@@ -1,13 +1,14 @@
 {
   "save_rgbir_params": {
-      "time_domain": "global",
-      "usb_ports": [
-          "2-1",
-          "2-3"
-      ],
-      "camera_name": [
-          "camera_01",
-          "camera_02"
-      ]
+    "time_domain": "global",
+    "stream_names": [],
+    "usb_ports": [
+      "2-1",
+      "2-3"
+    ],
+    "camera_name": [
+      "camera_01",
+      "camera_02"
+    ]
   }
-}
\ No newline at end of file
+}
diff --git a/orbbec_camera/examples/multi_camera_time_sync/image_sync_example_node.cpp b/orbbec_camera/examples/multi_camera_time_sync/image_sync_example_node.cpp
index 48791ba7..eb446ad7 100755
--- a/orbbec_camera/examples/multi_camera_time_sync/image_sync_example_node.cpp
+++ b/orbbec_camera/examples/multi_camera_time_sync/image_sync_example_node.cpp
@@ -1,4 +1,5 @@
 #include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/image_encodings.hpp>
 #include <sensor_msgs/msg/image.hpp>
 
 #if __has_include(<message_filters/subscriber.hpp>)
@@ -33,6 +34,7 @@
 #include <opencv2/opencv.hpp>
 #include <string>
 #include <thread>
+#include <utility>
 #include <vector>
 
 using Image = sensor_msgs::msg::Image;
@@ -69,7 +71,7 @@ class ImageSyncNode : public rclcpp::Node {
     if (sync_topics_.empty()) {
       sync_topics_ = discover_image_topics();
       RCLCPP_INFO(this->get_logger(),
-                  "Parameter sync_topics is empty. Auto-discovered %zu color/depth image topics.",
+                  "Parameter sync_topics is empty. Auto-discovered %zu supported image topics.",
                   sync_topics_.size());
     } else {
       RCLCPP_INFO(this->get_logger(), "Using %zu image topics from parameter sync_topics.",
@@ -166,6 +168,24 @@ class ImageSyncNode : public rclcpp::Node {
            str.compare(str.size() - suffix.size(), suffix.size(), suffix) == 0;
   }
 
+  static const std::array<std::pair<const char *, const char *>, 7> &supported_stream_suffixes() {
+    static const std::array<std::pair<const char *, const char *>, 7> suffixes = {{
+        {"left_color", "/left_color/image_raw"},
+        {"right_color", "/right_color/image_raw"},
+        {"left_ir", "/left_ir/image_raw"},
+        {"right_ir", "/right_ir/image_raw"},
+        {"color", "/color/image_raw"},
+        {"depth", "/depth/image_raw"},
+        {"ir", "/ir/image_raw"},
+    }};
+    return suffixes;
+  }
+
+  static bool is_supported_image_topic(const std::string &topic) {
+    return std::any_of(supported_stream_suffixes().begin(), supported_stream_suffixes().end(),
+                       [&topic](const auto &entry) { return has_suffix(topic, entry.second); });
+  }
+
   static double stamp_to_seconds(const builtin_interfaces::msg::Time &stamp) {
     return static_cast<double>(stamp.sec) + static_cast<double>(stamp.nanosec) * 1e-9;
   }
@@ -180,7 +200,7 @@ class ImageSyncNode : public rclcpp::Node {
       const auto names_and_types = this->get_topic_names_and_types();
       for (const auto &entry : names_and_types) {
         const auto &topic = entry.first;
-        if (!has_suffix(topic, "/color/image_raw") && !has_suffix(topic, "/depth/image_raw")) {
+        if (!is_supported_image_topic(topic)) {
           continue;
         }
 
@@ -207,8 +227,8 @@ class ImageSyncNode : public rclcpp::Node {
   void validate_topics() {
     if (sync_topics_.empty()) {
       throw std::runtime_error(
-          "No image topics to synchronize. Set parameter sync_topics or start color/depth cameras "
-          "before this node.");
+          "No image topics to synchronize. Set parameter sync_topics or start supported camera "
+          "streams before this node.");
     }
 
     std::vector<std::string> deduplicated_topics;
@@ -233,7 +253,7 @@ class ImageSyncNode : public rclcpp::Node {
           "Official ROS message_filters::Synchronizer supports at most 9 inputs, and this example "
           "supports 1-8 image topics. Found " +
           std::to_string(sync_topics_.size()) +
-          " color/depth image topics. Please pass <= 8 topics with sync_topics or split the sync "
+          " image topics. Please pass <= 8 topics with sync_topics or split the sync "
           "into multiple stages.");
     }
   }
@@ -247,14 +267,13 @@ class ImageSyncNode : public rclcpp::Node {
       info.image_type = "image";
       info.camera_name = topic;
 
-      const auto color_pos = topic.rfind("/color/image_raw");
-      const auto depth_pos = topic.rfind("/depth/image_raw");
-      if (color_pos != std::string::npos) {
-        info.image_type = "color";
-        info.camera_name = topic.substr(0, color_pos);
-      } else if (depth_pos != std::string::npos) {
-        info.image_type = "depth";
-        info.camera_name = topic.substr(0, depth_pos);
+      for (const auto &entry : supported_stream_suffixes()) {
+        const std::string suffix = entry.second;
+        if (has_suffix(topic, suffix)) {
+          info.image_type = entry.first;
+          info.camera_name = topic.substr(0, topic.size() - suffix.size());
+          break;
+        }
       }
 
       const auto slash_pos = info.camera_name.find_last_of('/');
@@ -280,7 +299,17 @@ class ImageSyncNode : public rclcpp::Node {
     try {
       for (const auto &msg : msgs) {
         auto cv_image = cv_bridge::toCvShare(msg);
-        images.push_back(cv_image->image.clone());
+        cv::Mat image;
+        if (msg->encoding == sensor_msgs::image_encodings::RGB8) {
+          cv::cvtColor(cv_image->image, image, cv::COLOR_RGB2BGR);
+        } else if (msg->encoding == sensor_msgs::image_encodings::RGBA8) {
+          cv::cvtColor(cv_image->image, image, cv::COLOR_RGBA2BGR);
+        } else if (msg->encoding == sensor_msgs::image_encodings::BGRA8) {
+          cv::cvtColor(cv_image->image, image, cv::COLOR_BGRA2BGR);
+        } else {
+          image = cv_image->image.clone();
+        }
+        images.push_back(std::move(image));
         timestamps.push_back(stamp_to_seconds(msg->header.stamp));
       }
     } catch (cv_bridge::Exception &e) {
@@ -459,8 +488,14 @@ class ImageSyncNode : public rclcpp::Node {
         cv::applyColorMap(tmp, image, cv::COLORMAP_JET);
       } else if (images[i].channels() == 3) {
         image = images[i].clone();
+      } else if (images[i].channels() == 4) {
+        cv::cvtColor(images[i], image, cv::COLOR_BGRA2BGR);
       } else {
-        cv::cvtColor(images[i], image, cv::COLOR_GRAY2BGR);
+        RCLCPP_WARN(this->get_logger(), "Display first channel of unsupported %d-channel image %s",
+                    images[i].channels(), topic_infos[i].topic.c_str());
+        cv::Mat first_channel;
+        cv::extractChannel(images[i], first_channel, 0);
+        cv::cvtColor(first_channel, image, cv::COLOR_GRAY2BGR);
       }
 
       const std::string text = topic_infos[i].camera_name + " " + topic_infos[i].image_type +
@@ -550,10 +585,8 @@ class ImageSyncNode : public rclcpp::Node {
     const double avg_diff = diff_sum_ / count_;
 
     std::cout << "\nImage Timestamp Difference Statistics" << std::endl;
-    std::cout << "cur: " << cur << " ms"
-              << " avg: " << avg_diff << " ms"
-              << " max: " << max_diff_ << " ms"
-              << " min: " << min_diff_ << " ms" << std::endl;
+    std::cout << "cur: " << cur << " ms" << " avg: " << avg_diff << " ms" << " max: " << max_diff_
+              << " ms" << " min: " << min_diff_ << " ms" << std::endl;
 
     if (last_time_ == 0.0) {
       last_time_ = base_t;
diff --git a/orbbec_camera/include/orbbec_camera/constants.h b/orbbec_camera/include/orbbec_camera/constants.h
index 02a3e45c..fdd609be 100644
--- a/orbbec_camera/include/orbbec_camera/constants.h
+++ b/orbbec_camera/include/orbbec_camera/constants.h
@@ -143,15 +143,26 @@ const int32_t GEMINI_435Le_PID = 0x815;                   // Gemini 435Le
 const int32_t GEMINI_305_PID = 0x0840;                    // Gemini 305
 const int32_t GEMINI_305_PID2 = 0x0841;                   // Gemini 305
 const int32_t GEMINI_305G_PID = 0x0842;                   // Gemini 305g
+const int32_t GEMINI_301G_PID = 0x0843;                   // Gemini 301g
 const int32_t GEMINI_309G_PID = 0x0845;                   // Gemini 309g
 const int32_t GEMINI_338LG_PID = 0x081A;                  // Gemini 338Lg
 const int32_t GEMINI_338LE_PID = 0x081B;                  // Gemini 338Le
 const int32_t GEMINI_338L_PID = 0x081C;                   // Gemini 338L
 const int32_t GEMINI_331L_PID = 0x081D;                   // Gemini 331L
 
-inline bool isGemini305SeriesPID(uint32_t pid) {
+inline bool isGemini330SeriesPID(uint32_t pid) {
+  return pid == GEMINI_335_PID || pid == GEMINI_330_PID || pid == GEMINI_336_PID ||
+         pid == GEMINI_335L_PID || pid == GEMINI_330L_PID || pid == GEMINI_336L_PID ||
+         pid == GEMINI_335LG_PID || pid == GEMINI_336LG_PID || pid == GEMINI_335LE_PID ||
+         pid == GEMINI_336LE_PID || pid == CUSTOM_ADVANTECH_GEMINI_336_PID ||
+         pid == CUSTOM_ADVANTECH_GEMINI_336L_PID || pid == GEMINI_338_PID ||
+         pid == GEMINI_338LG_PID || pid == GEMINI_338LE_PID || pid == GEMINI_338L_PID ||
+         pid == GEMINI_331L_PID;
+}
+
+inline bool isGemini301SeriesPID(uint32_t pid) {
   return pid == GEMINI_305_PID || pid == GEMINI_305_PID2 || pid == GEMINI_305G_PID ||
-         pid == GEMINI_309G_PID;
+         pid == GEMINI_301G_PID || pid == GEMINI_309G_PID;
 }
 
 inline bool isGmslCameraPID(uint32_t pid) {
diff --git a/orbbec_camera/include/orbbec_camera/fps_delay_status.hpp b/orbbec_camera/include/orbbec_camera/fps_delay_status.hpp
index e85501dc..ecb86a92 100644
--- a/orbbec_camera/include/orbbec_camera/fps_delay_status.hpp
+++ b/orbbec_camera/include/orbbec_camera/fps_delay_status.hpp
@@ -20,6 +20,7 @@
 #include "orbbec_camera_msgs/msg/device_status.hpp"
 #include <chrono>
 #include <functional>
+#include <limits>
 #include <mutex>
 namespace orbbec_camera {
 class FpsDelayStatus {
@@ -58,38 +59,57 @@ class FpsDelayStatus {
   }
 
   void fillColorStatus(orbbec_camera_msgs::msg::DeviceStatus &msg) {
-    std::lock_guard<std::mutex> lock(mutex_);
-    msg.color_frame_rate_cur = last_fps_;
-    msg.color_frame_rate_avg = frame_count_ > 0 ? fps_sum_ / frame_count_ : 0;
-    msg.color_frame_rate_min = fps_min_;
-    msg.color_frame_rate_max = fps_max_;
+    fillStatus(msg.color_frame_rate_cur, msg.color_frame_rate_avg, msg.color_frame_rate_min,
+               msg.color_frame_rate_max, msg.color_delay_ms_cur, msg.color_delay_ms_avg,
+               msg.color_delay_ms_min, msg.color_delay_ms_max);
+  }
 
-    msg.color_delay_ms_cur = last_delay_ms_;
-    msg.color_delay_ms_avg = frame_count_ > 0 ? delay_sum_ / frame_count_ : 0;
-    msg.color_delay_ms_min = delay_min_;
-    msg.color_delay_ms_max = delay_max_;
+  void fillLeftColorStatus(orbbec_camera_msgs::msg::DeviceStatus &msg) {
+    fillStatus(msg.left_color_frame_rate_cur, msg.left_color_frame_rate_avg,
+               msg.left_color_frame_rate_min, msg.left_color_frame_rate_max,
+               msg.left_color_delay_ms_cur, msg.left_color_delay_ms_avg,
+               msg.left_color_delay_ms_min, msg.left_color_delay_ms_max);
+  }
 
-    last_delay_ms_ = 0.0;
-    last_fps_ = 0.0;
-    frame_count_ = 0;
-    fps_sum_ = delay_sum_ = 0.0;
-    fps_max_ = delay_max_ = 0.0;
-    fps_min_ = delay_min_ = 0.0;
+  void fillRightColorStatus(orbbec_camera_msgs::msg::DeviceStatus &msg) {
+    fillStatus(msg.right_color_frame_rate_cur, msg.right_color_frame_rate_avg,
+               msg.right_color_frame_rate_min, msg.right_color_frame_rate_max,
+               msg.right_color_delay_ms_cur, msg.right_color_delay_ms_avg,
+               msg.right_color_delay_ms_min, msg.right_color_delay_ms_max);
   }
 
   void fillDepthStatus(orbbec_camera_msgs::msg::DeviceStatus &msg) {
+    fillStatus(msg.depth_frame_rate_cur, msg.depth_frame_rate_avg, msg.depth_frame_rate_min,
+               msg.depth_frame_rate_max, msg.depth_delay_ms_cur, msg.depth_delay_ms_avg,
+               msg.depth_delay_ms_min, msg.depth_delay_ms_max);
+  }
+
+  void fillLeftIrStatus(orbbec_camera_msgs::msg::DeviceStatus &msg) {
+    fillStatus(msg.left_ir_frame_rate_cur, msg.left_ir_frame_rate_avg, msg.left_ir_frame_rate_min,
+               msg.left_ir_frame_rate_max, msg.left_ir_delay_ms_cur, msg.left_ir_delay_ms_avg,
+               msg.left_ir_delay_ms_min, msg.left_ir_delay_ms_max);
+  }
+
+  void fillRightIrStatus(orbbec_camera_msgs::msg::DeviceStatus &msg) {
+    fillStatus(msg.right_ir_frame_rate_cur, msg.right_ir_frame_rate_avg,
+               msg.right_ir_frame_rate_min, msg.right_ir_frame_rate_max, msg.right_ir_delay_ms_cur,
+               msg.right_ir_delay_ms_avg, msg.right_ir_delay_ms_min, msg.right_ir_delay_ms_max);
+  }
+
+ private:
+  void fillStatus(double &frame_rate_cur, double &frame_rate_avg, double &frame_rate_min,
+                  double &frame_rate_max, double &delay_ms_cur, double &delay_ms_avg,
+                  double &delay_ms_min, double &delay_ms_max) {
     std::lock_guard<std::mutex> lock(mutex_);
-    msg.depth_frame_rate_cur = last_fps_;
-    msg.depth_frame_rate_avg = frame_count_ > 0 ? fps_sum_ / frame_count_ : 0;
-    msg.depth_frame_rate_min = fps_min_;
-    msg.depth_frame_rate_max = fps_max_;
+    frame_rate_cur = last_fps_;
+    frame_rate_avg = frame_count_ > 0 ? fps_sum_ / frame_count_ : 0;
+    frame_rate_min = frame_count_ > 0 ? fps_min_ : 0;
+    frame_rate_max = frame_count_ > 0 ? fps_max_ : 0;
 
-    msg.depth_delay_ms_cur = last_delay_ms_;
-    msg.depth_delay_ms_avg = frame_count_ > 0 ? delay_sum_ / frame_count_ : 0;
-    msg.depth_delay_ms_min = delay_min_;
-    msg.depth_delay_ms_max = delay_max_;
-
-    // RCLCPP_ERROR_STREAM(logger_, "Depth status: " << fps_sum_ << "," << frame_count_);
+    delay_ms_cur = last_delay_ms_;
+    delay_ms_avg = frame_count_ > 0 ? delay_sum_ / frame_count_ : 0;
+    delay_ms_min = frame_count_ > 0 ? delay_min_ : 0;
+    delay_ms_max = frame_count_ > 0 ? delay_max_ : 0;
 
     last_delay_ms_ = 0.0;
     last_fps_ = 0.0;
@@ -99,7 +119,6 @@ class FpsDelayStatus {
     fps_min_ = delay_min_ = 0.0;
   }
 
- private:
   mutable std::mutex mutex_;
   u_int64_t last_stream_timestamp_{0};
   double last_delay_ms_{0.0};
diff --git a/orbbec_camera/include/orbbec_camera/frame_timestamp_csv_logger.h b/orbbec_camera/include/orbbec_camera/frame_timestamp_csv_logger.h
index e9033a20..ba486036 100644
--- a/orbbec_camera/include/orbbec_camera/frame_timestamp_csv_logger.h
+++ b/orbbec_camera/include/orbbec_camera/frame_timestamp_csv_logger.h
@@ -21,7 +21,7 @@ namespace orbbec_camera {
 
 class FrameTimestampCsvLogger {
  public:
-  enum class OutputMode { SYNCED, COLOR, DEPTH };
+  enum class OutputMode { SYNCED, COLOR, LEFT_COLOR, RIGHT_COLOR, DEPTH, LEFT_IR, RIGHT_IR };
 
   FrameTimestampCsvLogger(bool drop_log_enabled, const std::string &csv_file_path,
                           OutputMode output_mode, rclcpp::Logger logger);
diff --git a/orbbec_camera/include/orbbec_camera/jetson_nv_decoder.h b/orbbec_camera/include/orbbec_camera/jetson_nv_decoder.h
index 1b7acc25..41c72d5b 100644
--- a/orbbec_camera/include/orbbec_camera/jetson_nv_decoder.h
+++ b/orbbec_camera/include/orbbec_camera/jetson_nv_decoder.h
@@ -14,15 +14,10 @@
  * limitations under the License.
  *******************************************************************************/
 #pragma once
-#include "utils.h"
-#include <opencv2/opencv.hpp>
+
+#include <memory>
 
 #include "jpeg_decoder.h"
-#include <NvJpegDecoder.h>
-#include <NvUtils.h>
-#include <NvV4l2Element.h>
-#include <NvJpegDecoder.h>
-#include <NvV4l2Element.h>
 
 namespace orbbec_camera {
 class JetsonNvJPEGDecoder : public JPEGDecoder {
@@ -33,6 +28,7 @@ class JetsonNvJPEGDecoder : public JPEGDecoder {
   bool decode(const std::shared_ptr<ob::ColorFrame>& frame, uint8_t* dest) override;
 
  private:
-  NvJPEGDecoder* decoder_;
+  class Impl;
+  std::unique_ptr<Impl> decoder_;
 };
-}  // namespace orbbec_camera
\ No newline at end of file
+}  // namespace orbbec_camera
diff --git a/orbbec_camera/include/orbbec_camera/ob_camera_node.h b/orbbec_camera/include/orbbec_camera/ob_camera_node.h
index 08ed6ab7..abe66fde 100644
--- a/orbbec_camera/include/orbbec_camera/ob_camera_node.h
+++ b/orbbec_camera/include/orbbec_camera/ob_camera_node.h
@@ -237,10 +237,14 @@ class OBCameraNode {
   }
   void getColorStatus(orbbec_camera_msgs::msg::DeviceStatus& status_msg) {
     fps_delay_status_color_->fillColorStatus(status_msg);
+    fps_delay_status_left_color_->fillLeftColorStatus(status_msg);
+    fps_delay_status_right_color_->fillRightColorStatus(status_msg);
   }
 
   void getDepthStatus(orbbec_camera_msgs::msg::DeviceStatus& status_msg) {
     fps_delay_status_depth_->fillDepthStatus(status_msg);
+    fps_delay_status_left_ir_->fillLeftIrStatus(status_msg);
+    fps_delay_status_right_ir_->fillRightIrStatus(status_msg);
   }
 
   bool checkUserCalibrationReady() {
@@ -305,6 +309,9 @@ class OBCameraNode {
 
   void setupProfiles();
 
+  bool validate301SeriesStreamFrameRates(const std::map<stream_index_pair, int>& fps,
+                                         std::string& message) const;
+
   std::shared_ptr<ob::VideoStreamProfile> selectVideoStreamProfile(
       const stream_index_pair& stream_index, int width, int height, int fps, OBFormat format);
 
@@ -470,7 +477,7 @@ class OBCameraNode {
                               const std::shared_ptr<std_srvs::srv::SetBool::Request>& request,
                               std::shared_ptr<std_srvs::srv::SetBool::Response>& response);
 
-  void setFloorEnableCallback(const std::shared_ptr<rmw_request_id_t>& request_header,
+  void setFloodEnableCallback(const std::shared_ptr<rmw_request_id_t>& request_header,
                               const std::shared_ptr<std_srvs::srv::SetBool::Request>& request,
                               std::shared_ptr<std_srvs::srv::SetBool::Response>& response);
 
@@ -666,8 +673,6 @@ class OBCameraNode {
 
   orbbec_camera_msgs::msg::IMUInfo createIMUInfo(const stream_index_pair& stream_index);
 
-  static bool isGemini335PID(uint32_t pid);
-
   static bool isGemini435LePID(uint32_t pid);
   static bool isPublishMetaData(uint32_t pid);
   static bool isDabaiASeriesForHwD2C(uint32_t pid);
@@ -804,7 +809,7 @@ class OBCameraNode {
   rclcpp::Service<orbbec_camera_msgs::srv::GetBool>::SharedPtr get_laser_status_srv_;
   rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr set_ptp_config_srv_;
   rclcpp::Service<orbbec_camera_msgs::srv::GetBool>::SharedPtr get_ptp_config_srv_;
-  rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr set_floor_enable_srv_;
+  rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr set_flood_enable_srv_;
   rclcpp::Service<SetInt32>::SharedPtr set_fan_work_mode_srv_;
   rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr toggle_sensors_srv_;
   rclcpp::Service<GetInt32>::SharedPtr get_lrm_measure_distance_srv_;
@@ -1188,12 +1193,18 @@ class OBCameraNode {
   bool show_fps_enable_ = false;
   bool enable_publish_extrinsic_ = false;
   std::unique_ptr<FpsCounter> fps_counter_color_{nullptr};
+  std::unique_ptr<FpsCounter> fps_counter_left_color_{nullptr};
+  std::unique_ptr<FpsCounter> fps_counter_right_color_{nullptr};
   std::unique_ptr<FpsCounter> fps_counter_depth_{nullptr};
   std::unique_ptr<FpsCounter> fps_counter_left_ir_{nullptr};
   std::unique_ptr<FpsCounter> fps_counter_right_ir_{nullptr};
 
   std::unique_ptr<FpsDelayStatus> fps_delay_status_color_{nullptr};
+  std::unique_ptr<FpsDelayStatus> fps_delay_status_left_color_{nullptr};
+  std::unique_ptr<FpsDelayStatus> fps_delay_status_right_color_{nullptr};
   std::unique_ptr<FpsDelayStatus> fps_delay_status_depth_{nullptr};
+  std::unique_ptr<FpsDelayStatus> fps_delay_status_left_ir_{nullptr};
+  std::unique_ptr<FpsDelayStatus> fps_delay_status_right_ir_{nullptr};
 
   std::string intra_camera_sync_reference_ = "";
   std::string ae_reference_stream_;
diff --git a/orbbec_camera/include/orbbec_camera/timestamp_csv_logger.h b/orbbec_camera/include/orbbec_camera/timestamp_csv_logger.h
index 8c3029de..0d7d4713 100644
--- a/orbbec_camera/include/orbbec_camera/timestamp_csv_logger.h
+++ b/orbbec_camera/include/orbbec_camera/timestamp_csv_logger.h
@@ -22,7 +22,11 @@ class TimestampCsvLogger {
     std::string csv_file_path;
     bool frame_sync_enabled = false;
     bool color_enabled = false;
+    bool left_color_enabled = false;
+    bool right_color_enabled = false;
     bool depth_enabled = false;
+    bool left_ir_enabled = false;
+    bool right_ir_enabled = false;
     bool imu_sync_enabled = false;
     bool accel_enabled = false;
     bool gyro_enabled = false;
@@ -44,6 +48,9 @@ class TimestampCsvLogger {
                            const std::shared_ptr<ob::Frame> &depth_frame, int64_t arrival_system_us,
                            int64_t arrival_steady_us, bool track_color, bool track_depth,
                            bool color_image_publish_expected, bool depth_image_publish_expected);
+  void recordImageFrameArrival(OBStreamType stream_type, const std::shared_ptr<ob::Frame> &frame,
+                               int64_t arrival_system_us, int64_t arrival_steady_us,
+                               bool image_publish_expected);
   void recordImagePrePublish(OBStreamType stream_type, const std::shared_ptr<ob::Frame> &frame,
                              int64_t publish_system_us, int64_t publish_steady_us);
   void recordImagePublishSkipped(OBStreamType stream_type, const std::shared_ptr<ob::Frame> &frame);
@@ -64,7 +71,11 @@ class TimestampCsvLogger {
   std::atomic_bool shutdown_requested_{false};
   std::unique_ptr<FrameTimestampCsvLogger> synced_image_logger_;
   std::unique_ptr<FrameTimestampCsvLogger> color_logger_;
+  std::unique_ptr<FrameTimestampCsvLogger> left_color_logger_;
+  std::unique_ptr<FrameTimestampCsvLogger> right_color_logger_;
   std::unique_ptr<FrameTimestampCsvLogger> depth_logger_;
+  std::unique_ptr<FrameTimestampCsvLogger> left_ir_logger_;
+  std::unique_ptr<FrameTimestampCsvLogger> right_ir_logger_;
   std::unique_ptr<ImuTimestampCsvLogger> synced_imu_logger_;
   std::unique_ptr<ImuTimestampCsvLogger> accel_logger_;
   std::unique_ptr<ImuTimestampCsvLogger> gyro_logger_;
diff --git a/orbbec_camera/launch/gemini_301_series.launch.py b/orbbec_camera/launch/gemini_301_series.launch.py
index d78fc15a..93d77a94 100644
--- a/orbbec_camera/launch/gemini_301_series.launch.py
+++ b/orbbec_camera/launch/gemini_301_series.launch.py
@@ -116,11 +116,11 @@ def generate_launch_description():
         DeclareLaunchArgument('left_color_frame_queue_max_frames', default_value='10'),
         DeclareLaunchArgument('right_color_frame_queue_max_frames', default_value='10'),
         DeclareLaunchArgument('ae_reference_stream', default_value='depth'), # depth or color
-        DeclareLaunchArgument('ae_strategy', default_value='motion'), # default or motion
-        DeclareLaunchArgument('color_width', default_value='848'),
-        DeclareLaunchArgument('color_height', default_value='530'),
-        DeclareLaunchArgument('color_fps', default_value='30'),
-        DeclareLaunchArgument('color_format', default_value='YUYV'),
+        DeclareLaunchArgument('ae_strategy', default_value='default'), # default or motion
+        DeclareLaunchArgument('color_width', default_value='0'),
+        DeclareLaunchArgument('color_height', default_value='0'),
+        DeclareLaunchArgument('color_fps', default_value='0'),
+        DeclareLaunchArgument('color_format', default_value='ANY'),
         DeclareLaunchArgument('enable_color', default_value='true'),
         DeclareLaunchArgument('color_qos', default_value='default'),
         DeclareLaunchArgument('color_qos_history', default_value='default'),
@@ -142,7 +142,6 @@ def generate_launch_description():
         DeclareLaunchArgument('color_gain', default_value='-1'),
         DeclareLaunchArgument('enable_color_auto_white_balance', default_value='true'),
         DeclareLaunchArgument('color_white_balance', default_value='-1'),
-        DeclareLaunchArgument('enable_color_auto_exposure', default_value='true'),
         DeclareLaunchArgument('color_ae_max_exposure', default_value='-1'),
         DeclareLaunchArgument('color_brightness', default_value='-1'),
         DeclareLaunchArgument('color_sharpness', default_value='-1'),
@@ -158,11 +157,11 @@ def generate_launch_description():
         DeclareLaunchArgument('color_denoising_level', default_value='-1'),#0: Auto; 1-8: higher values indicate stronger denoising.
         #Note: The color_denoising_level configuration is supported only when AE is enabled, and requires new firmware support.
 
-        DeclareLaunchArgument('depth_width', default_value='848'),
-        DeclareLaunchArgument('depth_height', default_value='530'),
+        DeclareLaunchArgument('depth_width', default_value='0'),
+        DeclareLaunchArgument('depth_height', default_value='0'),
         DeclareLaunchArgument('depth_decimation_factor', default_value='1'),
-        DeclareLaunchArgument('depth_fps', default_value='30'),
-        DeclareLaunchArgument('depth_format', default_value='Y16'),
+        DeclareLaunchArgument('depth_fps', default_value='0'),
+        DeclareLaunchArgument('depth_format', default_value='ANY'),
         DeclareLaunchArgument('enable_depth', default_value='true'),
         DeclareLaunchArgument('depth_qos', default_value='default'),
         DeclareLaunchArgument('depth_qos_history', default_value='default'),
@@ -178,11 +177,11 @@ def generate_launch_description():
         DeclareLaunchArgument('depth_ae_roi_top', default_value='-1'),
         DeclareLaunchArgument('depth_ae_roi_bottom', default_value='-1'),
         DeclareLaunchArgument('mean_intensity_set_point', default_value='-1'),
-        DeclareLaunchArgument('left_ir_width', default_value='848'),
-        DeclareLaunchArgument('left_ir_height', default_value='530'),
+        DeclareLaunchArgument('left_ir_width', default_value='0'),
+        DeclareLaunchArgument('left_ir_height', default_value='0'),
         DeclareLaunchArgument('left_ir_decimation_factor', default_value='1'),
-        DeclareLaunchArgument('left_ir_fps', default_value='30'),
-        DeclareLaunchArgument('left_ir_format', default_value='Y8'),
+        DeclareLaunchArgument('left_ir_fps', default_value='0'),
+        DeclareLaunchArgument('left_ir_format', default_value='ANY'),
         DeclareLaunchArgument('enable_left_ir', default_value='false'),
         DeclareLaunchArgument('left_ir_qos', default_value='default'),
         DeclareLaunchArgument('left_ir_qos_history', default_value='default'),
@@ -193,11 +192,11 @@ def generate_launch_description():
         DeclareLaunchArgument('left_ir_mirror', default_value='false'),
         DeclareLaunchArgument('enable_left_ir_sequence_id_filter', default_value='false'),
         DeclareLaunchArgument('left_ir_sequence_id_filter_id', default_value='-1'),
-        DeclareLaunchArgument('right_ir_width', default_value='848'),
-        DeclareLaunchArgument('right_ir_height', default_value='530'),
+        DeclareLaunchArgument('right_ir_width', default_value='0'),
+        DeclareLaunchArgument('right_ir_height', default_value='0'),
         DeclareLaunchArgument('right_ir_decimation_factor', default_value='1'),
-        DeclareLaunchArgument('right_ir_fps', default_value='30'),
-        DeclareLaunchArgument('right_ir_format', default_value='Y8'),
+        DeclareLaunchArgument('right_ir_fps', default_value='0'),
+        DeclareLaunchArgument('right_ir_format', default_value='ANY'),
         DeclareLaunchArgument('enable_right_ir', default_value='false'),
         DeclareLaunchArgument('right_ir_qos', default_value='default'),
         DeclareLaunchArgument('right_ir_qos_history', default_value='default'),
@@ -208,7 +207,8 @@ def generate_launch_description():
         DeclareLaunchArgument('right_ir_mirror', default_value='false'),
         DeclareLaunchArgument('enable_right_ir_sequence_id_filter', default_value='false'),
         DeclareLaunchArgument('right_ir_sequence_id_filter_id', default_value='-1'),
-        DeclareLaunchArgument('enable_ir_auto_exposure', default_value='true'),
+        # Gemini 301 color, depth, and IR streams share one auto-exposure switch.
+        DeclareLaunchArgument('enable_auto_exposure', default_value='true'),
         DeclareLaunchArgument('ir_exposure', default_value='-1'),
         DeclareLaunchArgument('ir_gain', default_value='-1'),
         DeclareLaunchArgument('ir_ae_max_exposure', default_value='-1'),
@@ -291,7 +291,7 @@ def generate_launch_description():
         DeclareLaunchArgument('config_file_path', default_value=''),
         DeclareLaunchArgument('enable_heartbeat', default_value='false'),
         DeclareLaunchArgument('enable_firmware_log', default_value='false'),
-        DeclareLaunchArgument('enable_fps_boost', default_value='false'),
+        DeclareLaunchArgument('enable_fps_boost', default_value='true'),
         DeclareLaunchArgument('monitor_poll_interval_sec', default_value='-1'),
         DeclareLaunchArgument('gmsl_trigger_fps', default_value='3000'),
         DeclareLaunchArgument('enable_gmsl_trigger', default_value='false'),
diff --git a/orbbec_camera/scripts/common_benchmark_node.py b/orbbec_camera/scripts/common_benchmark_node.py
index a012d5a6..15c59b05 100644
--- a/orbbec_camera/scripts/common_benchmark_node.py
+++ b/orbbec_camera/scripts/common_benchmark_node.py
@@ -4,11 +4,14 @@
 Support: ROS2
 name: common_benchmark_node.py
 function: A ROS2 node to monitor and log the performance of an Orbbec camera node:
-          frame rates, delays, CPU and RAM usage, packet/frame loss statistics.
+          subscriber-side frame rates, end-to-end delays, CPU and RAM usage,
+          and estimated frame loss statistics.
 usage:
     ros2 run orbbec_camera common_benchmark_node.py --run_time 20 --csv_file /tmp/cam_log.csv
                     You can also pass an ideal frame rate for drop detection: --ideal_fps 30
                     Monitor multiple cameras: --camera_names camera,camera01
+                    Select topics explicitly: --topics color,depth
+                    Compressed images and point clouds require full topic names.
 """
 
 import argparse
@@ -21,11 +24,22 @@ import csv
 import os
 from collections import defaultdict
 from orbbec_camera_msgs.msg import DeviceStatus
-from sensor_msgs.msg import Image
+from sensor_msgs.msg import CompressedImage, Image, PointCloud2
 
 from tabulate import tabulate
 
 CAMERA_NODE_NAMES = ["component_container", "orbbec_camera_node", "nodelet"]
+MONITORED_STREAMS = (
+    "color",
+    "depth",
+    "ir",
+    "left_ir",
+    "right_ir",
+    "left_color",
+    "right_color",
+)
+DISCOVERY_INTERVAL_SECONDS = 0.1
+DISCOVERY_DURATION_SECONDS = 1.0
 DOCUMENTATION_URL = (
     "https://orbbec.github.io/OrbbecSDK_ROS2/en/source/camera_devices/"
     "6_benchmark/benchmark_tools.html"
@@ -99,24 +113,62 @@ def estimate_dropped_frames(dt, expected_interval):
 # ----------------------------------------------
 
 class TopicTracker:
-    def __init__(self, logger=None):
+    def __init__(self, logger=None, sample_start_time=None):
         self.received = 0
-
-        self.last_time = None
+        self.sample_start_time = sample_start_time or time.monotonic()
+        self.last_sample_time = self.sample_start_time
+        self.last_sample_received = 0
+        self.last_header_stamp = None
+        self.estimated_interval = None
         self.drop_frames = 0
 
         self.logger = logger
 
-    def on_msg(self, header, avg_fps):
-        stamp = header.stamp.sec + header.stamp.nanosec * 1e-9
+    def on_msg(self, header, ros_receive_time, ideal_fps):
+        """Record one received message and return its age in milliseconds."""
+        header_stamp = header.stamp.sec + header.stamp.nanosec * 1e-9
         self.received += 1
+        delay_ms = None
 
-        if self.last_time is not None and avg_fps > 0:
-            dt = stamp - self.last_time
-            expected_interval = 1.0 / avg_fps
-            self.drop_frames += estimate_dropped_frames(dt, expected_interval)
+        if header_stamp > 0:
+            delay_ms = (ros_receive_time - header_stamp) * 1000.0
+            if self.last_header_stamp is not None:
+                header_dt = header_stamp - self.last_header_stamp
+                if header_dt > 0:
+                    expected_interval = (
+                        1.0 / ideal_fps
+                        if ideal_fps and ideal_fps > 0.0
+                        else self.estimated_interval
+                    )
+                    if expected_interval is not None:
+                        self.drop_frames += estimate_dropped_frames(
+                            header_dt, expected_interval
+                        )
+                    self.update_estimated_interval(header_dt)
+            self.last_header_stamp = header_stamp
 
-        self.last_time = stamp
+        return delay_ms
+
+    def sample_fps(self, sample_time):
+        """Calculate current and average subscriber throughput."""
+        window_elapsed = sample_time - self.last_sample_time
+        total_elapsed = sample_time - self.sample_start_time
+        if window_elapsed <= 0.0 or total_elapsed <= 0.0:
+            return None
+
+        window_received = self.received - self.last_sample_received
+        current_fps = window_received / window_elapsed
+        average_fps = self.received / total_elapsed
+        self.last_sample_time = sample_time
+        self.last_sample_received = self.received
+        return current_fps, average_fps
+
+    def update_estimated_interval(self, interval):
+        """Learn the nominal source interval while excluding likely frame gaps."""
+        if self.estimated_interval is None or interval < 0.75 * self.estimated_interval:
+            self.estimated_interval = interval
+        elif interval <= 1.5 * self.estimated_interval:
+            self.estimated_interval = 0.9 * self.estimated_interval + 0.1 * interval
 
     def frames_loss_rate(self):
         total = self.received + self.drop_frames
@@ -129,17 +181,27 @@ class TopicTracker:
 
 
 class CameraMonitorNode(Node):
-    def __init__(self, run_time, csv_file="camera_monitor_log.csv", ideal_fps: float = 0.0, camera_names=None):
+    def __init__(
+        self,
+        run_time,
+        csv_file="camera_monitor_log.csv",
+        ideal_fps: float = 0.0,
+        camera_names=None,
+        topics=None,
+    ):
         super().__init__("camera_monitor_node")
 
         self.run_time = run_time
-        self.start_time = time.time()
+        self.discovery_start_time = time.time()
+        self.discovery_start_monotonic = None
+        self.start_time = None
         self.process = psutil.Process(os.getpid())
         self.first_data_collected = False
         self.camera_names = parse_camera_names(camera_names)
         self.node_names = {camera_name: "Not Found" for camera_name in self.camera_names}
         self.total_node_name = "Not Found"
-        # If > 0, use this ideal fps value for drop-frame detection instead of the reported average
+        # If > 0, use this ideal FPS for drop detection instead of learning the
+        # nominal interval from received image timestamps.
         self.ideal_fps = float(ideal_fps) if ideal_fps is not None else 0.0
         self.finished = False
 
@@ -153,22 +215,26 @@ class CameraMonitorNode(Node):
                 "stats": defaultdict(make_stat),
                 "cpu_stats": make_stat(),
                 "ram_stats": make_stat(),
-                "trackers": {
-                    "color": TopicTracker(logger=self.get_logger()),
-                    "depth": TopicTracker(logger=self.get_logger())
-                }
+                "trackers": {},
             }
 
         self.total_cpu_stats = make_stat()
         self.total_ram_stats = make_stat()
+        self.topic_subscriptions = {}
+        self.topic_configs = {}
+        self.discovered_streams = {}
+        self.discovery_complete = False
+        self.discovery_timer = None
+        self.timer = None
+        self.requested_streams = self.parse_requested_topics(topics)
 
         # CSV
         self.csv_file = csv_file
         self.csv_fh = open(self.csv_file, "w", newline="")
         self.csv_writer = csv.writer(self.csv_fh)
-        self.csv_writer.writerow(self.build_csv_header())
 
-        # subscriptions
+        # Device status subscriptions are always present. Data subscriptions
+        # are created once after automatic discovery or explicit selection.
         for camera_name in self.camera_names:
             ns = self.camera_namespace(camera_name)
             self.create_subscription(
@@ -177,29 +243,26 @@ class CameraMonitorNode(Node):
                 lambda msg, name=camera_name: self.status_callback(msg, name),
                 5
             )
-            self.create_subscription(
-                Image,
-                f"{ns}/color/image_raw",
-                lambda msg, name=camera_name: self.image_callback(msg, name, "color"),
-                5
-            )
-            self.create_subscription(
-                Image,
-                f"{ns}/depth/image_raw",
-                lambda msg, name=camera_name: self.image_callback(msg, name, "depth"),
-                5
-            )
 
-        # timer runs every 1s to update system stats, log csv and print status
-        self.timer = self.create_timer(1.0, self.timer_callback)
+        if self.requested_streams:
+            self.finish_topic_discovery(self.requested_streams)
+        else:
+            self.discovery_start_monotonic = time.monotonic()
+            self.discovery_timer = self.create_timer(
+                DISCOVERY_INTERVAL_SECONDS, self.update_topic_discovery
+            )
 
     def timer_callback(self):
+        if not self.discovery_complete:
+            return
+
         elapsed = time.time() - self.start_time
         if elapsed > self.run_time:
             self.finish()
             rclpy.shutdown()
             return
 
+        self.update_topic_fps_stats()
         camera_sys_stats, total_cpu, total_ram, self.total_node_name = self.get_camera_stats()
         for camera_name in self.camera_names:
             camera = self.cameras[camera_name]
@@ -220,7 +283,8 @@ class CameraMonitorNode(Node):
             return
         self.finished = True
 
-        elapsed = time.time() - self.start_time
+        timer_start = self.start_time or self.discovery_start_time
+        elapsed = time.time() - timer_start
         try:
             self.csv_fh.close()
         except Exception:
@@ -231,6 +295,150 @@ class CameraMonitorNode(Node):
     def camera_namespace(self, camera_name):
         return "/" + camera_name.strip("/")
 
+    def image_topic_name(self, camera_name, stream):
+        return f"{self.camera_namespace(camera_name)}/{stream}/image_raw"
+
+    def make_raw_image_config(self, camera_name, stream):
+        return {
+            "camera_name": camera_name,
+            "topic_id": stream,
+            "topic_name": self.image_topic_name(camera_name, stream),
+            "msg_type": Image,
+        }
+
+    def parse_full_topic(self, topic_name):
+        normalized_topic = "/" + topic_name.strip("/")
+        for camera_name in self.camera_names:
+            namespace_prefix = self.camera_namespace(camera_name) + "/"
+            if not normalized_topic.startswith(namespace_prefix):
+                continue
+
+            relative_name = normalized_topic[len(namespace_prefix):]
+            for stream in MONITORED_STREAMS:
+                raw_name = f"{stream}/image_raw"
+                if relative_name == raw_name:
+                    return self.make_raw_image_config(camera_name, stream)
+                if relative_name == f"{raw_name}/compressed":
+                    return {
+                        "camera_name": camera_name,
+                        "topic_id": f"{stream}_compressed",
+                        "topic_name": normalized_topic,
+                        "msg_type": CompressedImage,
+                    }
+                if relative_name == f"{raw_name}/compressedDepth":
+                    return {
+                        "camera_name": camera_name,
+                        "topic_id": f"{stream}_compressed_depth",
+                        "topic_name": normalized_topic,
+                        "msg_type": CompressedImage,
+                    }
+
+            point_cloud_ids = {
+                "depth/points": "depth_points",
+                "depth_registered/points": "depth_registered_points",
+            }
+            if relative_name in point_cloud_ids:
+                return {
+                    "camera_name": camera_name,
+                    "topic_id": point_cloud_ids[relative_name],
+                    "topic_name": normalized_topic,
+                    "msg_type": PointCloud2,
+                }
+        return None
+
+    def parse_requested_topics(self, topics):
+        if not topics:
+            return {}
+
+        requested = str(topics).replace(";", ",").split(",")
+        selected = {}
+        for value in requested:
+            value = value.strip()
+            if not value:
+                continue
+            if value in MONITORED_STREAMS:
+                for camera_name in self.camera_names:
+                    config = self.make_raw_image_config(camera_name, value)
+                    selected[(camera_name, config["topic_id"])] = config
+                continue
+
+            config = self.parse_full_topic(value)
+            if config is None:
+                raise ValueError(
+                    f"Unsupported topic '{value}'. Specify a raw or compressed image topic, "
+                    "or a depth/points or depth_registered/points topic under a configured "
+                    "camera namespace."
+                )
+            selected[(config["camera_name"], config["topic_id"])] = config
+        return selected
+
+    def find_published_image_streams(self):
+        published_streams = {}
+        for camera_name in self.camera_names:
+            for stream in MONITORED_STREAMS:
+                config = self.make_raw_image_config(camera_name, stream)
+                key = (camera_name, config["topic_id"])
+                if self.count_publishers(config["topic_name"]) > 0:
+                    published_streams[key] = config
+        return published_streams
+
+    def update_topic_discovery(self):
+        if self.discovery_complete:
+            return
+
+        self.discovered_streams.update(self.find_published_image_streams())
+        elapsed = time.monotonic() - self.discovery_start_monotonic
+        if elapsed >= DISCOVERY_DURATION_SECONDS:
+            self.finish_topic_discovery(self.discovered_streams)
+
+    def finish_topic_discovery(self, selected_streams):
+        self.topic_configs = dict(selected_streams)
+        sample_start_time = time.monotonic()
+        for key, config in self.topic_configs.items():
+            camera_name = config["camera_name"]
+            topic_id = config["topic_id"]
+            self.cameras[camera_name]["trackers"][topic_id] = TopicTracker(
+                logger=self.get_logger(), sample_start_time=sample_start_time
+            )
+            self.topic_subscriptions[key] = self.create_subscription(
+                config["msg_type"],
+                config["topic_name"],
+                lambda msg, name=camera_name, selected_topic_id=topic_id: self.topic_callback(
+                    msg, name, selected_topic_id
+                ),
+                5,
+            )
+
+        self.csv_writer.writerow(self.build_csv_header())
+        self.csv_fh.flush()
+        self.start_time = time.time()
+        self.discovery_complete = True
+        self.timer = self.create_timer(1.0, self.timer_callback)
+        if self.discovery_timer is not None:
+            self.discovery_timer.cancel()
+
+    def topics_for_camera(self, camera_name):
+        return [
+            config
+            for config in self.topic_configs.values()
+            if config["camera_name"] == camera_name
+        ]
+
+    def update_topic_fps_stats(self):
+        sample_time = time.monotonic()
+        for config in self.topic_configs.values():
+            camera = self.cameras[config["camera_name"]]
+            topic_id = config["topic_id"]
+            fps_sample = camera["trackers"][topic_id].sample_fps(sample_time)
+            if fps_sample is None:
+                continue
+            current_fps, average_fps = fps_sample
+            self.update_sample_stat(
+                camera["stats"][f"{topic_id}_fps"],
+                current_fps,
+                average=average_fps,
+            )
+
     def cmdline_has_camera_namespace(self, cmdline_args, camera_name):
         ns = self.camera_namespace(camera_name)
         candidates = [
@@ -311,32 +519,30 @@ class CameraMonitorNode(Node):
 
         camera["prev_online"] = msg.device_online
 
-        # update stats from DeviceStatus message fields
-        self.update_stats(camera["stats"], "color_fps", msg.color_frame_rate_cur, msg.color_frame_rate_min, msg.color_frame_rate_max, msg.color_frame_rate_avg)
-        self.update_stats(camera["stats"], "color_delay", msg.color_delay_ms_cur, msg.color_delay_ms_min, msg.color_delay_ms_max, msg.color_delay_ms_avg)
-        self.update_stats(camera["stats"], "depth_fps", msg.depth_frame_rate_cur, msg.depth_frame_rate_min, msg.depth_frame_rate_max, msg.depth_frame_rate_avg)
-        self.update_stats(camera["stats"], "depth_delay", msg.depth_delay_ms_cur, msg.depth_delay_ms_min, msg.depth_delay_ms_max, msg.depth_delay_ms_avg)
-
-    def image_callback(self, msg: Image, camera_name: str, stream: str):
-        if stream not in ("color", "depth"):
-            return
-        header = msg.header
+    def topic_callback(self, msg, camera_name: str, topic_id: str):
+        self.first_data_collected = True
         camera = self.cameras[camera_name]
-        tracker = camera["trackers"][stream]
-        # Prefer a user-specified ideal fps for drop detection when provided.
-        fps_to_use = self.ideal_fps if (self.ideal_fps and self.ideal_fps > 0.0) else camera["stats"][f"{stream}_fps"]["avg"]
-        tracker.on_msg(header, fps_to_use)
+        camera["data_collected"] = True
+        tracker = camera["trackers"][topic_id]
+        ros_receive_time = self.get_clock().now().nanoseconds * 1e-9
+        delay_ms = tracker.on_msg(
+            msg.header,
+            ros_receive_time,
+            self.ideal_fps,
+        )
 
-    def update_stats(self, stats, key, cur, min_val, max_val, avg_val):
-        if min_val <= 1e-3 or avg_val < 0:  # ignore invalid data
-            return
-        s = stats[key]
-        s["cur"] = (cur)
-        s["count"] += 1
-        s["sum"] += avg_val
-        s["avg"] = s["sum"] / s["count"] if s["count"] > 0 else 0.0
-        s["min"] = min(s["min"], min_val)
-        s["max"] = max(s["max"], max_val)
+        if delay_ms is not None:
+            self.update_sample_stat(
+                camera["stats"][f"{topic_id}_delay"], delay_ms
+            )
+
+    def update_sample_stat(self, stat, value, average=None):
+        stat["cur"] = value
+        stat["count"] += 1
+        stat["sum"] += value
+        stat["avg"] = average if average is not None else stat["sum"] / stat["count"]
+        stat["min"] = min(stat["min"], value)
+        stat["max"] = max(stat["max"], value)
 
     def update_sys_stat(self, stat_dict, value, online=True):
         stat_dict["cur"] = value
@@ -353,7 +559,7 @@ class CameraMonitorNode(Node):
         row = [round(elapsed, 2)]
         for camera_name in self.camera_names:
             camera = self.cameras[camera_name]
-            row.extend(self.build_camera_csv_values(camera))
+            row.extend(self.build_camera_csv_values(camera_name, camera))
 
         row.extend([
             round(self.total_cpu_stats["cur"], 2), round(self.total_cpu_stats["avg"], 2),
@@ -365,19 +571,42 @@ class CameraMonitorNode(Node):
 
     def build_csv_header(self):
         header = ["time(s)"]
-        camera_fields = [
-            "connection_type", "status_online", "disconnects",
-            "color_fps_cur", "color_fps_avg", "color_fps_min", "color_fps_max",
-            "color_delay_cur", "color_delay_avg", "color_delay_min", "color_delay_max",
-            "depth_fps_cur", "depth_fps_avg", "depth_fps_min", "depth_fps_max",
-            "depth_delay_cur", "depth_delay_avg", "depth_delay_min", "depth_delay_max",
-            "cpu_cur", "cpu_avg", "cpu_min", "cpu_max",
-            "ram_cur", "ram_avg", "ram_min", "ram_max",
-            "color_frames_loss", "color_frames_loss_rate(%)",
-            "depth_frames_loss", "depth_frames_loss_rate(%)"
-        ]
         for camera_name in self.camera_names:
-            header.extend([f"{camera_name}_{field}" for field in camera_fields])
+            header.extend(
+                [
+                    f"{camera_name}_connection_type",
+                    f"{camera_name}_status_online",
+                    f"{camera_name}_disconnects",
+                ]
+            )
+            for config in self.topics_for_camera(camera_name):
+                topic_id = config["topic_id"]
+                header.extend(
+                    [
+                        f"{camera_name}_{topic_id}_fps_cur",
+                        f"{camera_name}_{topic_id}_fps_avg",
+                        f"{camera_name}_{topic_id}_fps_min",
+                        f"{camera_name}_{topic_id}_fps_max",
+                        f"{camera_name}_{topic_id}_delay_cur",
+                        f"{camera_name}_{topic_id}_delay_avg",
+                        f"{camera_name}_{topic_id}_delay_min",
+                        f"{camera_name}_{topic_id}_delay_max",
+                        f"{camera_name}_{topic_id}_sub_lost_count",
+                        f"{camera_name}_{topic_id}_sub_lost_rate(%)",
+                    ]
+                )
+            header.extend(
+                [
+                    f"{camera_name}_cpu_cur",
+                    f"{camera_name}_cpu_avg",
+                    f"{camera_name}_cpu_min",
+                    f"{camera_name}_cpu_max",
+                    f"{camera_name}_ram_cur",
+                    f"{camera_name}_ram_avg",
+                    f"{camera_name}_ram_min",
+                    f"{camera_name}_ram_max",
+                ]
+            )
 
         header.extend([
             "total_cpu_cur", "total_cpu_avg", "total_cpu_min", "total_cpu_max",
@@ -385,10 +614,7 @@ class CameraMonitorNode(Node):
         ])
         return header
 
-    def build_camera_csv_values(self, camera):
-        color_tracker = camera["trackers"]["color"]
-        depth_tracker = camera["trackers"]["depth"]
-
+    def build_camera_csv_values(self, camera_name, camera):
         def safe(k):
             v = camera["stats"].get(k, {})
             return (
@@ -398,29 +624,47 @@ class CameraMonitorNode(Node):
                 self.format_csv_number(v.get("max", 0.0)),
             )
 
-        if not camera["prev_online"]:
-            return [
-                camera["connection_type"], camera["prev_online"], camera["disconnect_count"],
-                *["N/A"] * 16,
-                round(camera["cpu_stats"]["cur"], 2), "N/A", "N/A", "N/A",
-                round(camera["ram_stats"]["cur"], 2), "N/A", "N/A", "N/A",
-                color_tracker.drop_frames, round(color_tracker.frames_loss_rate() * 100.0, 3),
-                depth_tracker.drop_frames, round(depth_tracker.frames_loss_rate() * 100.0, 3)
-            ]
-
-        return [
-            camera["connection_type"], camera["prev_online"], camera["disconnect_count"],
-            *safe("color_fps"),
-            *safe("color_delay"),
-            *safe("depth_fps"),
-            *safe("depth_delay"),
-            round(camera["cpu_stats"]["cur"], 2), round(camera["cpu_stats"]["avg"], 2),
-            self.format_csv_number(camera["cpu_stats"]["min"]), self.format_csv_number(camera["cpu_stats"]["max"]),
-            round(camera["ram_stats"]["cur"], 2), round(camera["ram_stats"]["avg"], 2),
-            self.format_csv_number(camera["ram_stats"]["min"]), self.format_csv_number(camera["ram_stats"]["max"]),
-            color_tracker.drop_frames, round(color_tracker.frames_loss_rate() * 100.0, 3),
-            depth_tracker.drop_frames, round(depth_tracker.frames_loss_rate() * 100.0, 3)
+        values = [
+            camera["connection_type"],
+            camera["prev_online"],
+            camera["disconnect_count"],
         ]
+        for config in self.topics_for_camera(camera_name):
+            topic_id = config["topic_id"]
+            tracker = camera["trackers"][topic_id]
+            if camera["prev_online"]:
+                values.extend(safe(f"{topic_id}_fps"))
+                values.extend(safe(f"{topic_id}_delay"))
+            else:
+                values.extend(["N/A"] * 8)
+            values.extend(
+                [
+                    tracker.drop_frames,
+                    round(tracker.frames_loss_rate() * 100.0, 3),
+                ]
+            )
+
+        if camera["prev_online"]:
+            values.extend(
+                [
+                    round(camera["cpu_stats"]["cur"], 2),
+                    round(camera["cpu_stats"]["avg"], 2),
+                    self.format_csv_number(camera["cpu_stats"]["min"]),
+                    self.format_csv_number(camera["cpu_stats"]["max"]),
+                    round(camera["ram_stats"]["cur"], 2),
+                    round(camera["ram_stats"]["avg"], 2),
+                    self.format_csv_number(camera["ram_stats"]["min"]),
+                    self.format_csv_number(camera["ram_stats"]["max"]),
+                ]
+            )
+        else:
+            values.extend(
+                [
+                    round(camera["cpu_stats"]["cur"], 2), "N/A", "N/A", "N/A",
+                    round(camera["ram_stats"]["cur"], 2), "N/A", "N/A", "N/A",
+                ]
+            )
+        return values
 
     def format_csv_number(self, value):
         if value == float("inf") or value == float("-inf"):
@@ -436,25 +680,30 @@ class CameraMonitorNode(Node):
         rows = []
         for camera_name in self.camera_names:
             camera = self.cameras[camera_name]
-            for stream in ["color", "depth"]:
-                fps_key = f"{stream}_fps"
-                delay_key = f"{stream}_delay"
-                topic_name = f"/{camera_name}/{stream}/image_raw"
+            for config in self.topics_for_camera(camera_name):
+                topic_id = config["topic_id"]
+                fps_key = f"{topic_id}_fps"
+                delay_key = f"{topic_id}_delay"
+                topic_name = config["topic_name"]
                 if not camera["prev_online"]:
                     rows.append([camera_name, topic_name, *["N/A"] * 10])
                 else:
                     fps_vals = format_stats(camera["stats"][fps_key])
                     delay_vals = format_stats(camera["stats"][delay_key])
-                    tracker = camera["trackers"][stream]
+                    tracker = camera["trackers"][topic_id]
 
                     frames_loss = tracker.drop_frames
                     frames_loss_rate = round(tracker.frames_loss_rate() * 100.0, 3)
                     rows.append([camera_name, topic_name, *fps_vals, *delay_vals, frames_loss, frames_loss_rate])
 
-        header_bottom = ["Camera", "Topic", "fps_cur", "fps_avg", "fps_min", "fps_max", "delay_cur(ms)", "delay_avg(ms)", "delay_min(ms)", "delay_max(ms)", "Pub_lost_count", "Pub_lost_rate(%)"]
+        header_bottom = [
+            "Camera", "Topic", "fps_cur", "fps_avg", "fps_min", "fps_max",
+            "delay_cur(ms)", "delay_avg(ms)", "delay_min(ms)", "delay_max(ms)",
+            "Sub_lost_count", "Sub_lost_rate(%)",
+        ]
 
         os.system("clear")
-        print("Orbbec Camera Benchmark\n")
+        print("Orbbec Camera Subscriber Benchmark\n")
         print(tabulate([header_bottom] + rows, tablefmt="fancy_grid"))
 
         sys_rows = []
@@ -491,13 +740,36 @@ def main(argv=None):
     )
     parser.add_argument("--run_time", type=str, default="10s", help="Total run time for monitoring, e.g., 10s, 5m, 1h.")
     parser.add_argument("--csv_file", type=str, default="camera_monitor_log.csv")
-    parser.add_argument("--ideal_fps", type=float, default=0.0, help="Optional ideal frame rate to use for drop detection (overrides reported avg).")
+    parser.add_argument(
+        "--ideal_fps",
+        type=float,
+        default=0.0,
+        help=(
+            "Optional ideal frame rate for subscriber-side drop detection; "
+            "otherwise it is learned from image timestamps."
+        ),
+    )
     parser.add_argument("--camera_names", type=str, default="camera", help="Comma-separated camera namespaces, e.g., camera,camera01,camera02.")
+    parser.add_argument(
+        "--topics",
+        type=str,
+        default="",
+        help=(
+            "Comma-separated raw stream names or full raw/compressed image and point cloud "
+            "topics. Automatic discovery only selects raw image topics."
+        ),
+    )
     cli_args, _ = parser.parse_known_args(argv)
 
     rclpy.init(args=argv)
     run_time = parse_duration(cli_args.run_time)
-    node = CameraMonitorNode(run_time, cli_args.csv_file, ideal_fps=cli_args.ideal_fps, camera_names=cli_args.camera_names)
+    node = CameraMonitorNode(
+        run_time,
+        cli_args.csv_file,
+        ideal_fps=cli_args.ideal_fps,
+        camera_names=cli_args.camera_names,
+        topics=cli_args.topics,
+    )
 
     try:
         rclpy.spin(node)
diff --git a/orbbec_camera/scripts/default_service.yaml b/orbbec_camera/scripts/default_service.yaml
index fae49b92..ea2256db 100644
--- a/orbbec_camera/scripts/default_service.yaml
+++ b/orbbec_camera/scripts/default_service.yaml
@@ -192,7 +192,7 @@ services:
   - name: /camera/read_customer_data
     type: orbbec_camera_msgs/srv/GetString
 
-  - name: /camera/set_floor_enable
+  - name: /camera/set_flood_enable
     type: std_srvs/srv/SetBool
     request: {data: false}
   - name: /camera/set_fan_work_mode
diff --git a/orbbec_camera/src/dynamic_params.cpp b/orbbec_camera/src/dynamic_params.cpp
index 657b9a36..52bb4c2c 100644
--- a/orbbec_camera/src/dynamic_params.cpp
+++ b/orbbec_camera/src/dynamic_params.cpp
@@ -21,21 +21,23 @@ Parameters::Parameters(rclcpp::Node *node)
     : node_(node), logger_(node_->get_logger()), params_backend_(node) {
   params_backend_.addOnSetParametersCallback(
       [this](const std::vector<rclcpp::Parameter> &parameters) {
+        rcl_interfaces::msg::SetParametersResult result;
+        result.successful = true;
         for (const auto &parameter : parameters) {
-          if (param_functions_.find(parameter.get_name()) != param_functions_.end()) {
-            auto functions = param_functions_[parameter.get_name()];
-            if (functions.empty()) {
-              RCLCPP_WARN_STREAM(logger_, "Parameter " << parameter.get_name()
-                                                       << " can not be changed in runtime.");
-            } else {
-              for (const auto &func : param_functions_[parameter.get_name()]) {
-                func(parameter);
-              }
+          const auto function_it = param_functions_.find(parameter.get_name());
+          if (function_it == param_functions_.end()) {
+            continue;
+          }
+          if (function_it->second.empty()) {
+            result.successful = false;
+            result.reason = "Parameter " + parameter.get_name() + " can not be changed in runtime.";
+            RCLCPP_WARN_STREAM(logger_, result.reason);
+          } else {
+            for (const auto &func : function_it->second) {
+              func(parameter);
             }
           }
         }
-        rcl_interfaces::msg::SetParametersResult result;
-        result.successful = true;
         return result;
       });
 }
diff --git a/orbbec_camera/src/frame_timestamp_csv_logger.cpp b/orbbec_camera/src/frame_timestamp_csv_logger.cpp
index 9100dcc1..474054aa 100644
--- a/orbbec_camera/src/frame_timestamp_csv_logger.cpp
+++ b/orbbec_camera/src/frame_timestamp_csv_logger.cpp
@@ -31,6 +31,47 @@ int64_t getExpectedIntervalUs(const std::shared_ptr<ob::Frame> &frame) {
   return static_cast<int64_t>(1000000.0 / static_cast<double>(fps));
 }
 
+std::optional<FrameTimestampCsvLogger::OutputMode> outputModeForStream(OBStreamType stream_type) {
+  using OutputMode = FrameTimestampCsvLogger::OutputMode;
+  switch (stream_type) {
+    case OB_STREAM_COLOR:
+      return OutputMode::COLOR;
+    case OB_STREAM_COLOR_LEFT:
+      return OutputMode::LEFT_COLOR;
+    case OB_STREAM_COLOR_RIGHT:
+      return OutputMode::RIGHT_COLOR;
+    case OB_STREAM_DEPTH:
+      return OutputMode::DEPTH;
+    case OB_STREAM_IR_LEFT:
+      return OutputMode::LEFT_IR;
+    case OB_STREAM_IR_RIGHT:
+      return OutputMode::RIGHT_IR;
+    default:
+      return std::nullopt;
+  }
+}
+
+const char *outputModeName(FrameTimestampCsvLogger::OutputMode output_mode) {
+  using OutputMode = FrameTimestampCsvLogger::OutputMode;
+  switch (output_mode) {
+    case OutputMode::COLOR:
+      return "color";
+    case OutputMode::LEFT_COLOR:
+      return "left_color";
+    case OutputMode::RIGHT_COLOR:
+      return "right_color";
+    case OutputMode::DEPTH:
+      return "depth";
+    case OutputMode::LEFT_IR:
+      return "left_ir";
+    case OutputMode::RIGHT_IR:
+      return "right_ir";
+    case OutputMode::SYNCED:
+      return "synced";
+  }
+  return "unknown";
+}
+
 }  // namespace
 
 FrameTimestampCsvLogger::FrameTimestampCsvLogger(bool drop_log_enabled,
@@ -104,9 +145,8 @@ void FrameTimestampCsvLogger::recordStandaloneFrameArrival(OBStreamType stream_t
                                                            int64_t arrival_system_us,
                                                            int64_t arrival_steady_us,
                                                            bool image_publish_expected) {
-  if (!enabled_ || !frame || !isTrackedStream(stream_type) ||
-      (stream_type == OB_STREAM_COLOR && output_mode_ != OutputMode::COLOR) ||
-      (stream_type == OB_STREAM_DEPTH && output_mode_ != OutputMode::DEPTH)) {
+  const auto expected_output_mode = outputModeForStream(stream_type);
+  if (!enabled_ || !frame || !expected_output_mode || output_mode_ != *expected_output_mode) {
     return;
   }
   recordStandaloneFrameArrivalInternal(stream_type, frame, arrival_system_us, arrival_steady_us,
@@ -163,14 +203,11 @@ void FrameTimestampCsvLogger::shutdown() {
 
 FrameTimestampCsvLogger::TrackedStream FrameTimestampCsvLogger::toTrackedStream(
     OBStreamType stream_type) const {
-  if (stream_type == OB_STREAM_COLOR) {
-    return TrackedStream::COLOR;
-  }
-  return TrackedStream::DEPTH;
+  return stream_type == OB_STREAM_DEPTH ? TrackedStream::DEPTH : TrackedStream::COLOR;
 }
 
 bool FrameTimestampCsvLogger::isTrackedStream(OBStreamType stream_type) const {
-  return stream_type == OB_STREAM_COLOR || stream_type == OB_STREAM_DEPTH;
+  return outputModeForStream(stream_type).has_value();
 }
 
 void FrameTimestampCsvLogger::recordFrameSetInternal(
@@ -294,10 +331,12 @@ void FrameTimestampCsvLogger::completeImagePublishInternal(
     auto row_id_it = row_map.find(frame_index);
     if (row_id_it == row_map.end()) {
       if (publish_system_us.has_value()) {
-        RCLCPP_WARN_STREAM(logger_,
-                           "Frame timestamp CSV logger missed row mapping for stream "
-                               << (tracked_stream == TrackedStream::COLOR ? "color" : "depth")
-                               << " frame index " << frame_index);
+        RCLCPP_WARN_STREAM(
+            logger_, "Frame timestamp CSV logger missed row mapping for stream "
+                         << (output_mode_ == OutputMode::SYNCED
+                                 ? (tracked_stream == TrackedStream::COLOR ? "color" : "depth")
+                                 : outputModeName(output_mode_))
+                         << " frame index " << frame_index);
       }
       return;
     }
@@ -357,7 +396,10 @@ void FrameTimestampCsvLogger::populateArrivalData(StreamState &state, TrackedStr
         previous.dropped_frames += lost_frames;
         RCLCPP_WARN_STREAM(logger_,
                            "Frame drop detected: stage=SDK_RECEIVE"
-                               << " stream=" << (stream == TrackedStream::COLOR ? "color" : "depth")
+                               << " stream="
+                               << (output_mode_ == OutputMode::SYNCED
+                                       ? (stream == TrackedStream::COLOR ? "color" : "depth")
+                                       : outputModeName(output_mode_))
                                << " frame_index=" << state.frame_index
                                << " dropped=" << previous.dropped_frames);
       }
@@ -408,7 +450,10 @@ void FrameTimestampCsvLogger::populatePublishData(StreamState &state, TrackedStr
         previous.publish_dropped_frames += lost_frames;
         RCLCPP_WARN_STREAM(logger_,
                            "Frame drop detected: stage=ROS_PUBLISH"
-                               << " stream=" << (stream == TrackedStream::COLOR ? "color" : "depth")
+                               << " stream="
+                               << (output_mode_ == OutputMode::SYNCED
+                                       ? (stream == TrackedStream::COLOR ? "color" : "depth")
+                                       : outputModeName(output_mode_))
                                << " frame_index=" << state.frame_index
                                << " dropped=" << previous.publish_dropped_frames);
       }
@@ -485,12 +530,12 @@ void FrameTimestampCsvLogger::eraseFrameIndexMappingLocked(const PendingRow &row
 }
 
 std::string FrameTimestampCsvLogger::serializeRow(const PendingRow &row) const {
-  if (output_mode_ == OutputMode::COLOR) {
-    return serializeStreamColumns(row.color);
-  }
   if (output_mode_ == OutputMode::DEPTH) {
     return serializeStreamColumns(row.depth);
   }
+  if (output_mode_ != OutputMode::SYNCED) {
+    return serializeStreamColumns(row.color);
+  }
   std::ostringstream ss;
   ss << serializeStreamColumns(row.color) << "," << serializeStreamColumns(row.depth);
   return ss.str();
@@ -560,14 +605,12 @@ std::string FrameTimestampCsvLogger::csvHeader() const {
     ss << prefix << "_sdk_delay_from_global_us,";
     ss << prefix << "_sdk_delay_from_system_us";
   };
-  if (output_mode_ == OutputMode::SYNCED || output_mode_ == OutputMode::COLOR) {
-    append_stream_header("color");
-  }
   if (output_mode_ == OutputMode::SYNCED) {
+    append_stream_header("color");
     ss << ",";
-  }
-  if (output_mode_ == OutputMode::SYNCED || output_mode_ == OutputMode::DEPTH) {
     append_stream_header("depth");
+  } else {
+    append_stream_header(outputModeName(output_mode_));
   }
   return ss.str();
 }
@@ -641,10 +684,8 @@ void FrameTimestampCsvLogger::writerThreadMain() {
 std::string FrameTimestampCsvLogger::csvFilePathForIndex(uint64_t file_index) const {
   const std::filesystem::path original_path(csv_file_path_);
   std::string suffix;
-  if (output_mode_ == OutputMode::COLOR) {
-    suffix = "_color";
-  } else if (output_mode_ == OutputMode::DEPTH) {
-    suffix = "_depth";
+  if (output_mode_ != OutputMode::SYNCED) {
+    suffix = "_" + std::string(outputModeName(output_mode_));
   }
 
   auto indexed_filename = original_path.stem().string() + suffix;
diff --git a/orbbec_camera/src/jetson_nv_decoder.cpp b/orbbec_camera/src/jetson_nv_decoder.cpp
index 3ad1dfc4..68310bc7 100644
--- a/orbbec_camera/src/jetson_nv_decoder.cpp
+++ b/orbbec_camera/src/jetson_nv_decoder.cpp
@@ -14,22 +14,125 @@
  * limitations under the License.
  *******************************************************************************/
 #include "orbbec_camera/jetson_nv_decoder.h"
-#include <NvJpegDecoder.h>
-#include <NvV4l2Element.h>
-#include <algorithm>
+
+#include <linux/videodev2.h>
+#include <setjmp.h>
+#include <unistd.h>
+
+#include <NvBufSurface.h>
+#include <cstring>
+#include <jpeglib.h>
 #include <nvbufsurface.h>
 #include <nvbufsurftransform.h>
-#include <NvBufSurface.h>
-#include <fstream>
 #include <libyuv.h>
 #include <rclcpp/rclcpp.hpp>
+#include <v4l2_nv_extensions.h>
+
+#include "jpegint.h"
 #include "orbbec_camera/utils.h"
 
 namespace orbbec_camera {
+namespace {
 
-JetsonNvJPEGDecoder::JetsonNvJPEGDecoder(int width, int height) : JPEGDecoder(width, height) {}
+struct JpegErrorManager {
+  jpeg_error_mgr base;
+  jmp_buf jump_buffer;
+  char message[JMSG_LENGTH_MAX];
+};
 
-JetsonNvJPEGDecoder::~JetsonNvJPEGDecoder() { delete decoder_; }
+void jpegErrorExit(j_common_ptr cinfo) {
+  auto *error = reinterpret_cast<JpegErrorManager *>(cinfo->err);
+  (*cinfo->err->format_message)(cinfo, error->message);
+  longjmp(error->jump_buffer, 1);
+}
+
+}  // namespace
+
+class JetsonNvJPEGDecoder::Impl {
+ public:
+  Impl() {
+    std::memset(&cinfo_, 0, sizeof(cinfo_));
+    std::memset(&error_, 0, sizeof(error_));
+    cinfo_.err = jpeg_std_error(&error_.base);
+    error_.base.error_exit = jpegErrorExit;
+
+    if (setjmp(error_.jump_buffer) != 0) {
+      return;
+    }
+
+    jpeg_create_decompress(&cinfo_);
+    initialized_ = true;
+    cinfo_.mjpeg_decode = TRUE;
+  }
+
+  ~Impl() {
+    if (initialized_) {
+      jpeg_destroy_decompress(&cinfo_);
+    }
+  }
+
+  Impl(const Impl &) = delete;
+  Impl &operator=(const Impl &) = delete;
+
+  bool isInitialized() const { return initialized_; }
+
+  const char *lastError() const { return error_.message; }
+
+  int decodeToFd(int &fd, unsigned char *input, unsigned long input_size, uint32_t &pixfmt,
+                 uint32_t &width, uint32_t &height) {
+    if (!initialized_ || input == nullptr || input_size == 0) {
+      return -1;
+    }
+
+    error_.message[0] = '\0';
+    if (setjmp(error_.jump_buffer) != 0) {
+      return -1;
+    }
+
+    NvBufSurface surface;
+    cinfo_.out_color_space = JCS_YCbCr;
+    jpeg_mem_src(&cinfo_, input, input_size);
+
+    (void)jpeg_read_header(&cinfo_, TRUE);
+
+    cinfo_.out_color_space = JCS_YCbCr;
+    cinfo_.IsVendorbuf = TRUE;
+    cinfo_.pVendor_buf = reinterpret_cast<unsigned char *>(&surface);
+
+    uint32_t pixel_format = 0;
+    if (cinfo_.comp_info[0].h_samp_factor == 2) {
+      pixel_format =
+          cinfo_.comp_info[0].v_samp_factor == 2 ? V4L2_PIX_FMT_YUV420M : V4L2_PIX_FMT_YUV422M;
+    } else {
+      pixel_format =
+          cinfo_.comp_info[0].v_samp_factor == 1 ? V4L2_PIX_FMT_YUV444M : V4L2_PIX_FMT_YUV422RM;
+    }
+
+    jpeg_start_decompress(&cinfo_);
+    if (cinfo_.global_state != DSTATE_READY) {
+      return -1;
+    }
+
+    jpeg_read_raw_data(&cinfo_, nullptr, cinfo_.comp_info[0].v_samp_factor * DCTSIZE);
+    jpeg_finish_decompress(&cinfo_);
+
+    width = cinfo_.image_width % 2 == 1 ? cinfo_.image_width + 1 : cinfo_.image_width;
+    height = cinfo_.image_height % 2 == 1 ? cinfo_.image_height + 1 : cinfo_.image_height;
+    pixfmt = pixel_format;
+    fd = cinfo_.fd;
+    return 0;
+  }
+
+ private:
+  jpeg_decompress_struct cinfo_{};
+  JpegErrorManager error_{};
+  bool initialized_ = false;
+};
+
+JetsonNvJPEGDecoder::JetsonNvJPEGDecoder(int width, int height)
+    : JPEGDecoder(width, height), decoder_(std::make_unique<Impl>()) {}
+
+JetsonNvJPEGDecoder::~JetsonNvJPEGDecoder() = default;
 
 bool JetsonNvJPEGDecoder::decode(const std::shared_ptr<ob::ColorFrame> &frame, uint8_t *dest) {
   if (!isValidJPEG(frame)) {
@@ -44,10 +147,24 @@ bool JetsonNvJPEGDecoder::decode(const std::shared_ptr<ob::ColorFrame> &frame, u
   while (data_size > 4 && data[data_size - 1] == 0x00) {
     data_size--;
   }
+
+  if (!decoder_ || !decoder_->isInitialized()) {
+    decoder_ = std::make_unique<Impl>();
+  }
+  if (!decoder_->isInitialized()) {
+    RCLCPP_ERROR_STREAM(rclcpp::get_logger("jetson_nv_decoder"),
+                        "Failed to initialize NVIDIA JPEG decoder");
+    decoder_.reset();
+    return false;
+  }
+
   int fd = -1;
-  decoder_ = NvJPEGDecoder::createJPEGDecoder("jpegdec");
-  std::shared_ptr<int> decoder_deleter(nullptr, [&](int *) { delete decoder_; });
-  decoder_->decodeToFd(fd, data, data_size, pixfmt, width, height);
+  if (decoder_->decodeToFd(fd, data, data_size, pixfmt, width, height) != 0) {
+    RCLCPP_ERROR_STREAM(rclcpp::get_logger("jetson_nv_decoder"), "Failed to decode JPEG frame");
+    decoder_.reset();
+    return false;
+  }
+
   if (pixfmt != V4L2_PIX_FMT_YUV422M) {
     RCLCPP_ERROR_STREAM(rclcpp::get_logger("jetson_nv_decoder"), "Unexpected pixfmt: " << pixfmt);
     if (fd != -1) {
diff --git a/orbbec_camera/src/ob_camera_node.cpp b/orbbec_camera/src/ob_camera_node.cpp
index 917ba861..34ca616c 100644
--- a/orbbec_camera/src/ob_camera_node.cpp
+++ b/orbbec_camera/src/ob_camera_node.cpp
@@ -650,7 +650,9 @@ void OBCameraNode::publishDepthFiltersStatus() {
   if (disp_outliers_filter_supported) {
     append_unique_filter_name("DispOutliersFilter");
   }
-  append_unique_filter_name("EnhancedDepthFilter");
+  if (isGemini330SeriesPID(pid_)) {
+    append_unique_filter_name("EnhancedDepthFilter");
+  }
 
   msg.filters.reserve(ordered_filter_names.size());
   for (const auto &filter_name : ordered_filter_names) {
@@ -741,7 +743,11 @@ OBCameraNode::OBCameraNode(rclcpp::Node *node, std::shared_ptr<ob::Device> devic
     timestamp_config.csv_file_path = frame_timestamp_csv_file_;
     timestamp_config.frame_sync_enabled = enable_frame_sync_;
     timestamp_config.color_enabled = enable_stream_[COLOR];
+    timestamp_config.left_color_enabled = enable_stream_[COLOR_LEFT];
+    timestamp_config.right_color_enabled = enable_stream_[COLOR_RIGHT];
     timestamp_config.depth_enabled = enable_stream_[DEPTH];
+    timestamp_config.left_ir_enabled = enable_stream_[INFRA1];
+    timestamp_config.right_ir_enabled = enable_stream_[INFRA2];
     timestamp_config.imu_sync_enabled = enable_sync_output_accel_gyro_;
     timestamp_config.accel_enabled = enable_stream_[ACCEL];
     timestamp_config.gyro_enabled = enable_stream_[GYRO];
@@ -761,6 +767,8 @@ OBCameraNode::OBCameraNode(rclcpp::Node *node, std::shared_ptr<ob::Device> devic
   is_camera_node_initialized_ = true;
 
   fps_counter_color_ = std::make_unique<FpsCounter>("Color", logger_, 1);
+  fps_counter_left_color_ = std::make_unique<FpsCounter>("Left Color", logger_, 1);
+  fps_counter_right_color_ = std::make_unique<FpsCounter>("Right Color", logger_, 1);
   fps_counter_depth_ = std::make_unique<FpsCounter>("Depth", logger_, 1);
   fps_counter_left_ir_ = std::make_unique<FpsCounter>("Left Ir", logger_, 1);
   fps_counter_right_ir_ = std::make_unique<FpsCounter>("Right Ir", logger_, 1);
@@ -770,12 +778,18 @@ OBCameraNode::OBCameraNode(rclcpp::Node *node, std::shared_ptr<ob::Device> devic
     log_level = LogLevel::INFO;
   }
   fps_counter_color_->setLogLevel(log_level);
+  fps_counter_left_color_->setLogLevel(log_level);
+  fps_counter_right_color_->setLogLevel(log_level);
   fps_counter_depth_->setLogLevel(log_level);
   fps_counter_left_ir_->setLogLevel(log_level);
   fps_counter_right_ir_->setLogLevel(log_level);
 
   fps_delay_status_color_ = std::make_unique<FpsDelayStatus>(logger_);
+  fps_delay_status_left_color_ = std::make_unique<FpsDelayStatus>(logger_);
+  fps_delay_status_right_color_ = std::make_unique<FpsDelayStatus>(logger_);
   fps_delay_status_depth_ = std::make_unique<FpsDelayStatus>(logger_);
+  fps_delay_status_left_ir_ = std::make_unique<FpsDelayStatus>(logger_);
+  fps_delay_status_right_ir_ = std::make_unique<FpsDelayStatus>(logger_);
 }
 
 template <class T>
@@ -1652,24 +1666,38 @@ void OBCameraNode::setupDevices() {
         "Current color anti-flicker to "
             << (device_->getBoolProperty(OB_PROP_COLOR_ANTI_FLICKER_BOOL) ? "ON" : "OFF")));
   }
-  if (!color_powerline_freq_.empty() &&
-      device_->isPropertySupported(OB_PROP_COLOR_POWER_LINE_FREQUENCY_INT, OB_PERMISSION_WRITE)) {
-    if (color_powerline_freq_ == "disable") {
-      TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_POWER_LINE_FREQUENCY_INT, 0);
-    } else if (color_powerline_freq_ == "50hz") {
-      TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_POWER_LINE_FREQUENCY_INT, 1);
-    } else if (color_powerline_freq_ == "60hz") {
-      TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_POWER_LINE_FREQUENCY_INT, 2);
-    } else if (color_powerline_freq_ == "auto") {
-      TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_POWER_LINE_FREQUENCY_INT, 3);
+  if (!color_powerline_freq_.empty()) {
+    const auto normalized_color_powerline_freq = lowerParameterValue(color_powerline_freq_);
+    int color_powerline_freq_value = -1;
+    if (normalized_color_powerline_freq == "disable") {
+      color_powerline_freq_value = 0;
+    } else if (normalized_color_powerline_freq == "50hz") {
+      color_powerline_freq_value = 1;
+    } else if (normalized_color_powerline_freq == "60hz") {
+      color_powerline_freq_value = 2;
+    } else if (normalized_color_powerline_freq == "auto") {
+      color_powerline_freq_value = 3;
+    } else {
+      RCLCPP_WARN_STREAM(logger_,
+                         "Invalid parameter color_powerline_freq "
+                             << formatParameterValue(color_powerline_freq_) << ". Valid values: "
+                             << formatValidParameterValues({"disable", "50hz", "60hz", "auto"})
+                             << ". Skip setting.");
+      color_powerline_freq_.clear();
+    }
+    if (color_powerline_freq_value >= 0 &&
+        device_->isPropertySupported(OB_PROP_COLOR_POWER_LINE_FREQUENCY_INT, OB_PERMISSION_WRITE)) {
+      color_powerline_freq_ = normalized_color_powerline_freq;
+      TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_POWER_LINE_FREQUENCY_INT,
+                          color_powerline_freq_value);
+      TRY_EXECUTE_BLOCK({
+        const auto current_color_powerline_freq =
+            device_->getIntProperty(OB_PROP_COLOR_POWER_LINE_FREQUENCY_INT);
+        RCLCPP_INFO_STREAM(logger_,
+                           "Current color powerline freq: "
+                               << colorPowerLineFrequencyToString(current_color_powerline_freq));
+      });
     }
-    TRY_EXECUTE_BLOCK({
-      const auto current_color_powerline_freq =
-          device_->getIntProperty(OB_PROP_COLOR_POWER_LINE_FREQUENCY_INT);
-      RCLCPP_INFO_STREAM(logger_,
-                         "Current color powerline freq: "
-                             << colorPowerLineFrequencyToString(current_color_powerline_freq));
-    });
   }
   if (depth_exposure_ != -1 &&
       device_->isPropertySupported(OB_PROP_DEPTH_EXPOSURE_INT, OB_PERMISSION_WRITE)) {
@@ -1707,7 +1735,8 @@ void OBCameraNode::setupDevices() {
         "Current depth auto exposure priority: "
             << (device_->getIntProperty(OB_PROP_DEPTH_AUTO_EXPOSURE_PRIORITY_INT) ? "ON" : "OFF")));
   }
-  if (should_apply_launch_config("enable_ir_auto_exposure") &&
+  if ((should_apply_launch_config("enable_auto_exposure") ||
+       should_apply_launch_config("enable_ir_auto_exposure")) &&
       device_->isPropertySupported(OB_PROP_IR_AUTO_EXPOSURE_BOOL, OB_PERMISSION_WRITE)) {
     TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_IR_AUTO_EXPOSURE_BOOL, enable_ir_auto_exposure_);
     TRY_EXECUTE_BLOCK(RCLCPP_INFO_STREAM(
@@ -3220,7 +3249,7 @@ void OBCameraNode::setupLeftIrPostProcessFilter() {
   }
   auto device_info = device_->getDeviceInfo();
   CHECK_NOTNULL(device_info);
-  if (isGemini335PID(pid_)) {
+  if (isGemini330SeriesPID(pid_) || isGemini301SeriesPID(pid_)) {
     auto left_ir_sensor = device_->getSensor(OB_SENSOR_IR_LEFT);
     left_ir_filter_list_ = left_ir_sensor->createRecommendedFilters();
     if (left_ir_filter_list_.empty()) {
@@ -3261,7 +3290,7 @@ void OBCameraNode::setupRightIrPostProcessFilter() {
   }
   auto device_info = device_->getDeviceInfo();
   CHECK_NOTNULL(device_info);
-  if (isGemini335PID(pid_)) {
+  if (isGemini330SeriesPID(pid_) || isGemini301SeriesPID(pid_)) {
     auto right_ir_sensor = device_->getSensor(OB_SENSOR_IR_RIGHT);
     right_ir_filter_list_ = right_ir_sensor->createRecommendedFilters();
     if (right_ir_filter_list_.empty()) {
@@ -3520,35 +3549,44 @@ void OBCameraNode::selectBaseStream() {
 
 void OBCameraNode::printSensorProfiles(const std::shared_ptr<ob::Sensor> &sensor) {
   auto profiles = sensor->getStreamProfileList();
+  const auto sensor_type = sensor->getType();
   for (size_t i = 0; i < profiles->getCount(); i++) {
     auto origin_profile = profiles->getProfile(i);
-    if (sensor->getType() == OB_SENSOR_COLOR) {
+    if (sensor_type == OB_SENSOR_COLOR || sensor_type == OB_SENSOR_COLOR_LEFT ||
+        sensor_type == OB_SENSOR_COLOR_RIGHT) {
       auto profile = origin_profile->as<ob::VideoStreamProfile>();
-      RCLCPP_INFO_STREAM(
-          logger_, "color profile: " << profile->getWidth() << "x" << profile->getHeight() << " "
-                                     << profile->getFps() << "fps " << profile->getFormat());
-    } else if (sensor->getType() == OB_SENSOR_DEPTH) {
+      const char *stream_name = sensor_type == OB_SENSOR_COLOR_LEFT    ? "left_color"
+                                : sensor_type == OB_SENSOR_COLOR_RIGHT ? "right_color"
+                                                                       : "color";
+      RCLCPP_INFO_STREAM(logger_, stream_name << " profile: " << profile->getWidth() << "x"
+                                              << profile->getHeight() << " " << profile->getFps()
+                                              << "fps " << profile->getFormat());
+    } else if (sensor_type == OB_SENSOR_DEPTH) {
       auto profile = origin_profile->as<ob::VideoStreamProfile>();
       RCLCPP_INFO_STREAM(
           logger_, "depth profile: " << profile->getWidth() << "x" << profile->getHeight() << " "
                                      << profile->getFps() << "fps " << profile->getFormat());
-    } else if (sensor->getType() == OB_SENSOR_IR) {
+    } else if (sensor_type == OB_SENSOR_IR || sensor_type == OB_SENSOR_IR_LEFT ||
+               sensor_type == OB_SENSOR_IR_RIGHT) {
       auto profile = origin_profile->as<ob::VideoStreamProfile>();
-      RCLCPP_INFO_STREAM(logger_, "ir profile: " << profile->getWidth() << "x"
-                                                 << profile->getHeight() << " " << profile->getFps()
-                                                 << "fps " << profile->getFormat());
-    } else if (sensor->getType() == OB_SENSOR_ACCEL) {
+      const char *stream_name = sensor_type == OB_SENSOR_IR_LEFT    ? "left_ir"
+                                : sensor_type == OB_SENSOR_IR_RIGHT ? "right_ir"
+                                                                    : "ir";
+      RCLCPP_INFO_STREAM(logger_, stream_name << " profile: " << profile->getWidth() << "x"
+                                              << profile->getHeight() << " " << profile->getFps()
+                                              << "fps " << profile->getFormat());
+    } else if (sensor_type == OB_SENSOR_ACCEL) {
       auto profile = origin_profile->as<ob::AccelStreamProfile>();
       RCLCPP_INFO_STREAM(logger_, "accel profile: sampleRate " << profile->getSampleRate()
                                                                << "  full scale_range "
                                                                << profile->getFullScaleRange());
-    } else if (sensor->getType() == OB_SENSOR_GYRO) {
+    } else if (sensor_type == OB_SENSOR_GYRO) {
       auto profile = origin_profile->as<ob::GyroStreamProfile>();
       RCLCPP_INFO_STREAM(logger_, "gyro profile: sampleRate " << profile->getSampleRate()
                                                               << "  full scale_range "
                                                               << profile->getFullScaleRange());
     } else {
-      RCLCPP_INFO_STREAM(logger_, "unknown profile: " << magic_enum::enum_name(sensor->getType()));
+      RCLCPP_INFO_STREAM(logger_, "unknown profile: " << magic_enum::enum_name(sensor_type));
     }
   }
 }
@@ -3571,33 +3609,30 @@ void OBCameraNode::setupProfiles() {
         if (profile == nullptr) {
           throw std::runtime_error("Failed cast profile to VideoStreamProfile");
         }
-        RCLCPP_DEBUG_STREAM(
-            logger_, "Sensor profile: "
-                         << "stream_type: " << magic_enum::enum_name(profile->getType())
-                         << "Format: " << profile->getFormat() << ", Width: " << profile->getWidth()
-                         << ", Height: " << profile->getHeight() << ", FPS: " << profile->getFps());
         supported_profiles_[elem].emplace_back(profile);
       }
       std::shared_ptr<ob::VideoStreamProfile> selected_profile;
       std::shared_ptr<ob::VideoStreamProfile> default_profile;
       try {
-        if (width_[elem] == 0 && height_[elem] == 0 && fps_[elem] == 0 &&
-            format_[elem] == OB_FORMAT_UNKNOWN) {
+        if (is_playback_device_) {
+          selected_profile = profiles->getProfile(0)->as<ob::VideoStreamProfile>();
+        } else if (width_[elem] == 0 && height_[elem] == 0 && fps_[elem] == 0 &&
+                   format_[elem] == OB_FORMAT_UNKNOWN) {
           selected_profile = profiles->getProfile(0)->as<ob::VideoStreamProfile>();
         } else {
-          if (isGemini305SeriesPID(pid_) && elem == DEPTH) {
+          if (isGemini301SeriesPID(pid_) && elem == DEPTH) {
             OBHardwareDecimationConfig conf;
             conf.originWidth = width_[elem];
             conf.originHeight = height_[elem];
             conf.factor = depth_decimation_factor_;
             selected_profile = profiles->getVideoStreamProfile(conf, format_[elem], fps_[elem]);
-          } else if (isGemini305SeriesPID(pid_) && elem == INFRA1) {
+          } else if (isGemini301SeriesPID(pid_) && elem == INFRA1) {
             OBHardwareDecimationConfig conf;
             conf.originWidth = width_[elem];
             conf.originHeight = height_[elem];
             conf.factor = left_ir_decimation_factor_;
             selected_profile = profiles->getVideoStreamProfile(conf, format_[elem], fps_[elem]);
-          } else if (isGemini305SeriesPID(pid_) && elem == INFRA2) {
+          } else if (isGemini301SeriesPID(pid_) && elem == INFRA2) {
             OBHardwareDecimationConfig conf;
             conf.originWidth = width_[elem];
             conf.originHeight = height_[elem];
@@ -3651,21 +3686,35 @@ void OBCameraNode::setupProfiles() {
       width_[elem] = static_cast<int>(selected_profile->getWidth());
       fps_[elem] = static_cast<int>(selected_profile->getFps());
       format_[elem] = selected_profile->getFormat();
+      if (is_playback_device_) {
+        format_str_[elem] = OBFormatToString(format_[elem]);
+        RCLCPP_INFO_STREAM(logger_, "Bag playback: using recorded "
+                                        << stream_name_[elem] << " profile " << width_[elem] << "x"
+                                        << height_[elem] << " " << fps_[elem] << "fps "
+                                        << format_str_[elem]);
+      }
       updateImageConfig(elem);
       if (selected_profile->format() == OB_FORMAT_BGRA) {
         images_[elem] = cv::Mat(height_[elem], width_[elem], CV_8UC4, cv::Scalar(0, 0, 0, 0));
         encoding_[elem] = sensor_msgs::image_encodings::BGRA8;
-        unit_step_size_[COLOR] = 4 * sizeof(uint8_t);
+        unit_step_size_[elem] = 4 * sizeof(uint8_t);
       } else if (selected_profile->format() == OB_FORMAT_RGBA) {
         images_[elem] = cv::Mat(height_[elem], width_[elem], CV_8UC4, cv::Scalar(0, 0, 0, 0));
         encoding_[elem] = sensor_msgs::image_encodings::RGBA8;
-        unit_step_size_[COLOR] = 4 * sizeof(uint8_t);
+        unit_step_size_[elem] = 4 * sizeof(uint8_t);
       } else {
         images_[elem] =
             cv::Mat(height_[elem], width_[elem], image_format_[elem], cv::Scalar(0, 0, 0));
       }
     }
   }
+
+  std::string stream_fps_message;
+  if (!validate301SeriesStreamFrameRates(fps_, stream_fps_message)) {
+    RCLCPP_ERROR_STREAM(logger_, stream_fps_message);
+    throw std::runtime_error(stream_fps_message);
+  }
+
   // IMU
   for (const auto &stream_index : HID_STREAMS) {
     if (!enable_stream_[stream_index]) {
@@ -3673,7 +3722,20 @@ void OBCameraNode::setupProfiles() {
     }
     try {
       auto profile_list = sensors_[stream_index]->getStreamProfileList();
-      if (stream_index == ACCEL) {
+      if (is_playback_device_) {
+        stream_profile_[stream_index] = profile_list->getProfile(0);
+        if (stream_index == ACCEL) {
+          auto profile = stream_profile_[stream_index]->as<ob::AccelStreamProfile>();
+          CHECK_NOTNULL(profile.get());
+          imu_range_[stream_index] = fullAccelScaleRangeToString(profile->getFullScaleRange());
+          imu_rate_[stream_index] = sampleRateToString(profile->getSampleRate());
+        } else if (stream_index == GYRO) {
+          auto profile = stream_profile_[stream_index]->as<ob::GyroStreamProfile>();
+          CHECK_NOTNULL(profile.get());
+          imu_range_[stream_index] = fullGyroScaleRangeToString(profile->getFullScaleRange());
+          imu_rate_[stream_index] = sampleRateToString(profile->getSampleRate());
+        }
+      } else if (stream_index == ACCEL) {
         auto full_scale_range = fullAccelScaleRangeFromString(imu_range_[stream_index]);
         auto sample_rate = sampleRateFromString(imu_rate_[stream_index]);
         auto profile = profile_list->getAccelStreamProfile(full_scale_range, sample_rate);
@@ -3697,6 +3759,55 @@ void OBCameraNode::setupProfiles() {
   }
 }
 
+bool OBCameraNode::validate301SeriesStreamFrameRates(const std::map<stream_index_pair, int> &fps,
+                                                     std::string &message) const {
+  if (!isGemini301SeriesPID(pid_)) {
+    return true;
+  }
+
+  int active_fps = 0;
+  bool fps_mismatch = false;
+  std::string active_streams;
+  for (const auto &stream_index : IMAGE_STREAMS) {
+    if (stream_index.first == OB_STREAM_LIDAR) {
+      continue;
+    }
+    const auto enable_it = enable_stream_.find(stream_index);
+    const auto fps_it = fps.find(stream_index);
+    if (enable_it == enable_stream_.end() || !enable_it->second || fps_it == fps.end() ||
+        fps_it->second <= 0) {
+      continue;
+    }
+
+    if (!active_streams.empty()) {
+      active_streams += ", ";
+    }
+    const auto name_it = stream_name_.find(stream_index);
+    if (name_it != stream_name_.end()) {
+      active_streams += name_it->second;
+    } else {
+      active_streams += std::string(magic_enum::enum_name(stream_index.first));
+    }
+    active_streams += "=" + std::to_string(fps_it->second);
+
+    if (active_fps == 0) {
+      active_fps = fps_it->second;
+    } else if (active_fps != fps_it->second) {
+      fps_mismatch = true;
+    }
+  }
+
+  if (!fps_mismatch) {
+    return true;
+  }
+
+  message =
+      "Gemini 301 series requires the same FPS for all enabled image streams. "
+      "Active stream FPS: " +
+      active_streams + ". Set all enabled image streams to the same FPS or disable unused streams.";
+  return false;
+}
+
 std::shared_ptr<ob::VideoStreamProfile> OBCameraNode::selectVideoStreamProfile(
     const stream_index_pair &stream_index, int width, int height, int fps, OBFormat format) {
   auto sensor_it = sensors_.find(stream_index);
@@ -3712,19 +3823,19 @@ std::shared_ptr<ob::VideoStreamProfile> OBCameraNode::selectVideoStreamProfile(
   std::shared_ptr<ob::VideoStreamProfile> selected_profile;
   if (width == 0 && height == 0 && fps == 0) {
     selected_profile = profiles->getProfile(0)->as<ob::VideoStreamProfile>();
-  } else if (isGemini305SeriesPID(pid_) && stream_index == DEPTH) {
+  } else if (!is_playback_device_ && isGemini301SeriesPID(pid_) && stream_index == DEPTH) {
     OBHardwareDecimationConfig conf;
     conf.originWidth = width;
     conf.originHeight = height;
     conf.factor = depth_decimation_factor_;
     selected_profile = profiles->getVideoStreamProfile(conf, format, fps);
-  } else if (isGemini305SeriesPID(pid_) && stream_index == INFRA1) {
+  } else if (!is_playback_device_ && isGemini301SeriesPID(pid_) && stream_index == INFRA1) {
     OBHardwareDecimationConfig conf;
     conf.originWidth = width;
     conf.originHeight = height;
     conf.factor = left_ir_decimation_factor_;
     selected_profile = profiles->getVideoStreamProfile(conf, format, fps);
-  } else if (isGemini305SeriesPID(pid_) && stream_index == INFRA2) {
+  } else if (!is_playback_device_ && isGemini301SeriesPID(pid_) && stream_index == INFRA2) {
     OBHardwareDecimationConfig conf;
     conf.originWidth = width;
     conf.originHeight = height;
@@ -3851,6 +3962,16 @@ bool OBCameraNode::validateStreamProfileRequest(
       return false;
     }
   }
+
+  auto requested_fps = fps_;
+  for (const auto &pending_profile : pending_profiles) {
+    requested_fps[pending_profile.stream_index] =
+        static_cast<int>(pending_profile.profile->getFps());
+  }
+  if (!validate301SeriesStreamFrameRates(requested_fps, message)) {
+    return false;
+  }
+
   if (!has_changes) {
     message = "requested stream profiles are already active";
     return false;
@@ -4758,7 +4879,11 @@ void OBCameraNode::getParameters() {
   setAndGetNodeParameter<int>(mean_intensity_set_point_, "mean_intensity_set_point",
                               depth_brightness_);
   setAndGetNodeParameter<std::string>(depth_precision_str_, "depth_precision", "");
-  setAndGetNodeParameter<bool>(enable_ir_auto_exposure_, "enable_ir_auto_exposure", true);
+  setAndGetNodeParameter<bool>(enable_ir_auto_exposure_,
+                               isLaunchParamProvided("enable_auto_exposure")
+                                   ? "enable_auto_exposure"
+                                   : "enable_ir_auto_exposure",
+                               true);
   setAndGetNodeParameter<int>(ir_exposure_, "ir_exposure", -1);
   setAndGetNodeParameter<int>(ir_gain_, "ir_gain", -1);
   setAndGetNodeParameter<int>(ir_ae_max_exposure_, "ir_ae_max_exposure", -1);
@@ -5274,6 +5399,11 @@ bool OBCameraNode::validateEnhancedDepthFilterConfig(std::string &message) const
   constexpr char kEnhancedDepthSupportedTargetResolutions[] = "640x480/1280x720/1280x800";
   constexpr char kEnhancedDepthSupportedDepthFormats[] = "Y10/Y11/Y12/Y14/Y16/Z16";
 
+  if (!isGemini330SeriesPID(pid_)) {
+    message = "Enhanced depth filter is only supported by Gemini 330 series devices";
+    return false;
+  }
+
   if (!enable_stream_.count(COLOR) || !enable_stream_.at(COLOR) || !enable_stream_.count(DEPTH) ||
       !enable_stream_.at(DEPTH)) {
     message = "Enhanced depth filter requires color and depth streams";
@@ -5792,6 +5922,11 @@ void OBCameraNode::syncSoftwareAlignment() {
       align_filter_ = std::make_unique<ob::Align>(align_target_stream_);
       RCLCPP_INFO_STREAM(logger_, "set align mode to " << align_mode_);
     }
+    if (align_target_stream_ != OB_STREAM_COLOR) {
+      releaseGlobalImageTransportPublisher(*node_, "depth/image_unaligned");
+      depth_unaligned_publisher_.reset();
+      return;
+    }
     if (!depth_unaligned_publisher_) {
       const auto depth_image_qos_profile = getImageQosProfile(DEPTH);
       if (use_intra_process_) {
@@ -5890,7 +6025,7 @@ cv::Mat OBCameraNode::colorizeDepthImage(const cv::Mat &depth_image,
   cv::Mat depth_16u;
   depth_image.convertTo(depth_16u, CV_16UC1);
 
-  const uint16_t min_depth = isGemini305SeriesPID(pid_) ? kViewerColorizerG305MinDistanceMm
+  const uint16_t min_depth = isGemini301SeriesPID(pid_) ? kViewerColorizerG305MinDistanceMm
                                                         : kViewerColorizerDefaultMinDistanceMm;
   const uint16_t max_depth = kViewerColorizerMaxDistanceMm;
   const uint32_t value_range = static_cast<uint32_t>(max_depth) - min_depth + 1;
@@ -6036,7 +6171,9 @@ void OBCameraNode::publishDepthPointCloud(const std::shared_ptr<ob::FrameSet> &f
   }
   auto frame_timestamp = getFrameTimestampUs(depth_frame);
   auto timestamp = fromUsToROSTime(frame_timestamp);
-  std::string frame_id = depth_registration_ ? optical_frame_id_[COLOR] : optical_frame_id_[DEPTH];
+  std::string frame_id = depth_registration_ && align_target_stream_ == OB_STREAM_COLOR
+                             ? depth_aligned_frame_id_[DEPTH]
+                             : optical_frame_id_[DEPTH];
   if (!cloud_frame_id_.empty()) {
     frame_id = cloud_frame_id_;
   }
@@ -6351,7 +6488,7 @@ void OBCameraNode::setDepthAutoExposureROI() {
   if (depth_roi_has_run) {
     return;
   }
-  if (isGemini305SeriesPID(pid_) && ae_reference_stream_ == "color") {
+  if (isGemini301SeriesPID(pid_) && ae_reference_stream_ == "color") {
     RCLCPP_WARN_STREAM(logger_, "Skip setting depth AE ROI because AE Reference Stream is color");
     depth_roi_has_run = true;
     return;
@@ -6396,7 +6533,7 @@ void OBCameraNode::setColorAutoExposureROI() {
   if (color_roi_has_run) {
     return;
   }
-  if (isGemini305SeriesPID(pid_) && ae_reference_stream_ == "depth") {
+  if (isGemini301SeriesPID(pid_) && ae_reference_stream_ == "depth") {
     RCLCPP_WARN_STREAM(logger_, "Skip setting color AE ROI because AE Reference Stream is depth");
     color_roi_has_run = true;
     return;
@@ -6473,6 +6610,20 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set
         final_color_frame, final_depth_frame, frame_set_arrival_system_us,
         frame_set_arrival_steady_us, track_color, track_depth, color_publish_expected,
         depth_publish_expected);
+
+    const auto record_side_stream = [&](const stream_index_pair &stream_index,
+                                        OBFrameType frame_type) {
+      auto frame = frame_set->getFrame(frame_type);
+      if (enable_stream_[stream_index] && frame) {
+        timestamp_csv_logger_->recordImageFrameArrival(stream_index.first, frame,
+                                                       frame_set_arrival_system_us,
+                                                       frame_set_arrival_steady_us, true);
+      }
+    };
+    record_side_stream(COLOR_LEFT, OB_FRAME_COLOR_LEFT);
+    record_side_stream(COLOR_RIGHT, OB_FRAME_COLOR_RIGHT);
+    record_side_stream(INFRA1, OB_FRAME_IR_LEFT);
+    record_side_stream(INFRA2, OB_FRAME_IR_RIGHT);
   }
 
   try {
@@ -6514,10 +6665,12 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set
       setColorAutoExposureROI();
       left_color_frame = processColorFrameFilter(left_color_frame);
       frame_set->pushFrame(left_color_frame);
+      fps_counter_left_color_->tick();
     }
     if (right_color_frame) {
       right_color_frame = processColorFrameFilter(right_color_frame);
       frame_set->pushFrame(right_color_frame);
+      fps_counter_right_color_->tick();
     }
     if (left_ir_frame) {
       left_ir_frame = processLeftIrFrameFilter(left_ir_frame);
@@ -6536,11 +6689,12 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set
       }
     }
     if (depth_registration_ && align_filter_ && depth_frame) {
-      publishRawDepthImage(depth_frame);
-      auto target_frame_type = STREAM_TYPE_TO_FRAME_TYPE.at(align_target_stream_);
-      if (!frame_set->getFrame(target_frame_type) || !color_frame) {
-        RCLCPP_DEBUG_STREAM(
-            logger_, "Depth registration requires depth and color frames, skip software alignment");
+      if (align_target_stream_ == OB_STREAM_COLOR) {
+        publishRawDepthImage(depth_frame);
+      }
+      if (!color_frame) {
+        RCLCPP_DEBUG_STREAM(logger_, "Software alignment requires a color frame, skip frame set");
+        return;
       } else {
         auto align_color_frame = color_frame;
         if (align_target_stream_ == OB_STREAM_DEPTH) {
@@ -6563,12 +6717,13 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set
           }
           if (!align_color_frame) {
             RCLCPP_ERROR_STREAM(logger_, "Failed to convert color frame for C2D alignment");
+            return;
           } else if (align_color_frame != color_frame) {
             color_frame = align_color_frame;
             frame_set->pushFrame(color_frame);
           }
         }
-        if (align_color_frame) {
+        if (align_target_stream_ != OB_STREAM_DEPTH || align_color_frame) {
           if (auto new_frame = align_filter_->process(frame_set)) {
             auto new_frame_set = new_frame->as<ob::FrameSet>();
             CHECK_NOTNULL(new_frame_set.get());
@@ -6583,8 +6738,8 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set
       }
     } else {
       RCLCPP_DEBUG_ONCE(logger_,
-                        "Depth registration is disabled or align filter is null or depth frame is "
-                        "null or color frame is null");
+                        "Depth registration is disabled, align filter is null, or depth frame is "
+                        "null");
     }
 
     if (enable_enhanced_depth_.load()) {
@@ -7035,6 +7190,7 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
       if (video_frame->getMetadataValue(OB_FRAME_METADATA_TYPE_HDR_SEQUENCE_INDEX) ==
           interleave_skip_index_) {
         RCLCPP_DEBUG(logger_, "interleave filter skip frame type: %d", frame->getType());
+        record_image_publish_skipped();
         return;
       }
     }
@@ -7077,7 +7233,7 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
     distortion = camera_params.rgbDistortion;
   }
   std::string frame_id = optical_frame_id_[stream_index];
-  if (depth_registration_ && stream_index == DEPTH) {
+  if (depth_registration_ && align_target_stream_ == OB_STREAM_COLOR && stream_index == DEPTH) {
     frame_id = depth_aligned_frame_id_[stream_index];
   }
   sensor_msgs::msg::CameraInfo camera_info{};
@@ -7117,13 +7273,19 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
   }
   if ((stream_index == COLOR || stream_index == COLOR_LEFT || stream_index == COLOR_RIGHT) &&
       frame->getFormat() == OB_FORMAT_MJPG && has_compressed_image_subscriber) {
-    if (!has_raw_image_subscriber && stream_index == COLOR && log_image_timestamps) {
+    if (!has_raw_image_subscriber && log_image_timestamps) {
       timestamp_csv_logger_->recordImagePrePublish(stream_index.first, frame, getSystemNowUs(),
                                                    getSteadyNowUs());
     }
     publishCompressedColorImage(frame, stream_index, timestamp, frame_id);
-    if (!has_raw_image_subscriber && stream_index == COLOR) {
-      fps_delay_status_color_->tick(frame_timestamp);
+    if (!has_raw_image_subscriber) {
+      if (stream_index == COLOR) {
+        fps_delay_status_color_->tick(frame_timestamp);
+      } else if (stream_index == COLOR_LEFT) {
+        fps_delay_status_left_color_->tick(frame_timestamp);
+      } else {
+        fps_delay_status_right_color_->tick(frame_timestamp);
+      }
     }
   }
 
@@ -7148,10 +7310,12 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
     }
     if (frame->getType() == OB_FRAME_COLOR_LEFT && !is_left_color_frame_decoded_) {
       RCLCPP_ERROR(logger_, "left color frame is not decoded");
+      record_image_publish_skipped();
       return;
     }
     if (frame->getType() == OB_FRAME_COLOR_RIGHT && !is_right_color_frame_decoded_) {
       RCLCPP_ERROR(logger_, "right color frame is not decoded");
+      record_image_publish_skipped();
       return;
     }
     if (frame->getType() == OB_FRAME_COLOR) {
@@ -7211,8 +7375,16 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
     }
     if (stream_index == COLOR) {
       fps_delay_status_color_->tick(frame_timestamp);
+    } else if (stream_index == COLOR_LEFT) {
+      fps_delay_status_left_color_->tick(frame_timestamp);
+    } else if (stream_index == COLOR_RIGHT) {
+      fps_delay_status_right_color_->tick(frame_timestamp);
     } else if (stream_index == DEPTH) {
       fps_delay_status_depth_->tick(frame_timestamp);
+    } else if (stream_index == INFRA1) {
+      fps_delay_status_left_ir_->tick(frame_timestamp);
+    } else if (stream_index == INFRA2) {
+      fps_delay_status_right_ir_->tick(frame_timestamp);
     }
     image_publishers_[stream_index]->publish(std::move(image_msg));
   }
@@ -7917,18 +8089,9 @@ bool OBCameraNode::setupFormatConvertType(OBFormat format, ob::FormatConvertFilt
   return true;
 }
 
-bool OBCameraNode::isGemini335PID(uint32_t pid) {
-  return pid == GEMINI_335_PID || pid == GEMINI_330_PID || pid == GEMINI_336_PID ||
-         pid == GEMINI_335L_PID || pid == GEMINI_330L_PID || pid == GEMINI_336L_PID ||
-         pid == GEMINI_335LG_PID || pid == GEMINI_336LG_PID || pid == GEMINI_335LE_PID ||
-         pid == GEMINI_336LE_PID || pid == CUSTOM_ADVANTECH_GEMINI_336_PID ||
-         pid == CUSTOM_ADVANTECH_GEMINI_336L_PID || pid == GEMINI_338_PID ||
-         pid == GEMINI_338L_PID || pid == GEMINI_338LE_PID || pid == GEMINI_338LG_PID;
-}
-
 bool OBCameraNode::isGemini435LePID(uint32_t pid) { return pid == GEMINI_435Le_PID; }
 bool OBCameraNode::isPublishMetaData(uint32_t pid) {
-  return isGemini335PID(pid) || isGemini435LePID(pid) || isGemini305SeriesPID(pid);
+  return isGemini330SeriesPID(pid) || isGemini435LePID(pid) || isGemini301SeriesPID(pid);
 }
 
 bool OBCameraNode::isDabaiASeriesForHwD2C(uint32_t pid) {
@@ -7938,7 +8101,7 @@ bool OBCameraNode::isDabaiASeriesForHwD2C(uint32_t pid) {
 
 bool OBCameraNode::isDepthWorkModeDevices(uint32_t pid) { return pid == GEMINI_435Le_PID; }
 
-bool OBCameraNode::isnotLaserDevices(uint32_t pid) { return isGemini305SeriesPID(pid); }
+bool OBCameraNode::isnotLaserDevices(uint32_t pid) { return isGemini301SeriesPID(pid); }
 
 orbbec_camera_msgs::msg::IMUInfo OBCameraNode::createIMUInfo(
     const stream_index_pair &stream_index) {
@@ -8265,6 +8428,11 @@ bool OBCameraNode::applyEnhancedDepthFilterConfig(
     bool enabled, const std::vector<float> &positional_params,
     const std::vector<orbbec_camera_msgs::msg::DepthFilterParam> &named_params,
     std::string &message) {
+  if (!isGemini330SeriesPID(pid_)) {
+    message = "Enhanced depth filter is only supported by Gemini 330 series devices";
+    return false;
+  }
+
   if (positional_params.size() > 1) {
     message = "EnhancedDepthFilter only supports one positional parameter";
     return false;
diff --git a/orbbec_camera/src/ob_camera_node_driver.cpp b/orbbec_camera/src/ob_camera_node_driver.cpp
index a22d6d09..db77106e 100644
--- a/orbbec_camera/src/ob_camera_node_driver.cpp
+++ b/orbbec_camera/src/ob_camera_node_driver.cpp
@@ -1232,10 +1232,9 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &dev
   CHECK_NOTNULL(device_info_.get());
   device_unique_id_ = device_info_->getUid();
 
-  if (enable_sync_host_time_ && !isOpenNIDevice(device_info_->pid()) && device_type_ == "camera" &&
-      !playback_device_) {
+  if (!isOpenNIDevice(device_info_->pid()) && device_type_ == "camera" && !playback_device_) {
     TRY_EXECUTE_BLOCK(device_->timerSyncWithHost());
-    if (g_time_domain != "global") {
+    if (enable_sync_host_time_ && g_time_domain != "global") {
       device_->enableGlobalTimestamp(false);
       sync_host_time_timer_ = this->create_wall_timer(time_sync_period_, [this]() {
         // Multiple safety checks before attempting time sync
@@ -1373,7 +1372,7 @@ void OBCameraNodeDriver::initializeDevice(const std::shared_ptr<ob::Device> &dev
   }
 
   const bool should_delay_stream_start = delay_stream_start_after_reconnect_.exchange(false) &&
-                                         isGemini305SeriesPID(device_info_->getPid());
+                                         isGemini301SeriesPID(device_info_->getPid());
   if (should_delay_stream_start) {
     std::this_thread::sleep_for(kStreamStartDelayAfterReconnect);
   }
@@ -1605,8 +1604,8 @@ void OBCameraNodeDriver::startDevice(const std::shared_ptr<ob::DeviceList> &list
     if (isGmslCameraPID(pid)) {
       ob_camera_node_->startGmslTrigger();
     }
-    // if (isGemini305SeriesPID(pid)) {
-    //   // Fixing 305 series hot-swap not outputting power
+    // if (isGemini301SeriesPID(pid)) {
+    //   // Fixing 301 series hot-swap not outputting power
     //   ob_camera_node_->startStreams();
     // }
   } catch (ob::Error &e) {
diff --git a/orbbec_camera/src/ros_service.cpp b/orbbec_camera/src/ros_service.cpp
index 95ee5d86..26995f45 100644
--- a/orbbec_camera/src/ros_service.cpp
+++ b/orbbec_camera/src/ros_service.cpp
@@ -217,11 +217,11 @@ void OBCameraNode::setupCameraCtrlServices() {
         });
   }
   if (isPropertyWritable(device_, OB_PROP_FLOOD_BOOL)) {
-    set_floor_enable_srv_ = node_->create_service<SetBool>(
-        "set_floor_enable", [this](const std::shared_ptr<rmw_request_id_t> request_header,
+    set_flood_enable_srv_ = node_->create_service<SetBool>(
+        "set_flood_enable", [this](const std::shared_ptr<rmw_request_id_t> request_header,
                                    const std::shared_ptr<SetBool::Request> request,
                                    std::shared_ptr<SetBool::Response> response) {
-          setFloorEnableCallback(request_header, request, response);
+          setFloodEnableCallback(request_header, request, response);
         });
   }
   if (isPropertyWritable(device_, OB_PROP_LASER_CONTROL_INT) ||
@@ -903,6 +903,8 @@ void OBCameraNode::setImageRegistrationModeCallback(
 
   auto rollback_after_error = [&](const std::string& error_message) {
     try {
+      stopColorFrameThreads();
+      clearColorFrameQueues();
       restore_old_mode();
       if (was_running && !pipeline_started_.load()) {
         startStreams();
@@ -924,6 +926,8 @@ void OBCameraNode::setImageRegistrationModeCallback(
     if (was_running) {
       stopStreams();
     }
+    stopColorFrameThreads();
+    clearColorFrameQueues();
 
     apply_image_registration_mode(mode);
 
@@ -1124,13 +1128,13 @@ void OBCameraNode::setAeRoiCallback(const std::shared_ptr<SetArrays ::Request>&
                                     std::shared_ptr<SetArrays::Response>& response,
                                     const stream_index_pair& stream_index) {
   auto stream = stream_index.first;
-  if (isGemini305SeriesPID(device_->getDeviceInfo()->getPid()) &&
+  if (isGemini301SeriesPID(device_->getDeviceInfo()->getPid()) &&
       (stream != OB_STREAM_COLOR && ae_reference_stream_ == "color")) {
     response->success = false;
     response->message = "AE Reference Stream is color, other sensors setting is not supported";
     return;
   }
-  if (isGemini305SeriesPID(device_->getDeviceInfo()->getPid()) &&
+  if (isGemini301SeriesPID(device_->getDeviceInfo()->getPid()) &&
       (stream != OB_STREAM_DEPTH && ae_reference_stream_ == "depth")) {
     response->success = false;
     response->message =
@@ -1530,15 +1534,15 @@ void OBCameraNode::setFanWorkModeCallback(const std::shared_ptr<SetInt32::Reques
   }
 }
 
-void OBCameraNode::setFloorEnableCallback(
+void OBCameraNode::setFloodEnableCallback(
     const std::shared_ptr<rmw_request_id_t>& request_header,
     const std::shared_ptr<std_srvs::srv::SetBool::Request>& request,
     std::shared_ptr<std_srvs::srv::SetBool::Response>& response) {
   (void)request_header;
   (void)response;
-  bool floor_enable = request->data;
+  bool flood_enable = request->data;
   try {
-    device_->setBoolProperty(OB_PROP_FLOOD_BOOL, floor_enable);
+    device_->setBoolProperty(OB_PROP_FLOOD_BOOL, flood_enable);
     response->success = true;
   } catch (const ob::Error& e) {
     response->success = false;
diff --git a/orbbec_camera/src/timestamp_csv_logger.cpp b/orbbec_camera/src/timestamp_csv_logger.cpp
index acd4f150..987abdc1 100644
--- a/orbbec_camera/src/timestamp_csv_logger.cpp
+++ b/orbbec_camera/src/timestamp_csv_logger.cpp
@@ -29,6 +29,18 @@ TimestampCsvLogger::TimestampCsvLogger(Config config, rclcpp::Logger logger)
       depth_logger_ = create_image_logger(FrameTimestampCsvLogger::OutputMode::DEPTH);
     }
   }
+  if (config.left_color_enabled) {
+    left_color_logger_ = create_image_logger(FrameTimestampCsvLogger::OutputMode::LEFT_COLOR);
+  }
+  if (config.right_color_enabled) {
+    right_color_logger_ = create_image_logger(FrameTimestampCsvLogger::OutputMode::RIGHT_COLOR);
+  }
+  if (config.left_ir_enabled) {
+    left_ir_logger_ = create_image_logger(FrameTimestampCsvLogger::OutputMode::LEFT_IR);
+  }
+  if (config.right_ir_enabled) {
+    right_ir_logger_ = create_image_logger(FrameTimestampCsvLogger::OutputMode::RIGHT_IR);
+  }
 
   if (config.csv_file_path.empty()) {
     return;
@@ -64,7 +76,12 @@ bool TimestampCsvLogger::enabled() const {
 
 bool TimestampCsvLogger::imageEnabled() const {
   return (synced_image_logger_ && synced_image_logger_->enabled()) ||
-         (color_logger_ && color_logger_->enabled()) || (depth_logger_ && depth_logger_->enabled());
+         (color_logger_ && color_logger_->enabled()) ||
+         (left_color_logger_ && left_color_logger_->enabled()) ||
+         (right_color_logger_ && right_color_logger_->enabled()) ||
+         (depth_logger_ && depth_logger_->enabled()) ||
+         (left_ir_logger_ && left_ir_logger_->enabled()) ||
+         (right_ir_logger_ && right_ir_logger_->enabled());
 }
 
 bool TimestampCsvLogger::imageStreamEnabled(OBStreamType stream_type) const {
@@ -104,6 +121,18 @@ void TimestampCsvLogger::recordImageFrameSet(const std::shared_ptr<ob::Frame> &c
   }
 }
 
+void TimestampCsvLogger::recordImageFrameArrival(OBStreamType stream_type,
+                                                 const std::shared_ptr<ob::Frame> &frame,
+                                                 int64_t arrival_system_us,
+                                                 int64_t arrival_steady_us,
+                                                 bool image_publish_expected) {
+  auto *timestamp_logger = imageLoggerForStream(stream_type);
+  if (timestamp_logger) {
+    timestamp_logger->recordStandaloneFrameArrival(stream_type, frame, arrival_system_us,
+                                                   arrival_steady_us, image_publish_expected);
+  }
+}
+
 void TimestampCsvLogger::recordImagePrePublish(OBStreamType stream_type,
                                                const std::shared_ptr<ob::Frame> &frame,
                                                int64_t publish_system_us,
@@ -164,7 +193,11 @@ void TimestampCsvLogger::shutdown() noexcept {
 
   shutdown_logger(synced_image_logger_, "synced image timestamp CSV logger");
   shutdown_logger(color_logger_, "color timestamp CSV logger");
+  shutdown_logger(left_color_logger_, "left color timestamp CSV logger");
+  shutdown_logger(right_color_logger_, "right color timestamp CSV logger");
   shutdown_logger(depth_logger_, "depth timestamp CSV logger");
+  shutdown_logger(left_ir_logger_, "left IR timestamp CSV logger");
+  shutdown_logger(right_ir_logger_, "right IR timestamp CSV logger");
   shutdown_logger(synced_imu_logger_, "synced IMU timestamp CSV logger");
   shutdown_logger(accel_logger_, "accel timestamp CSV logger");
   shutdown_logger(gyro_logger_, "gyro timestamp CSV logger");
@@ -177,6 +210,18 @@ FrameTimestampCsvLogger *TimestampCsvLogger::imageLoggerForStream(OBStreamType s
   if (stream_type == OB_STREAM_DEPTH) {
     return synced_image_logger_ ? synced_image_logger_.get() : depth_logger_.get();
   }
+  if (stream_type == OB_STREAM_COLOR_LEFT) {
+    return left_color_logger_.get();
+  }
+  if (stream_type == OB_STREAM_COLOR_RIGHT) {
+    return right_color_logger_.get();
+  }
+  if (stream_type == OB_STREAM_IR_LEFT) {
+    return left_ir_logger_.get();
+  }
+  if (stream_type == OB_STREAM_IR_RIGHT) {
+    return right_ir_logger_.get();
+  }
   return nullptr;
 }
 
diff --git a/orbbec_camera/src/utils.cpp b/orbbec_camera/src/utils.cpp
index c9fe56c4..1dfcf64e 100644
--- a/orbbec_camera/src/utils.cpp
+++ b/orbbec_camera/src/utils.cpp
@@ -946,17 +946,28 @@ std::string parseUsbPort(const std::string &line) {
 }
 
 bool isValidJPEG(const std::shared_ptr<ob::ColorFrame> &frame) {
-  if (frame->getDataSize() < 2) {  // Checking both start and end markers, so minimal size is 4
+  if (!frame) {
     return false;
   }
 
+  const auto data_size = frame->getDataSize();
   const auto *data = static_cast<const uint8_t *>(frame->getData());
+  if (data == nullptr || data_size < 4) {
+    return false;
+  }
 
   // Check for JPEG start marker
   if (data[0] != 0xFF || data[1] != 0xD8) {
     return false;
   }
-  return true;
+
+  auto jpeg_size = data_size;
+  while (jpeg_size > 2 && data[jpeg_size - 1] == 0x00) {
+    --jpeg_size;
+  }
+
+  // Check for JPEG end marker after trimming zero padding.
+  return jpeg_size >= 4 && data[jpeg_size - 2] == 0xFF && data[jpeg_size - 1] == 0xD9;
 }
 
 std::string metaDataTypeToString(const OBFrameMetadataType &meta_data_type) {
diff --git a/orbbec_camera/test/camera_info_distortion_test.cpp b/orbbec_camera/test/camera_info_distortion_test.cpp
index 5c303284..0796e9ef 100644
--- a/orbbec_camera/test/camera_info_distortion_test.cpp
+++ b/orbbec_camera/test/camera_info_distortion_test.cpp
@@ -1,5 +1,6 @@
 #include <gtest/gtest.h>
 
+#include <cstring>
 #include <vector>
 
 #include "orbbec_camera/utils.h"
@@ -32,6 +33,14 @@ OBCameraDistortion makeDistortion(OBCameraDistortionModel model) {
   return distortion;
 }
 
+std::shared_ptr<ob::ColorFrame> makeMjpegFrame(const std::vector<uint8_t>& data) {
+  auto frame = ob::FrameFactory::createFrame(OB_FRAME_COLOR, OB_FORMAT_MJPG,
+                                             static_cast<uint32_t>(data.size()));
+  auto color_frame = frame->as<ob::ColorFrame>();
+  std::memcpy(color_frame->getData(), data.data(), data.size());
+  return color_frame;
+}
+
 TEST(CameraInfoDistortionTest, ConvertsBrownConradyToPlumbBob) {
   const auto intrinsic = makeIntrinsic();
   const auto distortion = makeDistortion(OB_DISTORTION_BROWN_CONRADY);
@@ -69,5 +78,17 @@ TEST(CameraInfoDistortionTest, ConvertsKannalaBrandtToEquidistant) {
             std::vector<double>({distortion.k1, distortion.k2, distortion.k3, distortion.k4}));
 }
 
+TEST(JpegValidationTest, AcceptsEoiBeforeZeroPadding) {
+  const auto frame = makeMjpegFrame({0xFF, 0xD8, 0x01, 0x02, 0xFF, 0xD9, 0x00, 0x00});
+
+  EXPECT_TRUE(isValidJPEG(frame));
+}
+
+TEST(JpegValidationTest, RejectsMjpegWithoutEoi) {
+  const auto frame = makeMjpegFrame({0xFF, 0xD8, 0x01, 0x02, 0x00, 0x00});
+
+  EXPECT_FALSE(isValidJPEG(frame));
+}
+
 }  // namespace
 }  // namespace orbbec_camera
diff --git a/orbbec_camera/tools/list_camera_profile.cpp b/orbbec_camera/tools/list_camera_profile.cpp
index 7f3673a6..07e352e5 100644
--- a/orbbec_camera/tools/list_camera_profile.cpp
+++ b/orbbec_camera/tools/list_camera_profile.cpp
@@ -145,8 +145,8 @@ void listSensorProfiles(const std::shared_ptr<ob::Device>& device) {
       auto origin_profile = profile_list->getProfile(j);
       if ((sensor->getType() == OB_SENSOR_DEPTH || sensor->getType() == OB_SENSOR_IR_LEFT ||
            sensor->getType() == OB_SENSOR_IR_RIGHT) &&
-          isGemini305SeriesPID(pid)) {
-        // Gemini 305 series
+          isGemini301SeriesPID(pid)) {
+        // Gemini 301 series
         auto profile = origin_profile->as<ob::VideoStreamProfile>();
         std::cout << magic_enum::enum_name(sensor->getType()) << " profile: " << profile->getWidth()
                   << "x" << profile->getHeight() << " " << profile->getFps() << "fps "
@@ -154,8 +154,11 @@ void listSensorProfiles(const std::shared_ptr<ob::Device>& device) {
                   << " | width: " << profile->getDecimationConfig().originWidth
                   << " height: " << profile->getDecimationConfig().originHeight
                   << " downscale:" << profile->getDecimationConfig().factor << std::endl;
-      } else if (sensor->getType() == OB_SENSOR_COLOR || sensor->getType() == OB_SENSOR_DEPTH ||
-                 sensor->getType() == OB_SENSOR_IR || sensor->getType() == OB_SENSOR_IR_LEFT ||
+      } else if (sensor->getType() == OB_SENSOR_COLOR ||
+                 sensor->getType() == OB_SENSOR_COLOR_LEFT ||
+                 sensor->getType() == OB_SENSOR_COLOR_RIGHT ||
+                 sensor->getType() == OB_SENSOR_DEPTH || sensor->getType() == OB_SENSOR_IR ||
+                 sensor->getType() == OB_SENSOR_IR_LEFT ||
                  sensor->getType() == OB_SENSOR_IR_RIGHT) {
         auto profile = origin_profile->as<ob::VideoStreamProfile>();
         std::cout << magic_enum::enum_name(sensor->getType()) << " profile: " << profile->getWidth()
diff --git a/orbbec_camera/tools/multi_save_rgbir.cpp b/orbbec_camera/tools/multi_save_rgbir.cpp
index 69bbc290..19ebe69d 100644
--- a/orbbec_camera/tools/multi_save_rgbir.cpp
+++ b/orbbec_camera/tools/multi_save_rgbir.cpp
@@ -1,72 +1,114 @@
-
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp_components/register_node_macro.hpp>
-#include <orbbec_camera/ob_camera_node_driver.h>
-#include <orbbec_camera/utils.h>
-#include "orbbec_camera/ob_camera_node.h"
-#include "orbbec_camera_msgs/msg/metadata.hpp"
-#include <std_msgs/msg/int32.hpp>
+
+#include <algorithm>
+#include <array>
+#include <chrono>
+#include <cstdint>
+#include <ctime>
 #include <filesystem>
-#include <regex>
+#include <fstream>
+#include <functional>
+#include <iomanip>
+#include <map>
+#include <memory>
+#include <mutex>
+#include <sstream>
+#include <stdexcept>
+#include <string>
+#include <thread>
+#include <utility>
+#include <vector>
+
+#include <nlohmann/json.hpp>
+#include <sensor_msgs/image_encodings.hpp>
+#include <std_msgs/msg/header.hpp>
+
+#include "orbbec_camera/ob_camera_node.h"
+#include "orbbec_camera/utils.h"
+#include "orbbec_camera_msgs/msg/metadata.hpp"
+
 namespace orbbec_camera {
 namespace tools {
-struct ImageMetadata {
-  std::vector<std::vector<std::string>> exposure_buffs;
-  std::vector<std::vector<std::string>> gain_buffs;
+namespace {
+
+const std::array<std::string, 6> kSupportedStreamNames = {
+    "color", "left_color", "right_color", "ir", "left_ir", "right_ir",
+};
+constexpr auto kStreamDiscoveryPollInterval = std::chrono::milliseconds(100);
+constexpr auto kStreamDiscoveryStablePeriod = std::chrono::seconds(1);
+constexpr auto kStreamDiscoveryTimeout = std::chrono::seconds(5);
+constexpr size_t kMinimumPendingFrameLimit = 30;
+
+struct StreamTopicInfo {
+  std::string name;
+  bool metadata_available = false;
+
+  bool operator==(const StreamTopicInfo &other) const {
+    return name == other.name && metadata_available == other.metadata_available;
+  }
+};
+
+bool isSupportedStreamName(const std::string &stream_name) {
+  return std::find(kSupportedStreamNames.begin(), kSupportedStreamNames.end(), stream_name) !=
+         kSupportedStreamNames.end();
+}
+
+bool isColorCaptureStreamName(const std::string &stream_name) {
+  return stream_name == "color" || stream_name == "left_color" || stream_name == "right_color";
+}
+
+std::string cameraNamespace(const std::string &camera_name) {
+  if (!camera_name.empty() && camera_name.front() == '/') {
+    return camera_name;
+  }
+  return "/" + camera_name;
+}
+
+}  // namespace
+
+struct StreamCapture {
+  struct FrameMetadata {
+    std::string exposure;
+    std::string gain;
+  };
+
+  struct PendingImage {
+    cv::Mat image;
+    std::string current_timestamp;
+    std::string receive_timestamp;
+  };
+
+  std::vector<cv::Mat> images;
+  std::vector<std::string> current_timestamps;
+  std::vector<std::string> receive_timestamps;
+  std::vector<FrameMetadata> frame_metadata;
+  std::map<int64_t, PendingImage> pending_images;
+  std::map<int64_t, FrameMetadata> pending_metadata;
+  bool metadata_required = false;
+
+  void clear() {
+    images.clear();
+    current_timestamps.clear();
+    receive_timestamps.clear();
+    frame_metadata.clear();
+    pending_images.clear();
+    pending_metadata.clear();
+  }
 };
 
 class MultiCameraSubscriber : public rclcpp::Node {
  public:
   explicit MultiCameraSubscriber(const rclcpp::NodeOptions &options)
       : Node("MultiCameraSubscriber", options) {
-    device_init();
-  }
-  ~MultiCameraSubscriber() {
-    ir_image_buffers_.clear();
-    ir_current_timestamp_buffers_.clear();
-    ir_timestamp_buffers_.clear();
-    color_image_buffers_.clear();
-    color_current_timestamp_buffers_.clear();
-    color_timestamp_buffers_.clear();
-    left_ir_metadata_.exposure_buffs.clear();
-    left_ir_metadata_.gain_buffs.clear();
-    color_metadata_.exposure_buffs.clear();
-    color_metadata_.gain_buffs.clear();
-  }
-  void device_init() {
-    try {
-      auto context = std::make_unique<ob::Context>();
-      context->setLoggerSeverity(OBLogSeverity::OB_LOG_SEVERITY_NONE);
-      auto list = context->queryDeviceList();
-      for (size_t i = 0; i < list->deviceCount(); i++) {
-        auto device = list->getDevice(i);
-        auto device_info = device->getDeviceInfo();
-        auto pid = device_info->getPid();
-        std::string serial = device_info->serialNumber();
-        std::string uid = device_info->uid();
-        auto usb_port = parseUsbPort(uid);
-        serial_numbers_[usb_port] = serial;
-        is_gemini330_ = isGemini335PID(pid);
-      }
-    } catch (ob::Error &e) {
-      RCLCPP_ERROR_STREAM(get_logger(), orbbec_camera::formatObErrorWithStatus(e));
-    } catch (const std::exception &e) {
-      RCLCPP_ERROR_STREAM(get_logger(), e.what());
-    } catch (...) {
-      RCLCPP_ERROR_STREAM(get_logger(), "unknown error");
+    initializeDeviceInfo();
+    loadParameters();
+    for (size_t i = 0; i < usb_ports_.size(); ++i) {
+      usb_index_map_[usb_ports_[i]] = static_cast<int>(i);
     }
-    params_init();
-    for (size_t i = 0; i < usb_params_.size(); i++) {
-      usb_numbers_[i] = usb_params_[i];
-      usb_index_map_[usb_params_[i]] = i;
-    }
-    for (const auto &pair : serial_numbers_) {
-      RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"),
-                         "usb_port: " << pair.first << ", serial: " << pair.second);
-    }
-    for (const auto &pair : usb_index_map_) {
-      RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"),
-                         "usb_port: " << pair.first << ", index: " << pair.second);
+    for (const auto &entry : serial_numbers_) {
+      RCLCPP_INFO(get_logger(), "usb_port: %s, serial: %s", entry.first.c_str(),
+                  entry.second.c_str());
     }
     capture_control_srv_ = this->create_service<orbbec_camera_msgs::srv::SetInt32>(
         "start_capture", std::bind(&MultiCameraSubscriber::controlCaptureCallback, this,
@@ -74,337 +116,462 @@ class MultiCameraSubscriber : public rclcpp::Node {
   }
 
  private:
-  std::mutex image_mutex_;
-  std::mutex meta_mutex_;
-  bool isGemini335PID(uint32_t pid) {
-    return pid == GEMINI_335_PID || pid == GEMINI_330_PID || pid == GEMINI_336_PID ||
-           pid == GEMINI_335L_PID || pid == GEMINI_330L_PID || pid == GEMINI_336L_PID ||
-           pid == GEMINI_335LG_PID || pid == GEMINI_336LG_PID || pid == GEMINI_335LE_PID ||
-           pid == GEMINI_336LE_PID || pid == CUSTOM_ADVANTECH_GEMINI_336_PID ||
-           pid == CUSTOM_ADVANTECH_GEMINI_336L_PID || pid == GEMINI_338_PID ||
-           pid == GEMINI_338LG_PID || pid == GEMINI_338LE_PID || pid == GEMINI_338L_PID ||
-           pid == GEMINI_331L_PID;
+  void initializeDeviceInfo() {
+    try {
+      auto context = std::make_unique<ob::Context>();
+      context->setLoggerSeverity(OBLogSeverity::OB_LOG_SEVERITY_NONE);
+      auto list = context->queryDeviceList();
+      for (size_t i = 0; i < list->deviceCount(); ++i) {
+        auto device_info = list->getDevice(i)->getDeviceInfo();
+        const auto usb_port = parseUsbPort(device_info->uid());
+        serial_numbers_[usb_port] = device_info->serialNumber();
+      }
+    } catch (const ob::Error &e) {
+      RCLCPP_ERROR_STREAM(get_logger(), orbbec_camera::formatObErrorWithStatus(e));
+    } catch (const std::exception &e) {
+      RCLCPP_ERROR_STREAM(get_logger(), e.what());
+    } catch (...) {
+      RCLCPP_ERROR(get_logger(), "unknown error while querying devices");
+    }
   }
-  void params_init() {
+
+  void loadParameters() {
     std::ifstream file(
         "install/orbbec_camera/share/orbbec_camera/config/tools/multisavergbir/"
         "multi_save_rgbir_params.json");
     if (!file.is_open()) {
-      RCLCPP_ERROR(this->get_logger(), "Failed to open JSON file.");
+      RCLCPP_ERROR(get_logger(), "Failed to open JSON file.");
       return;
     }
+
     nlohmann::json json_data;
     file >> json_data;
-    time_domain_ = json_data["save_rgbir_params"]["time_domain"].get<std::string>();
-    time_domain_ =
-        (time_domain_ == "device") ? "_d" : (time_domain_ == "global" ? "_g" : "_unknown");
-    usb_params_ = json_data["save_rgbir_params"]["usb_ports"].get<std::vector<std::string>>();
-    camera_name_ = json_data["save_rgbir_params"]["camera_name"].get<std::vector<std::string>>();
-    left_ir_topics_.resize(camera_name_.size());
-    left_ir_metadata_topic_.resize(camera_name_.size());
-    color_topics_.resize(camera_name_.size());
-    color_metadata_topic_.resize(camera_name_.size());
-    for (size_t i = 0; i < camera_name_.size(); ++i) {
-      left_ir_topics_[i] =
-          "/" + camera_name_[i] + "/" + (is_gemini330_ ? "left_ir" : "ir") + "/image_raw";
-      left_ir_metadata_topic_[i] =
-          "/" + camera_name_[i] + "/" + (is_gemini330_ ? "left_ir" : "ir") + "/metadata";
-      color_topics_[i] = "/" + camera_name_[i] + "/color/image_raw";
-      color_metadata_topic_[i] = "/" + camera_name_[i] + "/color/metadata";
+    const auto &params = json_data["save_rgbir_params"];
+    const auto time_domain = params["time_domain"].get<std::string>();
+    time_domain_suffix_ =
+        time_domain == "device" ? "_d" : (time_domain == "global" ? "_g" : "_unknown");
+    usb_ports_ = params["usb_ports"].get<std::vector<std::string>>();
+    camera_names_ = params["camera_name"].get<std::vector<std::string>>();
+
+    if (params.contains("stream_names")) {
+      for (const auto &stream_name : params["stream_names"].get<std::vector<std::string>>()) {
+        if (!isSupportedStreamName(stream_name)) {
+          throw std::invalid_argument("Unsupported stream name in multi_save_rgbir config: " +
+                                      stream_name);
+        }
+        if (std::find(configured_stream_names_.begin(), configured_stream_names_.end(),
+                      stream_name) == configured_stream_names_.end()) {
+          configured_stream_names_.push_back(stream_name);
+        }
+      }
     }
   }
-  void topic_init() {
-    ir_image_buffers_.resize(left_ir_topics_.size());
-    color_image_buffers_.resize(left_ir_topics_.size());
-    ir_current_timestamp_buffers_.resize(left_ir_topics_.size());
-    color_current_timestamp_buffers_.resize(left_ir_topics_.size());
-    ir_timestamp_buffers_.resize(left_ir_topics_.size());
-    color_timestamp_buffers_.resize(left_ir_topics_.size());
-    left_ir_metadata_.exposure_buffs.resize(left_ir_topics_.size());
-    left_ir_metadata_.gain_buffs.resize(left_ir_topics_.size());
-    color_metadata_.exposure_buffs.resize(left_ir_topics_.size());
-    color_metadata_.gain_buffs.resize(left_ir_topics_.size());
-    callback_called_ = std::vector<bool>(left_ir_topics_.size(), false);
-    auto custom_qos = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default));
-    rclcpp::CallbackGroup::SharedPtr reentrant_callback_group_;
-    RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"),
-                       "camera_name_.size(): " << camera_name_.size());
-    for (size_t i = 0; i < camera_name_.size(); ++i) {
-      RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"),
-                         "left_ir_topic: " << left_ir_topics_[i]);
-      RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"),
-                         "left_ir_metadata_topic_: " << left_ir_metadata_topic_[i]);
-      RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"),
-                         "color_topic: " << color_topics_[i]);
-      RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"),
-                         "color_metadata_topic_: " << color_metadata_topic_[i]);
-      reentrant_callback_group_ = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant);
-      rclcpp::SubscriptionOptions ir_sub_options;
-      ir_sub_options.callback_group = reentrant_callback_group_;
 
-      rclcpp::SubscriptionOptions color_sub_options;
-      color_sub_options.callback_group = reentrant_callback_group_;
+  std::vector<StreamTopicInfo> discoverStreams(const std::string &camera_name) const {
+    std::vector<StreamTopicInfo> streams;
+    const auto names_and_types = this->get_topic_names_and_types();
+    const std::string prefix = cameraNamespace(camera_name) + "/";
+    for (const auto &stream_name : kSupportedStreamNames) {
+      const auto image_topic_it = names_and_types.find(prefix + stream_name + "/image_raw");
+      if (image_topic_it == names_and_types.end()) {
+        continue;
+      }
+      const auto &image_types = image_topic_it->second;
+      if (std::find(image_types.begin(), image_types.end(), "sensor_msgs/msg/Image") ==
+          image_types.end()) {
+        continue;
+      }
 
-      auto ir_sub = this->create_subscription<sensor_msgs::msg::Image>(
-          left_ir_topics_[i], custom_qos,
-          [this, i](std::shared_ptr<const sensor_msgs::msg::Image> msg) {
-            this->irCallback(msg, i);
-          },
-          ir_sub_options);
-
-      auto ir_metadata_sub = this->create_subscription<orbbec_camera_msgs::msg::Metadata>(
-          left_ir_metadata_topic_[i], custom_qos,
-          [this, i](std::shared_ptr<const orbbec_camera_msgs::msg::Metadata> msg) {
-            this->ir_meta_Callback(msg, i);
-          });
-
-      auto color_sub = this->create_subscription<sensor_msgs::msg::Image>(
-          color_topics_[i], custom_qos,
-          [this, i](std::shared_ptr<const sensor_msgs::msg::Image> msg) {
-            this->colorCallback(msg, i);
-          },
-          color_sub_options);
-
-      auto color_metadata_sub = this->create_subscription<orbbec_camera_msgs::msg::Metadata>(
-          color_metadata_topic_[i], custom_qos,
-          [this, i](std::shared_ptr<const orbbec_camera_msgs::msg::Metadata> msg) {
-            this->color_meta_Callback(msg, i);
-          });
-
-      ir_subscribers_.push_back(ir_sub);
-      ir_meta_subscribers_.push_back(ir_metadata_sub);
-      color_subscribers_.push_back(color_sub);
-      color_meta_subscribers_.push_back(color_metadata_sub);
+      const auto metadata_topic_it = names_and_types.find(prefix + stream_name + "/metadata");
+      const bool metadata_available =
+          metadata_topic_it != names_and_types.end() &&
+          std::find(metadata_topic_it->second.begin(), metadata_topic_it->second.end(),
+                    "orbbec_camera_msgs/msg/Metadata") != metadata_topic_it->second.end();
+      streams.push_back(StreamTopicInfo{stream_name, metadata_available});
     }
+    return streams;
   }
-  std::string getCurrentTimes() {
-    auto now = std::chrono::system_clock::now();
-    auto now_time_t = std::chrono::system_clock::to_time_t(now);
-    std::tm tm = *std::localtime(&now_time_t);
-    std::ostringstream date_stream;
-    date_stream << std::put_time(&tm, "%Y%m%d%H%M%S");
 
-    std::string date_str = date_stream.str();
-    return date_str;
+  std::vector<std::vector<StreamTopicInfo>> waitForStableStreams() const {
+    if (!configured_stream_names_.empty()) {
+      std::vector<std::vector<StreamTopicInfo>> configured_streams;
+      configured_streams.reserve(camera_names_.size());
+      for (const auto &camera_name : camera_names_) {
+        const auto discovered_streams = discoverStreams(camera_name);
+        std::vector<StreamTopicInfo> camera_streams;
+        camera_streams.reserve(configured_stream_names_.size());
+        for (const auto &stream_name : configured_stream_names_) {
+          const auto discovered_it = std::find_if(
+              discovered_streams.begin(), discovered_streams.end(),
+              [&stream_name](const auto &stream) { return stream.name == stream_name; });
+          camera_streams.push_back(StreamTopicInfo{
+              stream_name,
+              discovered_it != discovered_streams.end() && discovered_it->metadata_available});
+        }
+        configured_streams.push_back(std::move(camera_streams));
+      }
+      return configured_streams;
+    }
+
+    std::vector<std::vector<StreamTopicInfo>> candidate;
+    auto candidate_since = std::chrono::steady_clock::time_point{};
+    const auto deadline = std::chrono::steady_clock::now() + kStreamDiscoveryTimeout;
+    while (rclcpp::ok() && std::chrono::steady_clock::now() < deadline) {
+      std::vector<std::vector<StreamTopicInfo>> current;
+      current.reserve(camera_names_.size());
+      bool all_cameras_discovered = !camera_names_.empty();
+      for (const auto &camera_name : camera_names_) {
+        current.push_back(discoverStreams(camera_name));
+        all_cameras_discovered = all_cameras_discovered && !current.back().empty();
+      }
+
+      const auto now = std::chrono::steady_clock::now();
+      if (!all_cameras_discovered) {
+        candidate.clear();
+      } else if (current != candidate) {
+        candidate = std::move(current);
+        candidate_since = now;
+      } else if (now - candidate_since >= kStreamDiscoveryStablePeriod) {
+        return candidate;
+      }
+
+      std::this_thread::sleep_for(kStreamDiscoveryPollInterval);
+    }
+
+    RCLCPP_WARN_STREAM(get_logger(),
+                       "Supported image topics did not become stable within "
+                           << kStreamDiscoveryTimeout.count()
+                           << " seconds; start all cameras and streams first, retry the request, "
+                              "or configure stream_names explicitly");
+    return {};
   }
-  std::string generateFolderName(const std::string &serial_number, size_t serial_index) {
-    std::string path = std::string("multicamera_sync/output/") + currenttimes_ + "/" +
-                       "TotalModeFrames/" + "/" + "SN" + serial_number + "_Index" +
-                       std::to_string(serial_index);
 
+  bool initializeTopics() {
+    const auto streams_by_camera = waitForStableStreams();
+    if (streams_by_camera.size() != camera_names_.size()) {
+      return false;
+    }
+
+    callback_groups_.clear();
+    image_subscribers_.clear();
+    metadata_subscribers_.clear();
+    captures_.resize(camera_names_.size());
+    callback_called_.assign(camera_names_.size(), false);
+    const auto custom_qos =
+        rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default));
+
+    for (size_t camera_index = 0; camera_index < camera_names_.size(); ++camera_index) {
+      const auto &streams = streams_by_camera[camera_index];
+      const std::string prefix = cameraNamespace(camera_names_[camera_index]) + "/";
+      auto callback_group = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant);
+      callback_groups_.push_back(callback_group);
+      rclcpp::SubscriptionOptions options;
+      options.callback_group = callback_group;
+
+      for (const auto &stream : streams) {
+        const auto &stream_name = stream.name;
+        StreamCapture capture;
+        capture.metadata_required = stream.metadata_available;
+        captures_[camera_index].emplace(stream_name, std::move(capture));
+        const std::string image_topic = prefix + stream_name + "/image_raw";
+        RCLCPP_INFO(get_logger(), "Subscribing to %s", image_topic.c_str());
+
+        image_subscribers_.push_back(this->create_subscription<sensor_msgs::msg::Image>(
+            image_topic, custom_qos,
+            [this, camera_index,
+             stream_name](const std::shared_ptr<const sensor_msgs::msg::Image> image) {
+              imageCallback(image, camera_index, stream_name);
+            },
+            options));
+        if (stream.metadata_available) {
+          const std::string metadata_topic = prefix + stream_name + "/metadata";
+          metadata_subscribers_.push_back(
+              this->create_subscription<orbbec_camera_msgs::msg::Metadata>(
+                  metadata_topic, custom_qos,
+                  [this, camera_index, stream_name](
+                      const std::shared_ptr<const orbbec_camera_msgs::msg::Metadata> metadata) {
+                    metadataCallback(metadata, camera_index, stream_name);
+                  },
+                  options));
+        }
+      }
+    }
+    return !captures_.empty() && std::all_of(captures_.begin(), captures_.end(),
+                                             [](const auto &streams) { return !streams.empty(); });
+  }
+
+  std::string currentDateTime() const {
+    const auto now = std::chrono::system_clock::now();
+    const auto now_time = std::chrono::system_clock::to_time_t(now);
+    const std::tm time_info = *std::localtime(&now_time);
+    std::ostringstream output;
+    output << std::put_time(&time_info, "%Y%m%d%H%M%S");
+    return output.str();
+  }
+
+  std::string generateFolderName(const std::string &serial_number, size_t serial_index) const {
+    const std::string path = "multicamera_sync/output/" + current_date_time_ +
+                             "/TotalModeFrames/SN" + serial_number + "_Index" +
+                             std::to_string(serial_index);
     std::filesystem::create_directories(path);
     return path;
   }
-  std::string getTimestamp() {
-    auto now = this->get_clock()->now();
-    int64_t seconds = now.seconds();
-    int64_t nanoseconds = now.nanoseconds() % 1000000000;
-    int64_t milliseconds = nanoseconds / 1000000;
+
+  std::string receiveTimestamp() {
+    const auto now = this->get_clock()->now();
+    const int64_t seconds = now.seconds();
+    const int64_t milliseconds = now.nanoseconds() % 1000000000 / 1000000;
     return std::to_string(seconds) + std::to_string(milliseconds);
   }
-  std::string getCurrentTimestamp(const sensor_msgs::msg::Image::ConstSharedPtr &image_msg) {
-    int64_t seconds = image_msg->header.stamp.sec;
-    int64_t nanoseconds = image_msg->header.stamp.nanosec;
-
-    int64_t milliseconds = nanoseconds / 1000000;
 
+  std::string imageTimestamp(const sensor_msgs::msg::Image::ConstSharedPtr &image) const {
+    const int64_t milliseconds = image->header.stamp.nanosec / 1000000;
     std::ostringstream timestamp;
-    timestamp << seconds << std::setw(3) << std::setfill('0') << milliseconds;
-
+    timestamp << image->header.stamp.sec << std::setw(3) << std::setfill('0') << milliseconds;
     return timestamp.str();
   }
 
-  void saveAlignedImages(size_t index) {
-    auto &ir_images = ir_image_buffers_[index];
-    auto &ir_current_timestamps = ir_current_timestamp_buffers_[index];
-    auto &ir_timestamps = ir_timestamp_buffers_[index];
-    auto &color_images = color_image_buffers_[index];
-    auto &color_current_timestamps = color_current_timestamp_buffers_[index];
-    auto &color_timestamps = color_timestamp_buffers_[index];
-    auto &left_ir_meta_exposure = left_ir_metadata_.exposure_buffs[index];
-    auto &left_ir_meta_gain = left_ir_metadata_.gain_buffs[index];
-    auto &color_meta_exposure = color_metadata_.exposure_buffs[index];
-    auto &color_meta_gain = color_metadata_.gain_buffs[index];
-    callback_called_[index] = true;
-    if (ir_images.size() < static_cast<size_t>(saving_images_number_) ||
-        color_images.size() < static_cast<size_t>(saving_images_number_)) {
+  bool captureReady(size_t camera_index) const {
+    if (camera_index >= captures_.size() || captures_[camera_index].empty()) {
+      return false;
+    }
+    return std::all_of(
+        captures_[camera_index].begin(), captures_[camera_index].end(), [this](const auto &entry) {
+          return entry.second.images.size() >= static_cast<size_t>(saving_images_number_);
+        });
+  }
+
+  int64_t messageStampNs(const std_msgs::msg::Header &header) const {
+    return static_cast<int64_t>(header.stamp.sec) * 1000000000LL + header.stamp.nanosec;
+  }
+
+  size_t pendingFrameLimit() const {
+    return std::max(kMinimumPendingFrameLimit, static_cast<size_t>(saving_images_number_) * 2);
+  }
+
+  template <typename Value>
+  void trimPendingFrames(std::map<int64_t, Value> &pending) const {
+    while (pending.size() > pendingFrameLimit()) {
+      pending.erase(pending.begin());
+    }
+  }
+
+  void appendCompletedFrame(StreamCapture &capture, StreamCapture::PendingImage pending_image,
+                            StreamCapture::FrameMetadata metadata = {}) {
+    if (capture.images.size() >= static_cast<size_t>(saving_images_number_)) {
       return;
     }
-    RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"), "index:" << index);
-    auto usb_iter = usb_index_map_.find(usb_numbers_[index]);
-    auto serial_iter = serial_numbers_.find(usb_numbers_[index]);
-    int usb_index = usb_iter->second;
-    if (serial_iter == serial_numbers_.end()) {
-      RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"), "serial_iter is empty");
+    capture.images.push_back(std::move(pending_image.image));
+    capture.current_timestamps.push_back(std::move(pending_image.current_timestamp));
+    capture.receive_timestamps.push_back(std::move(pending_image.receive_timestamp));
+    capture.frame_metadata.push_back(std::move(metadata));
+  }
+
+  std::string metadataSuffix(const StreamCapture &capture, size_t frame_index) const {
+    if (frame_index >= capture.frame_metadata.size()) {
+      return "";
+    }
+    std::string suffix;
+    if (!capture.frame_metadata[frame_index].exposure.empty()) {
+      suffix += "_e" + capture.frame_metadata[frame_index].exposure;
+    }
+    if (!capture.frame_metadata[frame_index].gain.empty()) {
+      suffix += "_d" + capture.frame_metadata[frame_index].gain;
+    }
+    return suffix;
+  }
+
+  void saveImages(size_t camera_index) {
+    if (!captureReady(camera_index)) {
       return;
     }
-    std::string serial_index = serial_iter->second;
+    if (camera_index >= usb_ports_.size()) {
+      RCLCPP_ERROR(get_logger(), "Missing USB port configuration for camera index %zu",
+                   camera_index);
+      return;
+    }
+    const auto serial_it = serial_numbers_.find(usb_ports_[camera_index]);
+    if (serial_it == serial_numbers_.end()) {
+      RCLCPP_ERROR(get_logger(), "No serial number found for USB port %s",
+                   usb_ports_[camera_index].c_str());
+      return;
+    }
+    const auto usb_index_it = usb_index_map_.find(usb_ports_[camera_index]);
+    const size_t usb_index = usb_index_it == usb_index_map_.end()
+                                 ? camera_index
+                                 : static_cast<size_t>(usb_index_it->second);
+    const std::string &serial_number = serial_it->second;
+    const std::string folder = generateFolderName(serial_number, usb_index);
+    callback_called_[camera_index] = true;
 
-    for (size_t i = 0; i < static_cast<size_t>(saving_images_number_); i++) {
-      std::string folder = generateFolderName(serial_index, usb_index);
-      std::string ir_filename =
-          folder + "/ir#left_SN" + serial_index + "_Index" + std::to_string(usb_index) +
-          time_domain_ + ir_current_timestamps[i] + "_f" + std::to_string(i) + "_s" +
-          ir_timestamps[i] +
-          (is_gemini330_ ? ("_e" + left_ir_meta_exposure[i] + "_d" + left_ir_meta_gain[i]) : "") +
-          "_.jpg";
-      if (ir_images[i].empty()) {
-        RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"), "over ");
-        continue;
+    for (const auto &entry : captures_[camera_index]) {
+      const std::string &stream_name = entry.first;
+      const auto &capture = entry.second;
+      for (size_t i = 0; i < static_cast<size_t>(saving_images_number_); ++i) {
+        if (capture.images[i].empty()) {
+          continue;
+        }
+        const std::string filename = folder + "/" + stream_name + "_SN" + serial_number + "_Index" +
+                                     std::to_string(usb_index) + time_domain_suffix_ +
+                                     capture.current_timestamps[i] + "_f" + std::to_string(i) +
+                                     "_s" + capture.receive_timestamps[i] +
+                                     metadataSuffix(capture, i) + "_.jpg";
+        cv::imwrite(filename, capture.images[i]);
       }
-
-      cv::imwrite(ir_filename, ir_images[i]);
-      std::string color_filename =
-          folder + "/color_SN" + serial_index + "_Index" + std::to_string(usb_index) +
-          time_domain_ + color_current_timestamps[i] + "_f" + std::to_string(i) + "_s" +
-          color_timestamps[i] +
-          (is_gemini330_ ? ("_e" + color_meta_exposure[i] + "_d" + color_meta_gain[i]) : "") +
-          "_.jpg";
-      if (color_images[i].empty()) {
-        continue;
-      }
-      cv::imwrite(color_filename, color_images[i]);
-      // RCLCPP_INFO(this->get_logger(), "Saved Color image to: %s", color_filename.c_str());
     }
 
-    ir_image_buffers_[index].clear();
-    ir_current_timestamp_buffers_[index].clear();
-    ir_timestamp_buffers_[index].clear();
-    color_image_buffers_[index].clear();
-    color_current_timestamp_buffers_[index].clear();
-    color_timestamp_buffers_[index].clear();
-    left_ir_metadata_.exposure_buffs[index].clear();
-    left_ir_metadata_.gain_buffs[index].clear();
-    color_metadata_.exposure_buffs[index].clear();
-    color_metadata_.gain_buffs[index].clear();
-    RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"),
-                       "callback_called_ " << index << ":" << callback_called_[index]);
-    bool all_true =
-        std::all_of(callback_called_.begin(), callback_called_.end(), [](bool v) { return v; });
-    if (all_true) {
-      RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"), "over ");
+    for (auto &entry : captures_[camera_index]) {
+      entry.second.clear();
+    }
+    const bool all_cameras_complete = std::all_of(callback_called_.begin(), callback_called_.end(),
+                                                  [](bool value) { return value; });
+    if (all_cameras_complete) {
+      RCLCPP_INFO(get_logger(), "Capture completed for all cameras");
       saving_images_number_ = 0;
-      callback_called_.clear();
-      callback_called_ = std::vector<bool>(left_ir_topics_.size(), false);
+      callback_called_.assign(camera_names_.size(), false);
     }
   }
 
   void controlCaptureCallback(
       const std::shared_ptr<orbbec_camera_msgs::srv::SetInt32::Request> request,
       std::shared_ptr<orbbec_camera_msgs::srv::SetInt32::Response> response) {
-    (void)response;
-    currenttimes_ = getCurrentTimes();
+    std::lock_guard<std::mutex> lock(capture_mutex_);
+    if (request->data <= 0) {
+      response->success = false;
+      response->message = "capture image count must be greater than zero";
+      return;
+    }
+    if (!topics_initialized_) {
+      if (!initializeTopics()) {
+        callback_groups_.clear();
+        image_subscribers_.clear();
+        metadata_subscribers_.clear();
+        captures_.clear();
+        callback_called_.clear();
+        response->success = false;
+        response->message =
+            "supported image streams did not become stable; retry after all cameras and streams "
+            "start or configure stream_names";
+        return;
+      }
+      topics_initialized_ = true;
+    }
+
+    for (auto &camera_captures : captures_) {
+      for (auto &entry : camera_captures) {
+        entry.second.clear();
+      }
+    }
+    callback_called_.assign(camera_names_.size(), false);
+    current_date_time_ = currentDateTime();
     saving_images_number_ = request->data;
-    if (!topic_init_) {
-      topic_init();
-      topic_init_ = true;
-    }
-    RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"),
-                       "saving_images_number_: " << saving_images_number_);
-  }
-  void irCallback(std::shared_ptr<const sensor_msgs::msg::Image> image, size_t index) {
-    std::lock_guard<std::mutex> lock(image_mutex_);
-    if (!callback_called_[index] && static_cast<size_t>(saving_images_number_)) {
-      cv::Mat ir_mat = cv_bridge::toCvCopy(image, image->encoding)->image;
-      std::string current_timestamp_ir = getCurrentTimestamp(image);
-      std::string timestamp_ir = getTimestamp();
-      ir_image_buffers_[index].push_back(ir_mat);
-      ir_current_timestamp_buffers_[index].push_back(current_timestamp_ir);
-      ir_timestamp_buffers_[index].push_back(timestamp_ir);
-      RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"),
-                         ":ir: " << index << ":" << ir_image_buffers_[index].size());
-      if (ir_image_buffers_[index].size() >= static_cast<size_t>(saving_images_number_) &&
-          color_image_buffers_[index].size() >= static_cast<size_t>(saving_images_number_) &&
-          (!is_gemini330_ || (color_metadata_.exposure_buffs[index].size() >=
-                                  static_cast<size_t>(saving_images_number_) &&
-                              left_ir_metadata_.exposure_buffs[index].size() >=
-                                  static_cast<size_t>(saving_images_number_)))) {
-        saveAlignedImages(index);
-      }
-    }
-  }
-  void colorCallback(std::shared_ptr<const sensor_msgs::msg::Image> image, size_t index) {
-    std::lock_guard<std::mutex> lock(image_mutex_);
-    if (!callback_called_[index] && static_cast<size_t>(saving_images_number_)) {
-      cv::Mat color_mat = cv_bridge::toCvCopy(image, image->encoding)->image;
-      cv::Mat corrected_image;
-      cv::cvtColor(color_mat, corrected_image, cv::COLOR_RGB2BGR);
-      std::string current_timestamp_color = getCurrentTimestamp(image);
-      std::string timestamp_color = getTimestamp();
-      color_image_buffers_[index].push_back(corrected_image);
-      color_current_timestamp_buffers_[index].push_back(current_timestamp_color);
-      color_timestamp_buffers_[index].push_back(timestamp_color);
-      RCLCPP_INFO_STREAM(rclcpp::get_logger("multi_camera_subscriber"),
-                         ":color: " << index << ":" << color_image_buffers_[index].size());
-      if (ir_image_buffers_[index].size() >= static_cast<size_t>(saving_images_number_) &&
-          color_image_buffers_[index].size() >= static_cast<size_t>(saving_images_number_) &&
-          (!is_gemini330_ || (color_metadata_.exposure_buffs[index].size() >=
-                                  static_cast<size_t>(saving_images_number_) &&
-                              left_ir_metadata_.exposure_buffs[index].size() >=
-                                  static_cast<size_t>(saving_images_number_)))) {
-        saveAlignedImages(index);
-      }
-    }
+    response->success = true;
+    response->message = "capture started";
+    RCLCPP_INFO(get_logger(), "Capturing %d image(s) from each configured stream",
+                saving_images_number_);
   }
 
-  void ir_meta_Callback(std::shared_ptr<const orbbec_camera_msgs::msg::Metadata> msg,
-                        size_t index) {
-    std::lock_guard<std::mutex> lock(meta_mutex_);
-    if (!callback_called_[index] && static_cast<size_t>(saving_images_number_)) {
-      nlohmann::json json_data = nlohmann::json::parse(msg->json_data);
-      left_ir_metadata_.exposure_buffs[index].push_back(json_data["exposure"].dump());
-      left_ir_metadata_.gain_buffs[index].push_back(json_data["gain"].dump());
+  void imageCallback(const std::shared_ptr<const sensor_msgs::msg::Image> image,
+                     size_t camera_index, const std::string &stream_name) {
+    std::lock_guard<std::mutex> lock(capture_mutex_);
+    if (saving_images_number_ <= 0 || callback_called_[camera_index]) {
+      return;
     }
-  }
-  void color_meta_Callback(std::shared_ptr<const orbbec_camera_msgs::msg::Metadata> msg,
-                           size_t index) {
-    std::lock_guard<std::mutex> lock(meta_mutex_);
-    if (!callback_called_[index] && static_cast<size_t>(saving_images_number_)) {
-      nlohmann::json json_data = nlohmann::json::parse(msg->json_data);
-      color_metadata_.exposure_buffs[index].push_back(json_data["exposure"].dump());
-      color_metadata_.gain_buffs[index].push_back(json_data["gain"].dump());
+    auto &capture = captures_[camera_index].at(stream_name);
+    if (capture.images.size() >= static_cast<size_t>(saving_images_number_)) {
+      saveImages(camera_index);
+      return;
     }
+    cv::Mat output = cv_bridge::toCvCopy(image, image->encoding)->image;
+    if (isColorCaptureStreamName(stream_name) &&
+        image->encoding == sensor_msgs::image_encodings::RGB8) {
+      cv::Mat converted;
+      cv::cvtColor(output, converted, cv::COLOR_RGB2BGR);
+      output = converted;
+    } else if (isColorCaptureStreamName(stream_name) &&
+               image->encoding == sensor_msgs::image_encodings::RGBA8) {
+      cv::Mat converted;
+      cv::cvtColor(output, converted, cv::COLOR_RGBA2BGRA);
+      output = converted;
+    }
+    StreamCapture::PendingImage pending_image{std::move(output), imageTimestamp(image),
+                                              receiveTimestamp()};
+    if (capture.metadata_required) {
+      const auto stamp_ns = messageStampNs(image->header);
+      auto metadata_it = capture.pending_metadata.find(stamp_ns);
+      if (metadata_it == capture.pending_metadata.end()) {
+        capture.pending_images.insert_or_assign(stamp_ns, std::move(pending_image));
+        trimPendingFrames(capture.pending_images);
+        return;
+      }
+      appendCompletedFrame(capture, std::move(pending_image), std::move(metadata_it->second));
+      capture.pending_metadata.erase(metadata_it);
+    } else {
+      appendCompletedFrame(capture, std::move(pending_image));
+    }
+    RCLCPP_INFO(get_logger(), "%s[%zu]: %zu/%d", stream_name.c_str(), camera_index,
+                capture.images.size(), saving_images_number_);
+    saveImages(camera_index);
   }
 
+  void metadataCallback(const std::shared_ptr<const orbbec_camera_msgs::msg::Metadata> metadata,
+                        size_t camera_index, const std::string &stream_name) {
+    std::lock_guard<std::mutex> lock(capture_mutex_);
+    if (saving_images_number_ <= 0 || callback_called_[camera_index]) {
+      return;
+    }
+    auto &capture = captures_[camera_index].at(stream_name);
+    if (!capture.metadata_required ||
+        capture.images.size() >= static_cast<size_t>(saving_images_number_)) {
+      return;
+    }
+    StreamCapture::FrameMetadata frame_metadata;
+    try {
+      const auto json_data = nlohmann::json::parse(metadata->json_data);
+      if (json_data.contains("exposure")) {
+        frame_metadata.exposure = json_data["exposure"].dump();
+      }
+      if (json_data.contains("gain")) {
+        frame_metadata.gain = json_data["gain"].dump();
+      }
+    } catch (const std::exception &e) {
+      RCLCPP_WARN(get_logger(), "Failed to parse %s metadata: %s", stream_name.c_str(), e.what());
+    }
+    const auto stamp_ns = messageStampNs(metadata->header);
+    auto image_it = capture.pending_images.find(stamp_ns);
+    if (image_it == capture.pending_images.end()) {
+      capture.pending_metadata.insert_or_assign(stamp_ns, std::move(frame_metadata));
+      trimPendingFrames(capture.pending_metadata);
+      return;
+    }
+    appendCompletedFrame(capture, std::move(image_it->second), std::move(frame_metadata));
+    capture.pending_images.erase(image_it);
+    RCLCPP_INFO(get_logger(), "%s[%zu]: %zu/%d", stream_name.c_str(), camera_index,
+                capture.images.size(), saving_images_number_);
+    saveImages(camera_index);
+  }
+
+  std::mutex capture_mutex_;
+  std::vector<rclcpp::CallbackGroup::SharedPtr> callback_groups_;
+  std::vector<rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr> image_subscribers_;
   std::vector<rclcpp::Subscription<orbbec_camera_msgs::msg::Metadata>::SharedPtr>
-      ir_meta_subscribers_;
-  std::vector<rclcpp::Subscription<orbbec_camera_msgs::msg::Metadata>::SharedPtr>
-      color_meta_subscribers_;
-  std::vector<rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr> ir_subscribers_;
-  std::vector<rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr> color_subscribers_;
+      metadata_subscribers_;
   rclcpp::Service<orbbec_camera_msgs::srv::SetInt32>::SharedPtr capture_control_srv_;
 
   std::map<std::string, int> usb_index_map_;
   std::map<std::string, std::string> serial_numbers_;
-  std::array<std::string, 10> usb_numbers_;
-
-  std::vector<std::string> usb_params_;
-  std::vector<std::string> camera_name_;
-  std::vector<std::string> left_ir_metadata_topic_;
-  std::vector<std::string> color_metadata_topic_;
-  std::vector<std::string> left_ir_topics_;
-  std::vector<std::string> color_topics_;
-  std::string time_domain_;
-
-  std::vector<std::vector<cv::Mat>> ir_image_buffers_;
-  std::vector<std::vector<cv::Mat>> color_image_buffers_;
-  std::vector<std::vector<std::string>> ir_current_timestamp_buffers_;
-  std::vector<std::vector<std::string>> color_current_timestamp_buffers_;
-  std::vector<std::vector<std::string>> ir_timestamp_buffers_;
-  std::vector<std::vector<std::string>> color_timestamp_buffers_;
-
+  std::vector<std::string> usb_ports_;
+  std::vector<std::string> camera_names_;
+  std::vector<std::string> configured_stream_names_;
+  std::vector<std::map<std::string, StreamCapture>> captures_;
   std::vector<bool> callback_called_;
-
-  std::string currenttimes_;
-
-  int saving_images_number_ = 100;
-
-  bool topic_init_ = false;
-  bool is_gemini330_ = true;
-
-  ImageMetadata left_ir_metadata_ = ImageMetadata();
-  ImageMetadata color_metadata_ = ImageMetadata();
+  std::string time_domain_suffix_;
+  std::string current_date_time_;
+  int saving_images_number_ = 0;
+  bool topics_initialized_ = false;
 };
+
 }  // namespace tools
 }  // namespace orbbec_camera
+
 RCLCPP_COMPONENTS_REGISTER_NODE(orbbec_camera::tools::MultiCameraSubscriber)
diff --git a/orbbec_camera/tools/start_benchmark.cpp b/orbbec_camera/tools/start_benchmark.cpp
index 5987a554..9d1fe622 100644
--- a/orbbec_camera/tools/start_benchmark.cpp
+++ b/orbbec_camera/tools/start_benchmark.cpp
@@ -19,6 +19,16 @@ class StartBenchmark : public rclcpp::Node {
           [this, i](std::shared_ptr<const sensor_msgs::msg::Image> msg) {
             this->color_Callback(msg, i);
           }));
+      left_color_subs_.push_back(this->create_subscription<sensor_msgs::msg::Image>(
+          left_color_topics_[i], custom_qos,
+          [this, i](std::shared_ptr<const sensor_msgs::msg::Image> msg) {
+            this->leftColorCallback(msg, i);
+          }));
+      right_color_subs_.push_back(this->create_subscription<sensor_msgs::msg::Image>(
+          right_color_topics_[i], custom_qos,
+          [this, i](std::shared_ptr<const sensor_msgs::msg::Image> msg) {
+            this->rightColorCallback(msg, i);
+          }));
       depth_subs_.push_back(this->create_subscription<sensor_msgs::msg::Image>(
           depth_topics_[i], custom_qos,
           [this, i](std::shared_ptr<const sensor_msgs::msg::Image> msg) {
@@ -45,6 +55,10 @@ class StartBenchmark : public rclcpp::Node {
             this->color_point_cloud_Callback(msg, i);
           }));
       RCLCPP_INFO_STREAM(rclcpp::get_logger("StartBenchmark"), color_topics_[i] << " is subed ");
+      RCLCPP_INFO_STREAM(rclcpp::get_logger("StartBenchmark"),
+                         left_color_topics_[i] << " is subed ");
+      RCLCPP_INFO_STREAM(rclcpp::get_logger("StartBenchmark"),
+                         right_color_topics_[i] << " is subed ");
       RCLCPP_INFO_STREAM(rclcpp::get_logger("StartBenchmark"), depth_topics_[i] << " is subed ");
       RCLCPP_INFO_STREAM(rclcpp::get_logger("StartBenchmark"), left_ir_topics_[i] << " is subed ");
       RCLCPP_INFO_STREAM(rclcpp::get_logger("StartBenchmark"), right_ir_topics_[i] << " is subed ");
@@ -57,6 +71,8 @@ class StartBenchmark : public rclcpp::Node {
 
  private:
   std::vector<rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr> color_subs_;
+  std::vector<rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr> left_color_subs_;
+  std::vector<rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr> right_color_subs_;
   std::vector<rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr> depth_subs_;
   std::vector<rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr> left_ir_subs_;
   std::vector<rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr> right_ir_subs_;
@@ -67,6 +83,8 @@ class StartBenchmark : public rclcpp::Node {
 
   std::vector<std::string> camera_name_;
   std::vector<std::string> color_topics_;
+  std::vector<std::string> left_color_topics_;
+  std::vector<std::string> right_color_topics_;
   std::vector<std::string> depth_topics_;
   std::vector<std::string> left_ir_topics_;
   std::vector<std::string> right_ir_topics_;
@@ -88,6 +106,8 @@ class StartBenchmark : public rclcpp::Node {
     camera_name_ =
         json_data["start_benchmark_params"]["camera_name"].get<std::vector<std::string>>();
     color_topics_.resize(camera_name_.size());
+    left_color_topics_.resize(camera_name_.size());
+    right_color_topics_.resize(camera_name_.size());
     depth_topics_.resize(camera_name_.size());
     left_ir_topics_.resize(camera_name_.size());
     right_ir_topics_.resize(camera_name_.size());
@@ -95,6 +115,8 @@ class StartBenchmark : public rclcpp::Node {
     color_point_cloud_topics_.resize(camera_name_.size());
     for (size_t i = 0; i < camera_name_.size(); ++i) {
       color_topics_[i] = "/" + camera_name_[i] + "/color/image_raw";
+      left_color_topics_[i] = "/" + camera_name_[i] + "/left_color/image_raw";
+      right_color_topics_[i] = "/" + camera_name_[i] + "/right_color/image_raw";
       depth_topics_[i] = "/" + camera_name_[i] + "/depth/image_raw";
       left_ir_topics_[i] = "/" + camera_name_[i] + "/left_ir/image_raw";
       right_ir_topics_[i] = "/" + camera_name_[i] + "/right_ir/image_raw";
@@ -108,6 +130,17 @@ class StartBenchmark : public rclcpp::Node {
     RCLCPP_DEBUG_STREAM(rclcpp::get_logger("StartBenchmark"),
                         "time is : " << msg->step << "color is subed " << index << "is subed");
   }
+  void leftColorCallback(std::shared_ptr<const sensor_msgs::msg::Image> msg, size_t index) {
+    std::lock_guard<std::mutex> lock(image_mutex_);
+    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("StartBenchmark"),
+                        "time is : " << msg->step << "left_color is subed " << index << "is subed");
+  }
+  void rightColorCallback(std::shared_ptr<const sensor_msgs::msg::Image> msg, size_t index) {
+    std::lock_guard<std::mutex> lock(image_mutex_);
+    RCLCPP_DEBUG_STREAM(
+        rclcpp::get_logger("StartBenchmark"),
+        "time is : " << msg->step << "right_color is subed " << index << "is subed");
+  }
   void depth_Callback(std::shared_ptr<const sensor_msgs::msg::Image> msg, size_t index) {
     std::lock_guard<std::mutex> lock(image_mutex_);
     RCLCPP_DEBUG_STREAM(rclcpp::get_logger("StartBenchmark"),
diff --git a/orbbec_camera_msgs/msg/DeviceStatus.msg b/orbbec_camera_msgs/msg/DeviceStatus.msg
index 90662542..32b46777 100644
--- a/orbbec_camera_msgs/msg/DeviceStatus.msg
+++ b/orbbec_camera_msgs/msg/DeviceStatus.msg
@@ -11,6 +11,28 @@ float64 color_delay_ms_avg
 float64 color_delay_ms_min
 float64 color_delay_ms_max
 
+# --- Left color stream ---
+float64 left_color_frame_rate_cur
+float64 left_color_frame_rate_avg
+float64 left_color_frame_rate_min
+float64 left_color_frame_rate_max
+
+float64 left_color_delay_ms_cur
+float64 left_color_delay_ms_avg
+float64 left_color_delay_ms_min
+float64 left_color_delay_ms_max
+
+# --- Right color stream ---
+float64 right_color_frame_rate_cur
+float64 right_color_frame_rate_avg
+float64 right_color_frame_rate_min
+float64 right_color_frame_rate_max
+
+float64 right_color_delay_ms_cur
+float64 right_color_delay_ms_avg
+float64 right_color_delay_ms_min
+float64 right_color_delay_ms_max
+
 # --- Depth stream ---
 float64 depth_frame_rate_cur
 float64 depth_frame_rate_avg
@@ -22,6 +44,28 @@ float64 depth_delay_ms_avg
 float64 depth_delay_ms_min
 float64 depth_delay_ms_max
 
+# --- Left IR stream ---
+float64 left_ir_frame_rate_cur
+float64 left_ir_frame_rate_avg
+float64 left_ir_frame_rate_min
+float64 left_ir_frame_rate_max
+
+float64 left_ir_delay_ms_cur
+float64 left_ir_delay_ms_avg
+float64 left_ir_delay_ms_min
+float64 left_ir_delay_ms_max
+
+# --- Right IR stream ---
+float64 right_ir_frame_rate_cur
+float64 right_ir_frame_rate_avg
+float64 right_ir_frame_rate_min
+float64 right_ir_frame_rate_max
+
+float64 right_ir_delay_ms_cur
+float64 right_ir_delay_ms_avg
+float64 right_ir_delay_ms_min
+float64 right_ir_delay_ms_max
+
 # --- Device info ---
 bool device_online
 string connection_type   # e.g. "USB2.0", "USB3.0", "GigE"
diff --git a/orbbec_description/meshes/gemini435Le/camera_screw_frame.STL b/orbbec_description/meshes/gemini435Le/camera_screw_frame.STL
new file mode 100644
index 00000000..7983fb62
Binary files /dev/null and b/orbbec_description/meshes/gemini435Le/camera_screw_frame.STL differ
diff --git a/orbbec_description/urdf/gemini_435_Le.urdf.xacro b/orbbec_description/urdf/gemini_435_Le.urdf.xacro
new file mode 100644
index 00000000..5488796d
--- /dev/null
+++ b/orbbec_description/urdf/gemini_435_Le.urdf.xacro
@@ -0,0 +1,109 @@
+<?xml version="1.0" encoding="utf-8"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="gemini_435_Le">
+  <xacro:property name="mesh_path" value="package://orbbec_description/meshes/gemini435Le/" />
+
+  <!--
+    The root link owns the complete physical model. Sensor links below are
+    coordinate frames only, so their geometry, collision, mass, and inertia
+    are not counted repeatedly.
+  -->
+  <link name="camera_screw_frame">
+    <inertial>
+      <origin xyz="0.00362974814795868 0.0327239520253598 0.0201570762447024" rpy="0 0 0" />
+      <mass value="0.52" />
+      <inertia
+        ixx="0.000996547989654272"
+        ixy="-1.27347531187977E-05"
+        ixz="-6.37274993760709E-08"
+        iyy="0.000114258647753823"
+        iyz="2.49203178977503E-06"
+        izz="0.000945634655484896" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="${mesh_path}camera_screw_frame.STL" />
+      </geometry>
+      <material name="camera_body">
+        <color rgba="0.501960784313725 0.250980392156863 0.250980392156863 1" />
+      </material>
+    </visual>
+    <!-- Primitive collision avoids using a high-triangle-count dynamic mesh. -->
+    <collision>
+      <origin xyz="0.0024025 0.03375 0.0202" rpy="0 0 0" />
+      <geometry>
+        <box size="0.075505 0.1384 0.0404" />
+      </geometry>
+    </collision>
+  </link>
+
+  <link name="camera_link" />
+  <joint name="camera_link_joint" type="fixed">
+    <origin xyz="0.0354680400000002 0.0811997 0.0201999999999996" rpy="0 0 0" />
+    <parent link="camera_screw_frame" />
+    <child link="camera_link" />
+  </joint>
+
+  <link name="camera_depth_frame" />
+  <joint name="camera_depth_joint" type="fixed">
+    <origin xyz="0 0 0" rpy="0 0 0" />
+    <parent link="camera_link" />
+    <child link="camera_depth_frame" />
+  </joint>
+
+  <link name="camera_IMU_frame" />
+  <joint name="camera_IMU_joint" type="fixed">
+    <origin xyz="-0.0550580400000001 -0.0152696999999998 9.99999999999141E-05" rpy="0 0 0" />
+    <parent link="camera_depth_frame" />
+    <child link="camera_IMU_frame" />
+  </joint>
+
+  <link name="camera_color_frame" />
+  <joint name="camera_color_joint" type="fixed">
+    <origin xyz="0 -0.0237497 0" rpy="0 0 0" />
+    <parent link="camera_depth_frame" />
+    <child link="camera_color_frame" />
+  </joint>
+
+  <link name="camera_color_optical_frame" />
+  <joint name="camera_color_optical_joint" type="fixed">
+    <origin xyz="0 0 0" rpy="-1.57079632679489 0 -1.5707963267949" />
+    <parent link="camera_color_frame" />
+    <child link="camera_color_optical_frame" />
+  </joint>
+
+  <link name="camera_left_ir_frame" />
+  <joint name="camera_left_ir_joint" type="fixed">
+    <origin xyz="0 0 0" rpy="0 0 0" />
+    <parent link="camera_depth_frame" />
+    <child link="camera_left_ir_frame" />
+  </joint>
+
+  <link name="camera_left_ir_optical_frame" />
+  <joint name="camera_left_ir_optical_joint" type="fixed">
+    <origin xyz="0 0 0" rpy="-1.57079632679489 0 -1.5707963267949" />
+    <parent link="camera_left_ir_frame" />
+    <child link="camera_left_ir_optical_frame" />
+  </joint>
+
+  <link name="camera_right_ir_frame" />
+  <joint name="camera_right_ir_joint" type="fixed">
+    <origin xyz="0 -0.0949976 0" rpy="0 0 0" />
+    <parent link="camera_depth_frame" />
+    <child link="camera_right_ir_frame" />
+  </joint>
+
+  <link name="camera_right_ir_optical_frame" />
+  <joint name="camera_right_ir_optical_joint" type="fixed">
+    <origin xyz="0 0 0" rpy="-1.57079632679489 0 -1.5707963267949" />
+    <parent link="camera_right_ir_frame" />
+    <child link="camera_right_ir_optical_frame" />
+  </joint>
+
+  <link name="camera_depth_optical_frame" />
+  <joint name="camera_depth_optical_joint" type="fixed">
+    <origin xyz="0 0 0" rpy="-1.57079632679489 0 -1.5707963267949" />
+    <parent link="camera_depth_frame" />
+    <child link="camera_depth_optical_frame" />
+  </joint>
+</robot>
diff --git a/orbbec_description/urdf/test_gemini_435_Le.urdf.xacro b/orbbec_description/urdf/test_gemini_435_Le.urdf.xacro
new file mode 100644
index 00000000..2bfb1ba5
--- /dev/null
+++ b/orbbec_description/urdf/test_gemini_435_Le.urdf.xacro
@@ -0,0 +1,12 @@
+<?xml version="1.0" encoding="utf-8"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="gemini_435_Le">
+  <xacro:include filename="gemini_435_Le.urdf.xacro" />
+
+  <!-- Define a virtual root link for model tests and robot mounting. -->
+  <link name="base_link" />
+  <joint name="base_link_to_camera_screw_frame" type="fixed">
+    <parent link="base_link" />
+    <child link="camera_screw_frame" />
+    <origin xyz="0 0 0" rpy="0 0 0" />
+  </joint>
+</robot>