add frame drop judgement

orbbec_camera/config/OrbbecSDKConfig_v1.0.xml

@@ -37,7 +37,7 @@
     </Memory>
 
     <Misc>
-        <GlobalTimestampFitterEnable>false</GlobalTimestampFitterEnable>
+        <GlobalTimestampFitterEnable>true</GlobalTimestampFitterEnable>
         <!-- Global timestamp fitter refresh interval, unit: milliseconds, default value: 1000,
         minimum value: 100, it is recommended not to be greater than 1000 -->
         <GlobalTimestampFitterInterval>1000</GlobalTimestampFitterInterval>

orbbec_camera/config/camera_params.yaml

@@ -1,11 +1,11 @@
 # common params
-depth_registration: true
+depth_registration: false
 enable_point_cloud: false
 enable_colored_point_cloud: false
 device_preset: "High Accuracy"
 laser_on_off_mode: 0 # 0: off, 1: on-off, 1: off-on
 time_domain: "global" # global, device, system
-enable_sync_host_time: false
+enable_sync_host_time: true
 frames_per_trigger: 1
 
 # When 3D reconstruction mode is enabled:
@@ -18,33 +18,33 @@ enable_3d_reconstruction_mode: false
 enable_color: true
 color_width: 640
 color_height: 480
-color_fps: 30
+color_fps: 60
 color_format: "YUYV"
-enable_color_auto_exposure: true
+enable_color_auto_exposure: false
 color_exposure: 50 # 5ms
 color_gain: -1 # -1 default
 
 # depth params
 depth_width: 640
 depth_height: 480
-depth_fps: 30
+depth_fps: 60
 depth_format: "Y16"
 
 # ir exposure
-enable_ir_auto_exposure: false
+enable_ir_auto_exposure: true
-ir_exposure: 5000 # 5ms
+# ir_exposure: 5000 # 5ms
-ir_gain: 40
+# ir_gain: 40
 
 #left ir params
 enable_left_ir: true
 left_ir_width: 640
 left_ir_height: 480
-left_ir_fps: 30
+left_ir_fps: 60
 left_ir_format: "Y8"
 
 #right ir params
 enable_right_ir: true
 right_ir_width: 640
 right_ir_height: 480
-right_ir_fps: 30
+right_ir_fps: 60
 right_ir_format: "Y8"

orbbec_camera/include/orbbec_camera/ob_camera_node.h

@@ -138,6 +138,13 @@ typedef struct {
   uint16_t fps;
 } cs_param_t;
 
+struct VideoStreamInfo {
+  OBFrameType frame_type;
+  std::chrono::steady_clock::time_point last_frame_time;
+  int frame_count_ = 0;
+  double frame_rate_ = 0.0;
+};
+
 class OBCameraNode {
  public:
   OBCameraNode(rclcpp::Node* node, std::shared_ptr<ob::Device> device,
@@ -327,6 +334,8 @@ class OBCameraNode {
 
   std::shared_ptr<ob::Frame> decodeIRMJPGFrame(const std::shared_ptr<ob::Frame>& frame);
 
+  void updateStreamInfo(VideoStreamInfo& stream_info);
+
   void onNewFrameCallback(const std::shared_ptr<ob::Frame>& frame,
                           const stream_index_pair& stream_index);
 
@@ -615,5 +624,11 @@ class OBCameraNode {
   bool interleave_frame_enable_ = false;
   bool interleave_skip_enable_ = false;
   int interleave_skip_index_ = 1;
+  int interleave_skip_depth_index_ = 1;
+
+  VideoStreamInfo color_stream_info_ = {OB_FRAME_COLOR, std::chrono::steady_clock::now()};
+  VideoStreamInfo depth_stream_info_ = {OB_FRAME_DEPTH, std::chrono::steady_clock::now()};
+  VideoStreamInfo left_ir_stream_info_ = {OB_FRAME_IR_LEFT, std::chrono::steady_clock::now()};
+  VideoStreamInfo right_ir_stream_info_ = {OB_FRAME_IR_RIGHT, std::chrono::steady_clock::now()};
 };
 }  // namespace orbbec_camera

orbbec_camera/launch/gemini_330_series_interleave_laser_g335L.launch.py

@@ -183,8 +183,8 @@ def generate_launch_description():
         DeclareLaunchArgument('enable_hardware_reset', default_value='false'),
         DeclareLaunchArgument('interleave_ae_mode', default_value='laser'), # 'hdr' or 'laser'
         DeclareLaunchArgument('interleave_frame_enable', default_value='true'),
-        DeclareLaunchArgument('interleave_skip_enable', default_value='false'),
+        DeclareLaunchArgument('interleave_skip_enable', default_value='true'),
-        DeclareLaunchArgument('interleave_skip_index', default_value='1'), # 0:skip pattern ir  1: skip flood ir
+        DeclareLaunchArgument('interleave_skip_index', default_value='0'), # 0:skip pattern ir  1: skip flood ir
     ]
 
     def get_params(context, args):

orbbec_camera/launch/multi_camera_synced.launch.py

@@ -5,7 +5,6 @@ from launch_ros.actions import Node
 from launch.actions import IncludeLaunchDescription, GroupAction, TimerAction
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 
-
 def generate_launch_description():
     # Include launch files
     package_dir = get_package_share_directory("orbbec_camera")
@@ -13,83 +12,64 @@ def generate_launch_description():
     config_file_dir = os.path.join(package_dir, "config")
     config_file_path = os.path.join(config_file_dir, "camera_params.yaml")
 
-    G0_51 = IncludeLaunchDescription(
+    front_camera = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             os.path.join(launch_file_dir, "gemini_330_series_interleave_laser_g335L.launch.py")
         ),
         launch_arguments={
-            "camera_name": "G0_51",
+            "camera_name": "front_camera",
-            "usb_port": "2-2",
-            "device_num": "4",
-            "sync_mode": "primary",
-            "enable_left_ir":"true",
-            "config_file_path": config_file_path,
-        }.items(),
-    )
-
-    G1_54 = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            os.path.join(launch_file_dir, "gemini_330_series_interleave_laser_g335L.launch.py")
-        ),
-        launch_arguments={
-            "camera_name": "G1_54",
-            "usb_port": "2-3.3",
-            "device_num": "4",
-            "sync_mode": "secondary_synced",
-            "enable_left_ir":"true",
-            "config_file_path": config_file_path,
-        }.items(),
-    )
-    G2_5Y = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            os.path.join(launch_file_dir, "gemini_330_series_interleave_laser_g335L.launch.py")
-        ),
-        launch_arguments={
-            "camera_name": "G2_5Y",
-            "usb_port": "2-3.1",
-            "device_num": "4",
-            "sync_mode": "secondary_synced",
-            "enable_left_ir":"true",
-            "config_file_path": config_file_path,
-        }.items(),
-    )
-    G3_47 = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            os.path.join(launch_file_dir, "gemini_330_series_interleave_laser_g335L.launch.py")
-        ),
-        launch_arguments={
-            "camera_name": "G3_47",
             "usb_port": "2-1",
             "device_num": "4",
-            "sync_mode": "secondary_synced",
+            "sync_mode": "primary",
-            "enable_left_ir":"true",
             "config_file_path": config_file_path,
         }.items(),
     )
 
-    multi_save_rgbir_node = Node(
+    left_camera = IncludeLaunchDescription(
-        package="orbbec_camera",
+        PythonLaunchDescriptionSource(
-        executable="multi_save_rgbir_node",
+            os.path.join(launch_file_dir, "gemini_330_series_interleave_laser_g335L.launch.py")
-        name="multi_save_rgbir_node",
+        ),
+        launch_arguments={
+            "camera_name": "left_camera",
+            "usb_port": "2-3.2",
+            "device_num": "4",
+            "sync_mode": "secondary_synced",
+            "config_file_path": config_file_path,
+        }.items(),
+    )
+    right_camera = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(launch_file_dir, "gemini_330_series_interleave_laser_g335L.launch.py")
+        ),
+        launch_arguments={
+            "camera_name": "right_camera",
+            "usb_port": "2-3.4",
+            "device_num": "4",
+            "sync_mode": "secondary_synced",
+            "config_file_path": config_file_path,
+        }.items(),
+    )
+    rear_camera = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(launch_file_dir, "gemini_330_series_interleave_laser_g335L.launch.py")
+        ),
+        launch_arguments={
+            "camera_name": "rear_camera",
+            "usb_port": "2-2",
+            "device_num": "4",
+            "sync_mode": "secondary_synced",
+            "config_file_path": config_file_path,
+        }.items(),
     )
-
-    # If you need more cameras, just add more launch_include here, and change the usb_port and device_num
 
     # Launch description
     ld = LaunchDescription(
-        [
+      [
-            GroupAction([multi_save_rgbir_node]),
+          TimerAction(period=0.5, actions=[GroupAction([left_camera])]),
-            TimerAction(
+          TimerAction(period=0.5, actions=[GroupAction([right_camera])]),
-                period=2.0,
+          TimerAction(period=0.5, actions=[GroupAction([rear_camera])]),
-                actions=[
+          TimerAction(period=0.5, actions=[GroupAction([front_camera])]),
-                    TimerAction(period=0.5, actions=[GroupAction([G1_54])]),
+      ]
-                    TimerAction(period=0.5, actions=[GroupAction([G2_5Y])]),
-                    TimerAction(period=0.5, actions=[GroupAction([G3_47])]),
-                    TimerAction(period=0.5, actions=[GroupAction([G0_51])]),
-                ],
-            ),
-            # The primary camera should be launched at last
-        ]
     )
 
     return ld

orbbec_camera/src/ob_camera_node.cpp

@@ -2028,6 +2028,61 @@ std::shared_ptr<ob::Frame> OBCameraNode::decodeIRMJPGFrame(
   return frame;
 }
 
+void OBCameraNode::updateStreamInfo(VideoStreamInfo& stream_info) {
+    auto now = std::chrono::steady_clock::now();
+    auto duration = std::chrono::duration<double, std::micro>(now - stream_info.last_frame_time).count();
+    double dst_duration = duration;
+    int dst_fps = 0;
+    stream_index_pair dst_frame_type;
+
+    switch (stream_info.frame_type) {
+      case OB_FRAME_COLOR:
+        dst_frame_type = stream_index_pair{OB_STREAM_COLOR, 0};
+        break;
+      case OB_FRAME_DEPTH:
+        dst_frame_type = stream_index_pair{OB_STREAM_DEPTH, 0};
+        break;
+      case OB_FRAME_IR_LEFT:
+        dst_frame_type = stream_index_pair{OB_STREAM_IR_LEFT, 0};
+        break;
+      case OB_FRAME_IR_RIGHT:
+        dst_frame_type = stream_index_pair{OB_STREAM_IR_RIGHT, 0};
+        break;
+      default:
+        RCLCPP_INFO_STREAM(logger_, "[WARNING] Unknown frame_type "
+          << stream_info.frame_type << "\n");
+        return;
+    }
+
+    if (interleave_skip_enable_) {
+      dst_duration = (1000000.0 / fps_[dst_frame_type]) * 2;
+      dst_fps = fps_[dst_frame_type] / 2;
+    } else {
+      dst_duration = 1000000.0 / fps_[dst_frame_type];
+      dst_fps = fps_[dst_frame_type];
+    }
+
+    if (duration > dst_duration) {
+        RCLCPP_INFO_STREAM(logger_, "[WARNING] Frame interval for frame_type "
+                  << stream_info.frame_type << " exceeded " << dst_duration / 1000.0 << " ms. Interval: "
+                  << duration / 1000.0 << " ms\n");
+    }
+
+    stream_info.frame_count_++;
+
+    if (stream_info.frame_count_ % dst_fps == 0) {
+        double elapsed_seconds = std::chrono::duration<double>(now - stream_info.last_frame_time).count();
+        if (elapsed_seconds > 0) {
+            stream_info.frame_rate_ = dst_fps / elapsed_seconds;
+            RCLCPP_INFO_STREAM(logger_, "[INFO] Frame rate for frame_type " << stream_info.frame_type
+                      << ": " << stream_info.frame_rate_ << " FPS\n");
+        }
+    }
+
+    stream_info.last_frame_time = now;
+}
+
+
 void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
                                       const stream_index_pair &stream_index) {
   if (frame == nullptr) {
@@ -2045,8 +2100,20 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
   }
   std::shared_ptr<ob::VideoFrame> video_frame;
   if (frame->getType() == OB_FRAME_COLOR) {
+    updateStreamInfo(color_stream_info_);
     video_frame = frame->as<ob::ColorFrame>();
   } else if (frame->getType() == OB_FRAME_DEPTH) {
+    // interleave filter depth
+    if (interleave_skip_enable_) {
+      interleave_skip_depth_index_++;
+      RCLCPP_DEBUG(logger_, "interleave filter skip interleave_skip_index_: %d",
+                   interleave_skip_depth_index_);
+      if (interleave_skip_depth_index_ % 2 == 0 ) {
+        RCLCPP_DEBUG(logger_, "interleave filter skip frame type: %d", frame->getType());
+        return;
+      }
+      updateStreamInfo(depth_stream_info_);
+    }
     video_frame = frame->as<ob::DepthFrame>();
   } else if (frame->getType() == OB_FRAME_IR || frame->getType() == OB_FRAME_IR_LEFT ||
              frame->getType() == OB_FRAME_IR_RIGHT) {
@@ -2061,6 +2128,12 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
         RCLCPP_DEBUG(logger_, "interleave filter skip frame type: %d", frame->getType());
         return;
       }
+      if (frame->getType() == OB_FRAME_IR_LEFT){
+        updateStreamInfo(left_ir_stream_info_);
+      }
+       if (frame->getType() == OB_FRAME_IR_RIGHT){
+        updateStreamInfo(right_ir_stream_info_);
+      }
     }
 
   } else {