Add support for dual color streams in Gemini 305 camera

orbbec_camera/config/gemini305.yaml

@@ -0,0 +1,337 @@
+# device
+device_type: camera
+camera_name: camera
+serial_number: ""
+usb_port: ""
+device_num: 1
+upgrade_firmware: ""
+preset_firmware_path: ""
+load_config_json_file_path: ""
+export_config_json_file_path: ""
+uvc_backend: libuvc
+connection_delay: 10
+publish_tf: true
+tf_publish_rate: 0.0
+ir_info_url: ""
+color_info_url: ""
+
+# network
+enumerate_net_device: true
+net_device_ip: ""
+net_device_port: 0
+force_ip_enable: false
+force_ip_mac: ""
+force_ip_address: 192.168.1.10
+force_ip_subnet_mask: 255.255.255.0
+force_ip_gateway: 192.168.1.1
+
+# device_misc
+device_access_mode: Default
+exposure_range_mode: default
+log_level: none
+log_file_name: ""
+enable_publish_extrinsic: false
+enable_d2c_viewer: false
+disparity_to_depth_mode: ""
+align_mode: SW
+align_target_stream: COLOR
+diagnostic_period: 1.0
+device_preset: Dual Color Streams
+retry_on_usb3_detection_failure: false
+enable_sync_host_time: true
+time_sync_period: 6.0
+time_domain: global
+config_file_path: ""
+enable_heartbeat: false
+gmsl_trigger_fps: 3000
+enable_gmsl_trigger: false
+disparity_range_mode: -1
+disparity_search_offset: -1
+disparity_offset_config: false
+offset_index0: -1
+offset_index1: -1
+frame_aggregate_mode: ANY
+interleave_ae_mode: laser
+interleave_frame_enable: false
+interleave_skip_enable: false
+interleave_skip_index: 1
+show_fps_enable: false
+
+# point_cloud
+point_cloud_qos: default
+enable_point_cloud: false
+point_cloud_decimation_filter_factor: 1
+enable_colored_point_cloud: false
+cloud_frame_id: ""
+ordered_pc: false
+
+# color
+color_width: 0
+color_height: 0
+color_fps: 0
+color_format: ANY
+enable_color: true
+enable_left_color: true
+enable_right_color: true
+color_qos: default
+color_camera_info_qos: default
+enable_color_auto_exposure_priority: false
+color_rotation: -1
+color_flip: false
+color_mirror: false
+color_ae_roi_left: -1
+color_ae_roi_right: -1
+color_ae_roi_top: -1
+color_ae_roi_bottom: -1
+color_exposure: -1
+color_gain: -1
+enable_color_auto_white_balance: true
+color_white_balance: -1
+enable_color_auto_exposure: true
+color_ae_max_exposure: -1
+color_brightness: -1
+color_sharpness: -1
+color_gamma: -1
+color_saturation: -1
+color_contrast: -1
+color_hue: -1
+color_backlight_compensation: -1
+color_powerline_freq: ""
+enable_color_decimation_filter: false
+color_decimation_filter_scale: -1
+color_denoising_level: -1
+enable_color_undistortion: false
+
+
+# left_color
+color_width: 0
+color_height: 0
+color_fps: 0
+color_format: ANY
+enable_color: true
+color_qos: default
+color_camera_info_qos: default
+enable_color_auto_exposure_priority: false
+color_rotation: -1
+color_flip: false
+color_mirror: false
+color_ae_roi_left: -1
+color_ae_roi_right: -1
+color_ae_roi_top: -1
+color_ae_roi_bottom: -1
+color_exposure: -1
+color_gain: -1
+enable_color_auto_white_balance: true
+color_white_balance: -1
+enable_color_auto_exposure: true
+color_ae_max_exposure: -1
+color_brightness: -1
+color_sharpness: -1
+color_gamma: -1
+color_saturation: -1
+color_contrast: -1
+color_hue: -1
+color_backlight_compensation: -1
+color_powerline_freq: ""
+enable_color_decimation_filter: false
+color_decimation_filter_scale: -1
+color_denoising_level: -1
+enable_color_undistortion: false
+
+# right_color
+color_width: 0
+color_height: 0
+color_fps: 0
+color_format: ANY
+enable_color: true
+color_qos: default
+color_camera_info_qos: default
+enable_color_auto_exposure_priority: false
+color_rotation: -1
+color_flip: false
+color_mirror: false
+color_ae_roi_left: -1
+color_ae_roi_right: -1
+color_ae_roi_top: -1
+color_ae_roi_bottom: -1
+color_exposure: -1
+color_gain: -1
+enable_color_auto_white_balance: true
+color_white_balance: -1
+enable_color_auto_exposure: true
+color_ae_max_exposure: -1
+color_brightness: -1
+color_sharpness: -1
+color_gamma: -1
+color_saturation: -1
+color_contrast: -1
+color_hue: -1
+color_backlight_compensation: -1
+color_powerline_freq: ""
+enable_color_decimation_filter: false
+color_decimation_filter_scale: -1
+color_denoising_level: -1
+enable_color_undistortion: false
+
+
+
+# depth
+depth_width: 0
+depth_height: 0
+depth_fps: 0
+depth_format: ANY
+enable_depth: false
+depth_qos: default
+depth_camera_info_qos: default
+enable_depth_auto_exposure_priority: false
+depth_precision: ""
+depth_rotation: -1
+depth_flip: false
+depth_mirror: false
+depth_ae_roi_left: -1
+depth_ae_roi_right: -1
+depth_ae_roi_top: -1
+depth_ae_roi_bottom: -1
+mean_intensity_set_point: -1
+enable_depth_scale: false
+enable_decimation_filter: false
+decimation_filter_scale: -1
+enable_hdr_merge: false
+hdr_merge_exposure_1: -1
+hdr_merge_gain_1: -1
+hdr_merge_exposure_2: -1
+hdr_merge_gain_2: -1
+enable_sequence_id_filter: false
+sequence_id_filter_id: -1
+enable_threshold_filter: false
+threshold_filter_max: -1
+threshold_filter_min: -1
+enable_hardware_noise_removal_filter: false
+hardware_noise_removal_filter_threshold: -1.0
+enable_noise_removal_filter: false
+noise_removal_filter_min_diff: 256
+noise_removal_filter_max_size: 80
+enable_spatial_filter: false
+spatial_filter_alpha: -1.0
+spatial_filter_diff_threshold: -1
+spatial_filter_magnitude: -1
+spatial_filter_radius: -1
+enable_temporal_filter: false
+temporal_filter_diff_threshold: -1.0
+temporal_filter_weight: -1.0
+enable_disparity_to_depth: false
+hole_filling_filter_mode: ""
+enable_hole_filling_filter: false
+enable_spatial_fast_filter: false
+spatial_fast_filter_radius: -1
+enable_spatial_moderate_filter: false
+spatial_moderate_filter_diff_threshold: -1
+spatial_moderate_filter_magnitude: -1
+spatial_moderate_filter_radius: -1
+# ldp
+enable_ldp: true
+ldp_power_level: -1
+
+# left_ir
+left_ir_width: 0
+left_ir_height: 0
+left_ir_fps: 0
+left_ir_format: ANY
+enable_left_ir: false
+left_ir_qos: default
+left_ir_camera_info_qos: default
+left_ir_rotation: -1
+left_ir_flip: false
+left_ir_mirror: false
+enable_left_ir_sequence_id_filter: false
+left_ir_sequence_id_filter_id: -1
+
+# right_ir
+right_ir_width: 0
+right_ir_height: 0
+right_ir_fps: 0
+right_ir_format: ANY
+enable_right_ir: false
+right_ir_qos: default
+right_ir_camera_info_qos: default
+right_ir_rotation: -1
+right_ir_flip: false
+right_ir_mirror: false
+enable_right_ir_sequence_id_filter: false
+right_ir_sequence_id_filter_id: -1
+
+# ir_common
+enable_ir_auto_exposure: false
+ir_exposure: -1
+ir_gain: -1
+ir_ae_max_exposure: -1
+ir_brightness: -1
+
+# imu
+enable_sync_output_accel_gyro: false
+enable_accel: false
+enable_accel_data_correction: true
+accel_rate: 200hz
+accel_range: 4g
+enable_gyro: false
+enable_gyro_data_correction: true
+gyro_rate: 200hz
+gyro_range: 1000dps
+linear_accel_cov: 0.01
+angular_vel_cov: 0.01
+
+depth_delay_us: 0
+color_delay_us: 0
+trigger2image_delay_us: 0
+trigger_out_delay_us: 0
+trigger_out_enabled: true
+software_trigger_enabled: true
+frames_per_trigger: 2
+software_trigger_period: 33
+
+enable_ptp_config: false
+enable_frame_sync: true
+
+noise_removal_filter_min_diff: 256
+noise_removal_filter_max_size: 80
+
+# hdr_params
+hdr_index1_laser_control: 1
+hdr_index1_depth_exposure: 1
+hdr_index1_depth_gain: 16
+hdr_index1_ir_brightness: 30
+hdr_index1_ir_ae_max_exposure: 30458
+hdr_index0_laser_control: 1
+hdr_index0_depth_exposure: 7500
+hdr_index0_depth_gain: 16
+hdr_index0_ir_brightness: 90
+hdr_index0_ir_ae_max_exposure: 30458
+
+# laser_params
+laser_index1_laser_control: 0
+laser_index1_depth_exposure: 3000
+laser_index1_depth_gain: 16
+laser_index1_ir_brightness: 60
+laser_index1_ir_ae_max_exposure: 17000
+laser_index0_laser_control: 1
+laser_index0_depth_exposure: 3000
+laser_index0_depth_gain: 16
+laser_index0_ir_brightness: 60
+laser_index0_ir_ae_max_exposure: 30000
+
+# publishers and transports
+color_image_transport_plugins:
+  - image_transport/compressed
+  - image_transport/raw
+  - image_transport/theora
+depth_image_transport_plugins:
+  - image_transport/compressedDepth
+  - image_transport/raw
+left_ir_image_transport_plugins:
+  - image_transport/compressed
+  - image_transport/raw
+  - image_transport/theora
+right_ir_image_transport_plugins:
+  - image_transport/compressed
+  - image_transport/raw
+  - image_transport/theora

orbbec_camera/include/orbbec_camera/constants.h

@@ -135,5 +135,6 @@ const int32_t CUSTOM_ADVANTECH_GEMINI_336L_PID = 0x0817;  // Custom Advantech Ge
 const int32_t DABAI_MAX_PID = 0x069a;                     // dabai max
 const int32_t GEMINI_338_PID = 0x0818;                    // Gemini 338
 const uint16_t GEMINI_435Le_PID = 0x815;                  // Gemini 435Le
+const uint16_t GEMINI_305_PID = 0x0840;                   // Gemini 305
 
 }  // namespace orbbec_camera

orbbec_camera/include/orbbec_camera/ob_camera_node.h

@@ -121,6 +121,8 @@ using GetUserCalibParams = orbbec_camera_msgs::srv::GetUserCalibParams;
 typedef std::pair<ob_stream_type, int> stream_index_pair;
 
 const stream_index_pair COLOR{OB_STREAM_COLOR, 0};
+const stream_index_pair COLOR_LEFT{OB_STREAM_COLOR_LEFT, 0};
+const stream_index_pair COLOR_RIGHT{OB_STREAM_COLOR_RIGHT, 0};
 const stream_index_pair DEPTH{OB_STREAM_DEPTH, 0};
 const stream_index_pair INFRA0{OB_STREAM_IR, 0};
 const stream_index_pair INFRA1{OB_STREAM_IR_LEFT, 0};
@@ -130,12 +132,15 @@ const stream_index_pair LIDAR{OB_STREAM_LIDAR, 0};
 const stream_index_pair GYRO{OB_STREAM_GYRO, 0};
 const stream_index_pair ACCEL{OB_STREAM_ACCEL, 0};
 
-const std::vector<stream_index_pair> IMAGE_STREAMS = {COLOR, DEPTH, INFRA0, INFRA1, INFRA2, LIDAR};
+const std::vector<stream_index_pair> IMAGE_STREAMS = {COLOR,  COLOR_LEFT, COLOR_RIGHT, DEPTH,
+                                                      INFRA0, INFRA1,     INFRA2,      LIDAR};
 
 const std::vector<stream_index_pair> HID_STREAMS = {GYRO, ACCEL};
 
 const std::map<OBStreamType, OBFrameType> STREAM_TYPE_TO_FRAME_TYPE = {
     {OB_STREAM_COLOR, OB_FRAME_COLOR},
+    {OB_STREAM_COLOR_LEFT, OB_FRAME_COLOR_LEFT},
+    {OB_STREAM_COLOR_RIGHT, OB_FRAME_COLOR_RIGHT},
     {OB_STREAM_DEPTH, OB_FRAME_DEPTH},
     {OB_STREAM_IR, OB_FRAME_IR},
     {OB_STREAM_IR_LEFT, OB_FRAME_IR_LEFT},
@@ -423,7 +428,8 @@ class OBCameraNode {
 
   void onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set);
 
-  std::shared_ptr<ob::Frame> softwareDecodeColorFrame(const std::shared_ptr<ob::Frame>& frame);
+  std::shared_ptr<ob::Frame> softwareDecodeColorFrame(const std::shared_ptr<ob::Frame>& frame,
+                                                      const stream_index_pair& stream_index);
 
   bool decodeColorFrameToBuffer(const std::shared_ptr<ob::Frame>& frame, uint8_t* buffer);
 
@@ -437,6 +443,10 @@ class OBCameraNode {
 
   void onNewColorFrameCallback();
 
+  void onNewLeftColorFrameCallback();
+
+  void onNewRightColorFrameCallback();
+
   void saveImageToFile(const stream_index_pair& stream_index, const cv::Mat& image,
                        const sensor_msgs::msg::Image& image_msg);
 
@@ -456,6 +466,7 @@ class OBCameraNode {
   void FillImuDataCopy(const IMUData& imu_data, std::deque<sensor_msgs::msg::Imu>& imu_msgs);
 
   bool setupFormatConvertType(OBFormat format);
+  bool setupFormatConvertType(OBFormat format, ob::FormatConvertFilter& filter);
 
   orbbec_camera_msgs::msg::IMUInfo createIMUInfo(const stream_index_pair& stream_index);
 
@@ -531,6 +542,8 @@ class OBCameraNode {
   std::map<stream_index_pair, int> unit_step_size_;
   std::vector<int> compression_params_;
   ob::FormatConvertFilter format_convert_filter_;
+  ob::FormatConvertFilter format_convert_filter_left_;
+  ob::FormatConvertFilter format_convert_filter_right_;
 
   std::map<stream_index_pair, bool> enable_stream_;
   std::map<stream_index_pair, bool> flip_stream_;
@@ -700,7 +713,13 @@ class OBCameraNode {
   bool enable_gyro_data_correction_ = true;
   // mjpeg decoder
   std::shared_ptr<JPEGDecoder> jpeg_decoder_ = nullptr;
+  std::shared_ptr<JPEGDecoder> jpeg_decoder_left_ = nullptr;
+  std::shared_ptr<JPEGDecoder> jpeg_decoder_right_ = nullptr;
   uint8_t* rgb_buffer_ = nullptr;
+  uint8_t* rgb_buffer_left_ = nullptr;
+  uint8_t* rgb_buffer_right_ = nullptr;
+  bool is_left_color_frame_decoded_ = false;
+  bool is_right_color_frame_decoded_ = false;
   bool is_color_frame_decoded_ = false;
   std::recursive_mutex device_lock_;
   // For color
@@ -709,6 +728,18 @@ class OBCameraNode {
   std::mutex color_frame_queue_lock_;
   std::condition_variable color_frame_queue_cv_;
 
+  // For left color
+  std::queue<std::shared_ptr<ob::FrameSet>> left_color_frame_queue_;
+  std::shared_ptr<std::thread> leftColorFrameThread_ = nullptr;
+  std::mutex left_color_frame_queue_lock_;
+  std::condition_variable left_color_frame_queue_cv_;
+
+  // For right color
+  std::queue<std::shared_ptr<ob::FrameSet>> right_color_frame_queue_;
+  std::shared_ptr<std::thread> rightColorFrameThread_ = nullptr;
+  std::mutex right_color_frame_queue_lock_;
+  std::condition_variable right_color_frame_queue_cv_;
+
   bool ordered_pc_ = false;
   bool enable_depth_scale_ = true;
   int depth_downscale_ = 1;
@@ -794,6 +825,8 @@ class OBCameraNode {
   std::string cloud_frame_id_;
   std::vector<std::shared_ptr<ob::Filter>> depth_filter_list_;
   std::vector<std::shared_ptr<ob::Filter>> color_filter_list_;
+  std::vector<std::shared_ptr<ob::Filter>> left_color_filter_list_;
+  std::vector<std::shared_ptr<ob::Filter>> right_color_filter_list_;
   std::vector<std::shared_ptr<ob::Filter>> left_ir_filter_list_;
   std::vector<std::shared_ptr<ob::Filter>> right_ir_filter_list_;
 

orbbec_camera/launch/gemini305.launch.py

@@ -91,6 +91,8 @@ def generate_launch_description():
         DeclareLaunchArgument('color_fps', default_value='0'),
         DeclareLaunchArgument('color_format', default_value='ANY'),
         DeclareLaunchArgument('enable_color', default_value='true'),
+        DeclareLaunchArgument('enable_left_color', default_value='true'),
+        DeclareLaunchArgument('enable_right_color', default_value='true'),
         DeclareLaunchArgument('color_qos', default_value='default'),
         DeclareLaunchArgument('color_camera_info_qos', default_value='default'),
         DeclareLaunchArgument('enable_color_auto_exposure_priority', default_value='false'),
@@ -250,7 +252,7 @@ def generate_launch_description():
         DeclareLaunchArgument('diagnostic_period', default_value='1.0'), # seconds
         DeclareLaunchArgument('enable_laser', default_value='true'),
         DeclareLaunchArgument('depth_precision', default_value=''),
-        DeclareLaunchArgument('device_preset', default_value='Default'),
+        DeclareLaunchArgument('device_preset', default_value='Default'), # Default, High Accuracy, Close Range High Accuracy, Factory Calib, Dual Color Streams, Custom
         DeclareLaunchArgument('retry_on_usb3_detection_failure', default_value='false'),
         DeclareLaunchArgument('laser_energy_level', default_value='-1'),
         DeclareLaunchArgument('enable_sync_host_time', default_value='true'),

orbbec_camera/src/ob_camera_node.cpp

@@ -48,6 +48,8 @@ OBCameraNode::OBCameraNode(rclcpp::Node *node, std::shared_ptr<ob::Device> devic
                      "OBCameraNode: use_intra_process: " << (use_intra_process ? "ON" : "OFF"));
   is_running_.store(true);
   stream_name_[COLOR] = "color";
+  stream_name_[COLOR_LEFT] = "left_color";
+  stream_name_[COLOR_RIGHT] = "right_color";
   stream_name_[DEPTH] = "depth";
   stream_name_[INFRA0] = "ir";
   stream_name_[INFRA1] = "left_ir";
@@ -61,9 +63,28 @@ OBCameraNode::OBCameraNode(rclcpp::Node *node, std::shared_ptr<ob::Device> devic
   setupDefaultImageFormat();
   setupTopics();
 #if defined(USE_RK_HW_DECODER)
-  jpeg_decoder_ = std::make_unique<RKJPEGDecoder>(width_[COLOR], height_[COLOR]);
+  if (enable_stream_[COLOR] && width_.count(COLOR) && height_.count(COLOR)) {
+    jpeg_decoder_ = std::make_unique<RKJPEGDecoder>(width_[COLOR], height_[COLOR]);
+  }
+  if (enable_stream_[COLOR_LEFT] && width_.count(COLOR_LEFT) && height_.count(COLOR_LEFT)) {
+    jpeg_decoder_left_ = std::make_unique<RKJPEGDecoder>(width_[COLOR_LEFT], height_[COLOR_LEFT]);
+  }
+  if (enable_stream_[COLOR_RIGHT] && width_.count(COLOR_RIGHT) && height_.count(COLOR_RIGHT)) {
+    jpeg_decoder_right_ =
+        std::make_unique<RKJPEGDecoder>(width_[COLOR_RIGHT], height_[COLOR_RIGHT]);
+  }
 #elif defined(USE_NV_HW_DECODER)
-  jpeg_decoder_ = std::make_unique<JetsonNvJPEGDecoder>(width_[COLOR], height_[COLOR]);
+  if (enable_stream_[COLOR] && width_.count(COLOR) && height_.count(COLOR)) {
+    jpeg_decoder_ = std::make_unique<JetsonNvJPEGDecoder>(width_[COLOR], height_[COLOR]);
+  }
+  if (enable_stream_[COLOR_LEFT] && width_.count(COLOR_LEFT) && height_.count(COLOR_LEFT)) {
+    jpeg_decoder_left_ =
+        std::make_unique<JetsonNvJPEGDecoder>(width_[COLOR_LEFT], height_[COLOR_LEFT]);
+  }
+  if (enable_stream_[COLOR_RIGHT] && width_.count(COLOR_RIGHT) && height_.count(COLOR_RIGHT)) {
+    jpeg_decoder_right_ =
+        std::make_unique<JetsonNvJPEGDecoder>(width_[COLOR_RIGHT], height_[COLOR_RIGHT]);
+  }
 #endif
   if (enable_d2c_viewer_) {
     auto rgb_qos = getRMWQosProfileFromString(image_qos_[COLOR]);
@@ -73,6 +94,12 @@ OBCameraNode::OBCameraNode(rclcpp::Node *node, std::shared_ptr<ob::Device> devic
   if (enable_stream_[COLOR]) {
     rgb_buffer_ = new uint8_t[width_[COLOR] * height_[COLOR] * 4];
   }
+  if (enable_stream_[COLOR_LEFT]) {
+    rgb_buffer_left_ = new uint8_t[width_[COLOR_LEFT] * height_[COLOR_LEFT] * 4];
+  }
+  if (enable_stream_[COLOR_RIGHT]) {
+    rgb_buffer_right_ = new uint8_t[width_[COLOR_RIGHT] * height_[COLOR_RIGHT] * 4];
+  }
   if (enable_colored_point_cloud_ && enable_stream_[DEPTH] && enable_stream_[COLOR]) {
     rgb_point_cloud_buffer_size_ = width_[COLOR] * height_[COLOR] * sizeof(OBColorPoint);
     xy_table_data_size_ = width_[DEPTH] * height_[DEPTH] * 2;
@@ -175,6 +202,14 @@ void OBCameraNode::clean() noexcept {
       color_frame_queue_cv_.notify_all();
       colorFrameThread_->join();
     }
+    if (leftColorFrameThread_ && leftColorFrameThread_->joinable()) {
+      left_color_frame_queue_cv_.notify_all();
+      leftColorFrameThread_->join();
+    }
+    if (rightColorFrameThread_ && rightColorFrameThread_->joinable()) {
+      right_color_frame_queue_cv_.notify_all();
+      rightColorFrameThread_->join();
+    }
   } catch (...) {
     RCLCPP_WARN_STREAM(logger_, "Exception while stopping color frame thread");
   }
@@ -201,6 +236,20 @@ void OBCameraNode::clean() noexcept {
   try {
     delete[] rgb_buffer_;
     rgb_buffer_ = nullptr;
+    delete[] rgb_buffer_left_;
+    rgb_buffer_left_ = nullptr;
+    delete[] rgb_buffer_right_;
+    rgb_buffer_right_ = nullptr;
+
+    if (jpeg_decoder_) {
+      jpeg_decoder_.reset();
+    }
+    if (jpeg_decoder_left_) {
+      jpeg_decoder_left_.reset();
+    }
+    if (jpeg_decoder_right_) {
+      jpeg_decoder_right_.reset();
+    }
   } catch (...) {
     RCLCPP_WARN_STREAM(logger_, "Exception while cleaning up buffers");
   }
@@ -210,6 +259,25 @@ void OBCameraNode::clean() noexcept {
 }
 
 void OBCameraNode::setupDevices() {
+  if (!device_preset_.empty()) {
+    try {
+      RCLCPP_INFO_STREAM(logger_, "Available presets:");
+      auto preset_list = device_->getAvailablePresetList();
+      for (uint32_t i = 0; i < preset_list->getCount(); i++) {
+        RCLCPP_INFO_STREAM(logger_, "Preset " << i << ": " << preset_list->getName(i));
+      }
+      RCLCPP_INFO_STREAM(logger_, "Load device preset: " << device_preset_);
+      TRY_EXECUTE_BLOCK(device_->loadPreset(device_preset_.c_str()));
+      RCLCPP_INFO_STREAM(logger_, "Device preset " << device_->getCurrentPresetName() << " loaded");
+    } catch (const ob::Error &e) {
+      RCLCPP_ERROR_STREAM(logger_, "Failed to load device preset: " << e.getMessage());
+    } catch (const std::exception &e) {
+      RCLCPP_ERROR_STREAM(logger_, "Failed to load device preset: " << e.what());
+    } catch (...) {
+      RCLCPP_ERROR_STREAM(logger_, "Failed to load device preset");
+    }
+  }
+
   if (!preset_resolution_config_.empty()) {
     OBPresetResolutionConfig presetResolutionConfig;
     std::istringstream iss(preset_resolution_config_);
@@ -308,7 +376,8 @@ void OBCameraNode::setupDevices() {
     RCLCPP_INFO_STREAM(logger_, "Create align filter");
     align_filter_ = std::make_unique<ob::Align>(align_target_stream_);
   }
-  if (device_->isPropertySupported(OB_PROP_DISPARITY_TO_DEPTH_BOOL, OB_PERMISSION_READ_WRITE) &&
+  if (enable_stream_[DEPTH] &&
+      device_->isPropertySupported(OB_PROP_DISPARITY_TO_DEPTH_BOOL, OB_PERMISSION_READ_WRITE) &&
       device_->isPropertySupported(OB_PROP_SDK_DISPARITY_TO_DEPTH_BOOL, OB_PERMISSION_READ_WRITE)) {
     if (disparity_to_depth_mode_ == "HW") {
       device_->setBoolProperty(OB_PROP_DISPARITY_TO_DEPTH_BOOL, 1);
@@ -362,24 +431,6 @@ void OBCameraNode::setupDevices() {
     RCLCPP_INFO_STREAM(logger_, "Setting laser control to " << enable_laser_);
     TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_LASER_BOOL, enable_laser_);
   }
-  if (!device_preset_.empty()) {
-    try {
-      RCLCPP_INFO_STREAM(logger_, "Available presets:");
-      auto preset_list = device_->getAvailablePresetList();
-      for (uint32_t i = 0; i < preset_list->getCount(); i++) {
-        RCLCPP_INFO_STREAM(logger_, "Preset " << i << ": " << preset_list->getName(i));
-      }
-      RCLCPP_INFO_STREAM(logger_, "Load device preset: " << device_preset_);
-      TRY_EXECUTE_BLOCK(device_->loadPreset(device_preset_.c_str()));
-      RCLCPP_INFO_STREAM(logger_, "Device preset " << device_->getCurrentPresetName() << " loaded");
-    } catch (const ob::Error &e) {
-      RCLCPP_ERROR_STREAM(logger_, "Failed to load device preset: " << e.getMessage());
-    } catch (const std::exception &e) {
-      RCLCPP_ERROR_STREAM(logger_, "Failed to load device preset: " << e.what());
-    } catch (...) {
-      RCLCPP_ERROR_STREAM(logger_, "Failed to load device preset");
-    }
-  }
   if (!depth_work_mode_.empty() &&
       device_->isPropertySupported(OB_STRUCT_CURRENT_DEPTH_ALG_MODE, OB_PERMISSION_READ_WRITE)) {
     auto depthModeList = device_->getDepthWorkModeList();
@@ -503,7 +554,8 @@ void OBCameraNode::setupDevices() {
     }
   }
 
-  if (device_->isPropertySupported(OB_PROP_DEPTH_SOFT_FILTER_BOOL, OB_PERMISSION_READ_WRITE)) {
+  if (enable_stream_[DEPTH] &&
+      device_->isPropertySupported(OB_PROP_DEPTH_SOFT_FILTER_BOOL, OB_PERMISSION_READ_WRITE)) {
     device_->setBoolProperty(OB_PROP_DEPTH_SOFT_FILTER_BOOL, enable_noise_removal_filter_);
     RCLCPP_INFO_STREAM(
         logger_, "Setting noise removal filter:" << (enable_noise_removal_filter_ ? "ON" : "OFF"));
@@ -776,7 +828,8 @@ void OBCameraNode::setupDevices() {
     TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_IR_LONG_EXPOSURE_BOOL, enable_ir_long_exposure_);
   }
 
-  if (device_->isPropertySupported(OB_PROP_DEPTH_MAX_DIFF_INT, OB_PERMISSION_WRITE)) {
+  if (enable_stream_[DEPTH] &&
+      device_->isPropertySupported(OB_PROP_DEPTH_MAX_DIFF_INT, OB_PERMISSION_WRITE)) {
     auto default_noise_removal_filter_min_diff =
         device_->getIntProperty(OB_PROP_DEPTH_MAX_DIFF_INT);
     RCLCPP_INFO_STREAM(logger_, "default noise removal filter min diff: "
@@ -800,7 +853,8 @@ void OBCameraNode::setupDevices() {
     }
   }
 
-  if (device_->isPropertySupported(OB_PROP_DEPTH_MAX_SPECKLE_SIZE_INT, OB_PERMISSION_WRITE)) {
+  if (enable_stream_[DEPTH] &&
+      device_->isPropertySupported(OB_PROP_DEPTH_MAX_SPECKLE_SIZE_INT, OB_PERMISSION_WRITE)) {
     auto default_noise_removal_filter_max_size =
         device_->getIntProperty(OB_PROP_DEPTH_MAX_SPECKLE_SIZE_INT);
     RCLCPP_INFO_STREAM(logger_, "default noise removal filter max size: "
@@ -903,11 +957,25 @@ void OBCameraNode::setupDevices() {
   }
 }
 void OBCameraNode::setupColorPostProcessFilter() {
-  auto color_sensor = device_->getSensor(OB_SENSOR_COLOR);
+  try {
-  color_filter_list_ = color_sensor->createRecommendedFilters();
+    auto color_sensor = device_->getSensor(OB_SENSOR_COLOR);
-  if (color_filter_list_.empty()) {
+    if (color_sensor) {
-    RCLCPP_WARN_STREAM(logger_, "Failed to get color sensor filter list");
+      color_filter_list_ = color_sensor->createRecommendedFilters();
-    //   return;
+    }
+  } catch (const std::exception &e) {
+    RCLCPP_DEBUG_STREAM(logger_, "Main color sensor not found, trying left/right color sensors");
+    auto left_color_sensor = device_->getSensor(OB_SENSOR_COLOR_LEFT);
+    if (left_color_sensor) {
+      left_color_filter_list_ = left_color_sensor->createRecommendedFilters();
+    }
+    auto right_color_sensor = device_->getSensor(OB_SENSOR_COLOR_RIGHT);
+    if (right_color_sensor) {
+      right_color_filter_list_ = right_color_sensor->createRecommendedFilters();
+    }
+  }
+  if (color_filter_list_.empty() && left_color_filter_list_.empty() &&
+      right_color_filter_list_.empty()) {
+    RCLCPP_WARN_STREAM(logger_, "Failed to get any color sensor filter list");
   }
   for (size_t i = 0; i < color_filter_list_.size(); i++) {
     auto filter = color_filter_list_[i];
@@ -959,6 +1027,20 @@ void OBCameraNode::setupColorPostProcessFilter() {
       }
     }
   }
+  if (pid == GEMINI_305_PID) {
+    if (enable_color_decimation_filter_) {
+      if (!left_color_filter_list_.empty()) {
+        auto decimation_filter = std::make_shared<ob::DecimationFilter>();
+        decimation_filter->enable(true);
+        left_color_filter_list_.push_back(decimation_filter);
+      }
+      if (!right_color_filter_list_.empty()) {
+        auto decimation_filter = std::make_shared<ob::DecimationFilter>();
+        decimation_filter->enable(true);
+        right_color_filter_list_.push_back(decimation_filter);
+      }
+    }
+  }
 }
 void OBCameraNode::setupLeftIrPostProcessFilter() {
   auto device_info = device_->getDeviceInfo();
@@ -1196,6 +1278,10 @@ void OBCameraNode::selectBaseStream() {
     base_stream_ = INFRA1;
   } else if (enable_stream_[INFRA2]) {
     base_stream_ = INFRA2;
+  } else if (enable_stream_[COLOR_LEFT]) {
+    base_stream_ = COLOR_LEFT;
+  } else if (enable_stream_[COLOR_RIGHT]) {
+    base_stream_ = COLOR_RIGHT;
   } else if (enable_stream_[COLOR]) {
     base_stream_ = COLOR;
   }
@@ -1455,6 +1541,14 @@ void OBCameraNode::startStreams() {
   if (enable_stream_[COLOR] && !colorFrameThread_) {
     colorFrameThread_ = std::make_shared<std::thread>([this]() { onNewColorFrameCallback(); });
   }
+  if (enable_stream_[COLOR_LEFT] && !leftColorFrameThread_) {
+    leftColorFrameThread_ =
+        std::make_shared<std::thread>([this]() { onNewLeftColorFrameCallback(); });
+  }
+  if (enable_stream_[COLOR_RIGHT] && !rightColorFrameThread_) {
+    rightColorFrameThread_ =
+        std::make_shared<std::thread>([this]() { onNewRightColorFrameCallback(); });
+  }
   if (enable_frame_sync_) {
     RCLCPP_INFO_STREAM(logger_, "Enable frame sync");
     TRY_EXECUTE_BLOCK(pipeline_->enableFrameSync());
@@ -1750,6 +1844,14 @@ void OBCameraNode::setupDefaultImageFormat() {
   image_format_[COLOR] = CV_8UC3;
   encoding_[COLOR] = sensor_msgs::image_encodings::RGB8;
   unit_step_size_[COLOR] = 3 * sizeof(uint8_t);
+
+  image_format_[COLOR_LEFT] = CV_8UC3;
+  encoding_[COLOR_LEFT] = sensor_msgs::image_encodings::RGB8;
+  unit_step_size_[COLOR_LEFT] = 3 * sizeof(uint8_t);
+
+  image_format_[COLOR_RIGHT] = CV_8UC3;
+  encoding_[COLOR_RIGHT] = sensor_msgs::image_encodings::RGB8;
+  unit_step_size_[COLOR_RIGHT] = 3 * sizeof(uint8_t);
 }
 
 void OBCameraNode::getParameters() {
@@ -1975,6 +2077,27 @@ void OBCameraNode::getParameters() {
   auto device_info = device_->getDeviceInfo();
   CHECK_NOTNULL(device_info.get());
   auto pid = device_info->getPid();
+
+  if (device_preset_ == "Dual Color Streams") {
+    RCLCPP_INFO_STREAM(logger_,
+                       "Using Double Color preset, only left and right color streams are enabled.");
+    enable_stream_[COLOR] = false;
+    enable_stream_[DEPTH] = false;
+    enable_stream_[INFRA0] = false;
+    enable_stream_[INFRA1] = false;
+    enable_stream_[INFRA2] = false;
+    enable_stream_[LIDAR] = false;
+    enable_stream_[COLOR_LEFT] = true;
+    enable_stream_[COLOR_RIGHT] = true;
+
+    enable_point_cloud_ = false;
+    enable_colored_point_cloud_ = false;
+    depth_registration_ = false;
+    enable_d2c_viewer_ = false;
+    enable_depth_filter_ = false;
+    enable_color_undistortion_ = false;
+  }
+
   if (isOpenNIDevice(pid)) {
     time_domain_ = "system";
   }
@@ -2064,10 +2187,18 @@ void OBCameraNode::setupTopics() {
   try {
     getParameters();
     setupDevices();
-    setupDepthPostProcessFilter();
+    if (enable_stream_[DEPTH]) {
-    setupColorPostProcessFilter();
+      setupDepthPostProcessFilter();
-    setupRightIrPostProcessFilter();
+    }
-    setupLeftIrPostProcessFilter();
+    if (enable_stream_[COLOR] || enable_stream_[COLOR_LEFT] || enable_stream_[COLOR_RIGHT]) {
+      setupColorPostProcessFilter();
+    }
+    if (enable_stream_[INFRA2]) {
+      setupRightIrPostProcessFilter();
+    }
+    if (enable_stream_[INFRA1]) {
+      setupLeftIrPostProcessFilter();
+    }
     setupProfiles();
     setupCameraInfo();
     selectBaseStream();
@@ -2699,21 +2830,51 @@ std::shared_ptr<ob::Frame> OBCameraNode::processLeftIrFrameFilter(
 }
 std::shared_ptr<ob::Frame> OBCameraNode::processColorFrameFilter(
     std::shared_ptr<ob::Frame> &frame) {
-  if (frame == nullptr || frame->getType() != OB_FRAME_COLOR) {
+  if (frame == nullptr) {
     return nullptr;
   }
-  for (size_t i = 0; i < color_filter_list_.size(); i++) {
+  auto frame_type = frame->getType();
-    auto filter = color_filter_list_[i];
+  if (frame_type == OB_FRAME_COLOR) {
-    CHECK_NOTNULL(filter.get());
+    for (size_t i = 0; i < color_filter_list_.size(); i++) {
-    if (filter->isEnabled() && frame != nullptr) {
+      auto filter = color_filter_list_[i];
-      frame = filter->process(frame);
+      CHECK_NOTNULL(filter.get());
-      if (frame == nullptr) {
+      if (filter->isEnabled() && frame != nullptr) {
-        RCLCPP_ERROR_STREAM(logger_, "Color filter process failed");
+        frame = filter->process(frame);
-        break;
+        if (frame == nullptr) {
+          RCLCPP_ERROR_STREAM(logger_, "Color filter process failed");
+          break;
+        }
       }
     }
+    return frame;
+  } else if (frame_type == OB_FRAME_COLOR_LEFT) {
+    for (size_t i = 0; i < left_color_filter_list_.size(); i++) {
+      auto filter = left_color_filter_list_[i];
+      CHECK_NOTNULL(filter.get());
+      if (filter->isEnabled() && frame != nullptr) {
+        frame = filter->process(frame);
+        if (frame == nullptr) {
+          RCLCPP_ERROR_STREAM(logger_, "Left color filter process failed");
+          break;
+        }
+      }
+    }
+    return frame;
+  } else if (frame_type == OB_FRAME_COLOR_RIGHT) {
+    for (size_t i = 0; i < right_color_filter_list_.size(); i++) {
+      auto filter = right_color_filter_list_[i];
+      CHECK_NOTNULL(filter.get());
+      if (filter->isEnabled() && frame != nullptr) {
+        frame = filter->process(frame);
+        if (frame == nullptr) {
+          RCLCPP_ERROR_STREAM(logger_, "Right color filter process failed");
+          break;
+        }
+      }
+    }
+    return frame;
   }
-  return frame;
+  return nullptr;
 }
 std::shared_ptr<ob::Frame> OBCameraNode::processDepthFrameFilter(
     std::shared_ptr<ob::Frame> &frame) {
@@ -2876,6 +3037,8 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set
     auto color_frame = frame_set->getFrame(OB_FRAME_COLOR);
     auto left_ir_frame = frame_set->getFrame(OB_FRAME_IR_LEFT);
     auto right_ir_frame = frame_set->getFrame(OB_FRAME_IR_RIGHT);
+    auto left_color_frame = frame_set->getFrame(OB_FRAME_COLOR_LEFT);
+    auto right_color_frame = frame_set->getFrame(OB_FRAME_COLOR_RIGHT);
     if (depth_frame) {
       setDisparitySearchOffset();
       setDepthAutoExposureROI();
@@ -2891,6 +3054,14 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set
 
       fps_counter_color_->tick();
     }
+    if (left_color_frame) {
+      left_color_frame = processColorFrameFilter(left_color_frame);
+      frame_set->pushFrame(left_color_frame);
+    }
+    if (right_color_frame) {
+      right_color_frame = processColorFrameFilter(right_color_frame);
+      frame_set->pushFrame(right_color_frame);
+    }
     if (left_ir_frame && isGemini335PID(pid)) {
       left_ir_frame = processLeftIrFrameFilter(left_ir_frame);
       frame_set->pushFrame(left_ir_frame);
@@ -2925,10 +3096,23 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set
     } else {
       publishPointCloud(frame_set);
     }
+
+    if (enable_stream_[COLOR_LEFT] && left_color_frame) {
+      std::unique_lock<std::mutex> lock(left_color_frame_queue_lock_);
+      left_color_frame_queue_.push(frame_set);
+      left_color_frame_queue_cv_.notify_all();
+    }
+    if (enable_stream_[COLOR_RIGHT] && right_color_frame) {
+      std::unique_lock<std::mutex> lock(right_color_frame_queue_lock_);
+      right_color_frame_queue_.push(frame_set);
+      right_color_frame_queue_cv_.notify_all();
+    }
+
     for (const auto &stream_index : IMAGE_STREAMS) {
       if (enable_stream_[stream_index]) {
         auto frame_type = STREAM_TYPE_TO_FRAME_TYPE.at(stream_index.first);
-        if (frame_type == OB_FRAME_COLOR) {
+        if (frame_type == OB_FRAME_COLOR || frame_type == OB_FRAME_COLOR_LEFT ||
+            frame_type == OB_FRAME_COLOR_RIGHT) {
           continue;
         }
 
@@ -2967,8 +3151,44 @@ void OBCameraNode::onNewColorFrameCallback() {
   RCLCPP_INFO_STREAM(logger_, "Color frame thread exit!");
 }
 
+void OBCameraNode::onNewLeftColorFrameCallback() {
+  while (enable_stream_[COLOR_LEFT] && rclcpp::ok() && is_running_.load()) {
+    std::unique_lock<std::mutex> lock(left_color_frame_queue_lock_);
+    left_color_frame_queue_cv_.wait(
+        lock, [this]() { return !left_color_frame_queue_.empty() || !(is_running_.load()); });
+
+    if (!rclcpp::ok() || !is_running_.load()) {
+      break;
+    }
+    std::shared_ptr<ob::FrameSet> frameSet = left_color_frame_queue_.front();
+    is_left_color_frame_decoded_ =
+        decodeColorFrameToBuffer(frameSet->getFrame(OB_FRAME_COLOR_LEFT), rgb_buffer_left_);
+    onNewFrameCallback(frameSet->getFrame(OB_FRAME_COLOR_LEFT), COLOR_LEFT);
+    left_color_frame_queue_.pop();
+  }
+  RCLCPP_INFO_STREAM(logger_, "Left Color frame thread exit!");
+}
+
+void OBCameraNode::onNewRightColorFrameCallback() {
+  while (enable_stream_[COLOR_RIGHT] && rclcpp::ok() && is_running_.load()) {
+    std::unique_lock<std::mutex> lock(right_color_frame_queue_lock_);
+    right_color_frame_queue_cv_.wait(
+        lock, [this]() { return !right_color_frame_queue_.empty() || !(is_running_.load()); });
+
+    if (!rclcpp::ok() || !is_running_.load()) {
+      break;
+    }
+    std::shared_ptr<ob::FrameSet> frameSet = right_color_frame_queue_.front();
+    is_right_color_frame_decoded_ =
+        decodeColorFrameToBuffer(frameSet->getFrame(OB_FRAME_COLOR_RIGHT), rgb_buffer_right_);
+    onNewFrameCallback(frameSet->getFrame(OB_FRAME_COLOR_RIGHT), COLOR_RIGHT);
+    right_color_frame_queue_.pop();
+  }
+  RCLCPP_INFO_STREAM(logger_, "Right Color frame thread exit!");
+}
+
 std::shared_ptr<ob::Frame> OBCameraNode::softwareDecodeColorFrame(
-    const std::shared_ptr<ob::Frame> &frame) {
+    const std::shared_ptr<ob::Frame> &frame, const stream_index_pair &stream_index) {
   if (frame == nullptr) {
     return nullptr;
   }
@@ -2981,11 +3201,32 @@ std::shared_ptr<ob::Frame> OBCameraNode::softwareDecodeColorFrame(
   if (frame->getFormat() == OB_FORMAT_Y16 || frame->getFormat() == OB_FORMAT_Y8) {
     return frame;
   }
-  if (!setupFormatConvertType(frame->getFormat())) {
+
+  ob::FormatConvertFilter *filter = &format_convert_filter_;
+  if (stream_index == COLOR_LEFT) {
+    filter = &format_convert_filter_left_;
+  } else if (stream_index == COLOR_RIGHT) {
+    filter = &format_convert_filter_right_;
+  }
+
+  if (!setupFormatConvertType(frame->getFormat(), *filter)) {
     RCLCPP_ERROR(logger_, "Unsupported color format: %d", frame->getFormat());
     return nullptr;
   }
-  auto color_frame = format_convert_filter_.process(frame);
+
+  std::shared_ptr<ob::Frame> color_frame;
+  try {
+    color_frame = filter->process(frame);
+  } catch (const ob::Error &e) {
+    RCLCPP_ERROR_STREAM(logger_, "Format convert failed: " << e.getMessage());
+    return nullptr;
+  } catch (const std::exception &e) {
+    RCLCPP_ERROR_STREAM(logger_, "Format convert failed: " << e.what());
+    return nullptr;
+  } catch (...) {
+    RCLCPP_ERROR(logger_, "Format convert failed: unknown error");
+    return nullptr;
+  }
   if (color_frame == nullptr) {
     RCLCPP_ERROR_SKIPFIRST_THROTTLE(logger_, *(node_->get_clock()), 1000,
                                     "Failed to convert frame to RGB format");
@@ -2999,24 +3240,57 @@ bool OBCameraNode::decodeColorFrameToBuffer(const std::shared_ptr<ob::Frame> &fr
   if (frame == nullptr) {
     return false;
   }
-  if (!rgb_buffer_) {
+  if (!buffer) {
     return false;
   }
-  CHECK_NOTNULL(image_publishers_[COLOR]);
+
-  bool has_subscriber = image_publishers_[COLOR]->get_subscription_count() > 0;
+  stream_index_pair stream_index = COLOR;
-  if (enable_colored_point_cloud_ && depth_registration_cloud_pub_->get_subscription_count() > 0) {
+  switch (frame->getType()) {
+    case OB_FRAME_COLOR:
+      stream_index = COLOR;
+      break;
+    case OB_FRAME_COLOR_LEFT:
+      stream_index = COLOR_LEFT;
+      break;
+    case OB_FRAME_COLOR_RIGHT:
+      stream_index = COLOR_RIGHT;
+      break;
+    default:
+      stream_index = COLOR;
+      break;
+  }
+
+  bool has_subscriber = false;
+  if (image_publishers_.count(stream_index) && image_publishers_[stream_index]) {
+    has_subscriber = image_publishers_[stream_index]->get_subscription_count() > 0;
+  }
+
+  if (frame->getType() == OB_FRAME_COLOR && enable_colored_point_cloud_ &&
+      depth_registration_cloud_pub_ &&
+      depth_registration_cloud_pub_->get_subscription_count() > 0) {
     has_subscriber = true;
   }
+
+  if (metadata_publishers_.count(stream_index) && metadata_publishers_[stream_index] &&
+      metadata_publishers_[stream_index]->get_subscription_count() > 0) {
+    has_subscriber = true;
+  }
+  if (camera_info_publishers_.count(stream_index) && camera_info_publishers_[stream_index] &&
+      camera_info_publishers_[stream_index]->get_subscription_count() > 0) {
+    has_subscriber = true;
+  }
+
   if (!has_subscriber) {
     return false;
   }
-  if (metadata_publishers_.count(COLOR) &&
+
-      metadata_publishers_[COLOR]->get_subscription_count() > 0) {
+  std::shared_ptr<JPEGDecoder> decoder;
-    has_subscriber = true;
+  if (stream_index == COLOR_LEFT) {
-  }
+    decoder = jpeg_decoder_left_;
-  if (camera_info_publishers_.count(COLOR) &&
+  } else if (stream_index == COLOR_RIGHT) {
-      camera_info_publishers_[COLOR]->get_subscription_count() > 0) {
+    decoder = jpeg_decoder_right_;
-    has_subscriber = true;
+  } else {
+    decoder = jpeg_decoder_;
   }
   bool is_decoded = false;
   if (!frame) {
@@ -3025,11 +3299,16 @@ bool OBCameraNode::decodeColorFrameToBuffer(const std::shared_ptr<ob::Frame> &fr
 
 #if defined(USE_RK_HW_DECODER) || defined(USE_NV_HW_DECODER)
   if (frame && frame->getFormat() != OB_FORMAT_RGB888) {
-    if (frame->getFormat() == OB_FORMAT_MJPG && jpeg_decoder_) {
+    if (frame->getFormat() == OB_FORMAT_MJPG && decoder) {
-      CHECK_NOTNULL(jpeg_decoder_.get());
+      CHECK_NOTNULL(decoder.get());
-      CHECK_NOTNULL(rgb_buffer_);
+      CHECK_NOTNULL(buffer);
       auto video_frame = frame->as<ob::ColorFrame>();
-      bool ret = jpeg_decoder_->decode(video_frame, rgb_buffer_);
+      bool ret = false;
+      if (video_frame && width_.count(stream_index) && height_.count(stream_index) &&
+          static_cast<int>(video_frame->getWidth()) == width_[stream_index] &&
+          static_cast<int>(video_frame->getHeight()) == height_[stream_index]) {
+        ret = decoder->decode(video_frame, buffer);
+      }
       if (!ret) {
         RCLCPP_ERROR_STREAM(logger_, "Decode frame failed");
         is_decoded = false;
@@ -3041,13 +3320,13 @@ bool OBCameraNode::decodeColorFrameToBuffer(const std::shared_ptr<ob::Frame> &fr
   }
 #endif
   if (!is_decoded) {
-    auto video_frame = softwareDecodeColorFrame(frame);
+    auto video_frame = softwareDecodeColorFrame(frame, stream_index);
     if (!video_frame) {
       RCLCPP_ERROR_STREAM(logger_, "Failed to convert frame to video frame");
       return false;
     }
     CHECK_NOTNULL(buffer);
-    memcpy(rgb_buffer_, video_frame->getData(), video_frame->getDataSize());
+    memcpy(buffer, video_frame->getData(), video_frame->getDataSize());
     return true;
   }
   return true;
@@ -3104,7 +3383,8 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
     return;
   }
   std::shared_ptr<ob::VideoFrame> video_frame;
-  if (frame->getType() == OB_FRAME_COLOR) {
+  if (frame->getType() == OB_FRAME_COLOR || frame->getType() == OB_FRAME_COLOR_LEFT ||
+      frame->getType() == OB_FRAME_COLOR_RIGHT) {
     video_frame = frame->as<ob::ColorFrame>();
   } else if (frame->getType() == OB_FRAME_DEPTH) {
     video_frame = frame->as<ob::DepthFrame>();
@@ -3223,10 +3503,28 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
     RCLCPP_ERROR(logger_, "color frame is not decoded");
     return;
   }
+  if (frame->getType() == OB_FRAME_COLOR_LEFT && !is_left_color_frame_decoded_) {
+    RCLCPP_ERROR(logger_, "left color frame is not decoded");
+    return;
+  }
+  if (frame->getType() == OB_FRAME_COLOR_RIGHT && !is_right_color_frame_decoded_) {
+    RCLCPP_ERROR(logger_, "right color frame is not decoded");
+    return;
+  }
   if (frame->getType() == OB_FRAME_COLOR && frame->format() != OB_FORMAT_Y8 &&
       frame->format() != OB_FORMAT_Y16 && frame->format() != OB_FORMAT_BGRA &&
       frame->format() != OB_FORMAT_RGBA && image_publishers_[COLOR]->get_subscription_count() > 0) {
     memcpy(image.data, rgb_buffer_, video_frame->getWidth() * video_frame->getHeight() * 3);
+  } else if (frame->getType() == OB_FRAME_COLOR_LEFT && frame->format() != OB_FORMAT_Y8 &&
+             frame->format() != OB_FORMAT_Y16 && frame->format() != OB_FORMAT_BGRA &&
+             frame->format() != OB_FORMAT_RGBA &&
+             image_publishers_[COLOR_LEFT]->get_subscription_count() > 0) {
+    memcpy(image.data, rgb_buffer_left_, video_frame->getWidth() * video_frame->getHeight() * 3);
+  } else if (frame->getType() == OB_FRAME_COLOR_RIGHT && frame->format() != OB_FORMAT_Y8 &&
+             frame->format() != OB_FORMAT_Y16 && frame->format() != OB_FORMAT_BGRA &&
+             frame->format() != OB_FORMAT_RGBA &&
+             image_publishers_[COLOR_RIGHT]->get_subscription_count() > 0) {
+    memcpy(image.data, rgb_buffer_right_, video_frame->getWidth() * video_frame->getHeight() * 3);
   } else {
     memcpy(image.data, video_frame->getData(), video_frame->getDataSize());
   }
@@ -3786,26 +4084,30 @@ void OBCameraNode::FillImuDataCopy(const IMUData &imu_data,
 }
 
 bool OBCameraNode::setupFormatConvertType(OBFormat format) {
+  return setupFormatConvertType(format, format_convert_filter_);
+}
+
+bool OBCameraNode::setupFormatConvertType(OBFormat format, ob::FormatConvertFilter &filter) {
   switch (format) {
     case OB_FORMAT_RGB888:
       return true;
     case OB_FORMAT_I420:
-      format_convert_filter_.setFormatConvertType(FORMAT_I420_TO_RGB888);
+      filter.setFormatConvertType(FORMAT_I420_TO_RGB888);
       break;
     case OB_FORMAT_MJPG:
-      format_convert_filter_.setFormatConvertType(FORMAT_MJPEG_TO_RGB888);
+      filter.setFormatConvertType(FORMAT_MJPEG_TO_RGB888);
       break;
     case OB_FORMAT_YUYV:
-      format_convert_filter_.setFormatConvertType(FORMAT_YUYV_TO_RGB888);
+      filter.setFormatConvertType(FORMAT_YUYV_TO_RGB888);
       break;
     case OB_FORMAT_NV21:
-      format_convert_filter_.setFormatConvertType(FORMAT_NV21_TO_RGB888);
+      filter.setFormatConvertType(FORMAT_NV21_TO_RGB888);
       break;
     case OB_FORMAT_NV12:
-      format_convert_filter_.setFormatConvertType(FORMAT_NV12_TO_RGB888);
+      filter.setFormatConvertType(FORMAT_NV12_TO_RGB888);
       break;
     case OB_FORMAT_UYVY:
-      format_convert_filter_.setFormatConvertType(FORMAT_UYVY_TO_RGB888);
+      filter.setFormatConvertType(FORMAT_UYVY_TO_RGB888);
       break;
     default:
       return false;