add gmsl support

orbbec_camera/config/camera_params.yaml

@@ -2,9 +2,9 @@
 depth_registration: false
 enable_point_cloud: false
 enable_colored_point_cloud: false
-device_preset: "High Accuracy"
+device_preset: "Default"
 laser_on_off_mode: 0 # 0: off, 1: on-off, 1: off-on
-time_domain: "global" # global, device, system
+time_domain: "device" # global, device, system
 enable_sync_host_time: true
 frames_per_trigger: 1
 log_level: "warning"

orbbec_camera/include/orbbec_camera/ob_camera_node.h

@@ -637,6 +637,7 @@ class OBCameraNode {
   bool interleave_frame_enable_ = false;
   bool interleave_skip_enable_ = false;
   int interleave_skip_index_ = 1;
+  int interleave_skip_color_index_ = 1;
   int interleave_skip_depth_index_ = 1;
 
   double delta_duration_ = 5000.0;

orbbec_camera/launch/gemini_330_series_interleave_laser_g335L.launch.py

@@ -65,7 +65,7 @@ def generate_launch_description():
         DeclareLaunchArgument('color_width', default_value='0'),
         DeclareLaunchArgument('color_height', default_value='0'),
         DeclareLaunchArgument('color_fps', default_value='30'),
-        DeclareLaunchArgument('color_format', default_value='MJPG'),
+        DeclareLaunchArgument('color_format', default_value='YUYV'),
         DeclareLaunchArgument('enable_color', default_value='true'),
         DeclareLaunchArgument('color_qos', default_value='default'),
         DeclareLaunchArgument('color_camera_info_qos', default_value='default'),
@@ -191,6 +191,8 @@ def generate_launch_description():
         DeclareLaunchArgument('component_container_name', default_value='shared_orbbec_container'),
         DeclareLaunchArgument('delta_duration', default_value='1000.0'),
         DeclareLaunchArgument('delta_fps', default_value='1'),
+        DeclareLaunchArgument('gmsl_trigger_fps', default_value='3000'),
+        DeclareLaunchArgument('enable_gmsl_trigger', default_value='false'),
     ]
 
     def get_params(context, args):

orbbec_camera/launch/multi_camera_synced.launch.py

@@ -45,13 +45,17 @@ def generate_launch_description():
         ),
         launch_arguments={
             "camera_name": "front_camera",
-            "usb_port": "2-1",
+            # "usb_port": "2-1",
+            "usb_port": "gmsl2-4",
             "device_num": "4",
-            "sync_mode": "primary",
+            # "sync_mode": "primary",
+            "sync_mode": "secondary_synced",
             "config_file_path": config_file_path,
             'use_intra_process_comms': LaunchConfiguration("use_intra_process_comms"),
             'attach_to_shared_component_container': attach_to_shared_component_container_arg,
             'component_container_name': component_container_name_arg,
+            'gmsl_trigger_fps': "5990",
+            'enable_gmsl_trigger': "true",
         }.items(),
     )
     left_camera = IncludeLaunchDescription(
@@ -60,7 +64,8 @@ def generate_launch_description():
         ),
         launch_arguments={
             "camera_name": "left_camera",
-            "usb_port": "2-3.1",
+            # "usb_port": "2-3.1",
+            "usb_port": "gmsl2-3",
             "device_num": "4",
             "sync_mode": "secondary_synced",
             "config_file_path": config_file_path,
@@ -75,7 +80,8 @@ def generate_launch_description():
         ),
         launch_arguments={
             "camera_name": "right_camera",
-            "usb_port": "2-3.3",
+            # "usb_port": "2-3.3",
+            "usb_port": "gmsl2-1",
             "device_num": "4",
             "sync_mode": "secondary_synced",
             "config_file_path": config_file_path,
@@ -90,7 +96,8 @@ def generate_launch_description():
         ),
         launch_arguments={
             "camera_name": "rear_camera",
-            "usb_port": "2-2",
+            # "usb_port": "2-2",
+            "usb_port": "gmsl2-7",
             "device_num": "4",
             "sync_mode": "secondary_synced",
             "config_file_path": config_file_path,

orbbec_camera/src/ob_camera_node.cpp

@@ -2039,9 +2039,8 @@ void OBCameraNode::updateStreamInfo(const std::shared_ptr<ob::Frame> &frame,
   double dst_duration = 0.0;
   int dst_fps = 0;
   stream_index_pair dst_frame_type;
-  double delta_duration = delta_duration_;
+
-  int delta_fps = delta_fps_;
+  if (current_timestamp < stream_info.last_frame_time) {
-  if (current_timestamp <= stream_info.last_frame_time) {
     RCLCPP_INFO_STREAM(logger_, "[WARNING] current_timestamp "
                                     << current_timestamp << " stream_info.last_frame_time "
                                     << stream_info.last_frame_time << "\n");
@@ -2123,10 +2122,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(frame, color_stream_info_);
+    // updateStreamInfo(frame, color_stream_info_);
+    if (interleave_skip_enable_) {
+      interleave_skip_color_index_++;
+      RCLCPP_DEBUG(logger_, "interleave filter skip interleave_skip_color_index_: %d",
+                   interleave_skip_color_index_);
+      if (interleave_skip_color_index_ % 2 == 0) {
+        RCLCPP_DEBUG(logger_, "interleave filter skip frame type: %d", frame->getType());
+        return;
+      }
+      updateStreamInfo(frame, color_stream_info_);
+    }
     video_frame = frame->as<ob::ColorFrame>();
   } else if (frame->getType() == OB_FRAME_DEPTH) {
-    // interleave filter depth
+    // updateStreamInfo(frame, depth_stream_info_);
     if (interleave_skip_enable_) {
       interleave_skip_depth_index_++;
       RCLCPP_DEBUG(logger_, "interleave filter skip interleave_skip_index_: %d",