[interleave_ae]Added interleave_ae_mode

orbbec_camera/config/tools/multisavergbir/multi_save_rgbir_params.json

@@ -2,19 +2,22 @@
     "save_rgbir_params": {
         "image_number": "100",
         "usb_ports": [
-            "2-7",
             "2-2",
+            "2-3.3",
+            "2-3.1",
             "2-1"
         ],
         "ir_topics": [
-            "/G0_4J/left_ir/image_raw",
+            "/G330_0/left_ir/image_raw",
-            "/G1_F0/left_ir/image_raw",
+            "/G330_1/left_ir/image_raw",
-            "/G2_15/left_ir/image_raw"
+            "/G330_2/left_ir/image_raw",
+            "/G330_3/left_ir/image_raw"
         ],
         "color_topics": [
-            "/G0_4J/color/image_raw",
+            "/G330_0/color/image_raw",
-            "/G1_F0/color/image_raw",
+            "/G330_1/color/image_raw",
-            "/G2_15/color/image_raw"
+            "/G330_2/color/image_raw",
+            "/G330_3/color/image_raw"
         ]
     }
 }

orbbec_camera/launch/gemini_330_series.launch.py

@@ -174,6 +174,10 @@ def generate_launch_description():
         DeclareLaunchArgument('enable_heartbeat', default_value='false'),
         DeclareLaunchArgument('gmsl_trigger_fps', default_value='3000'),
         DeclareLaunchArgument('enable_gmsl_trigger', 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_index', default_value='1'), # 0:skip pattern ir  1: skip flood ir
     ]
 
     def get_params(context, args):

orbbec_camera/launch/multi_camera_synced.launch.py

@@ -13,97 +13,45 @@ 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_4J = IncludeLaunchDescription(
+    G330_0 = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             os.path.join(launch_file_dir, "gemini_330_series.launch.py")
         ),
         launch_arguments={
-            "camera_name": "G0_4J",
+            "camera_name": "G330_0",
-            "usb_port": "2-7",
+            "usb_port": "2-3.4",
-            "device_num": "5",
+            "device_num": "2",
-            "sync_mode": "standalone",
+            "sync_mode": "primary",
-            "enable_left_ir":"true",
             "config_file_path": config_file_path,
         }.items(),
     )
 
-    G1_F0 = IncludeLaunchDescription(
+    G330_1 = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             os.path.join(launch_file_dir, "gemini_330_series.launch.py")
         ),
         launch_arguments={
-            "camera_name": "G1_F0",
+            "camera_name": "G330_1",
-            "usb_port": "2-2",
+            "usb_port": "2-3.3",
-            "device_num": "5",
+            "device_num": "2",
-            "sync_mode": "standalone",
-            "enable_left_ir":"true",
-            "config_file_path": config_file_path,
-        }.items(),
-    )
-    G2_15 = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            os.path.join(launch_file_dir, "gemini_330_series.launch.py")
-        ),
-        launch_arguments={
-            "camera_name": "G2_15",
-            "usb_port": "2-1",
-            "device_num": "5",
-            "sync_mode": "standalone",
-            "enable_left_ir":"true",
-            "config_file_path": config_file_path,
-        }.items(),
-    )
-    G3_1M = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            os.path.join(launch_file_dir, "gemini_330_series.launch.py")
-        ),
-        launch_arguments={
-            "camera_name": "G3_1M",
-            "usb_port": "2-1.4.3",
-            "device_num": "5",
             "sync_mode": "secondary_synced",
-            "enable_left_ir":"true",
-            "config_file_path": config_file_path,
-        }.items(),
-    )
-    G4_74 = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            os.path.join(launch_file_dir, "gemini_330_series.launch.py")
-        ),
-        launch_arguments={
-            "camera_name": "G4_74",
-            "usb_port": "2-1.4.4.1",
-            "device_num": "5",
-            "sync_mode": "secondary_synced",
-            "enable_left_ir":"true",
             "config_file_path": config_file_path,
         }.items(),
     )
 
-    multi_save_rgbir_node = Node(
-        package="orbbec_camera",
-        executable="multi_save_rgbir_node",
-        name="multi_save_rgbir_node",
-    )
 
     # 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(
+            TimerAction(period=0.2, actions=[GroupAction([G330_1])]),
-                period=2.0,
+            TimerAction(period=0.2, actions=[GroupAction([G330_0])]),
-                actions=[
+
-                    TimerAction(period=0.2, actions=[GroupAction([G1_F0])]),
-                    TimerAction(period=0.2, actions=[GroupAction([G2_15])]),
-                    # TimerAction(period=0.2, actions=[GroupAction([G3_1M])]),
-                    # TimerAction(period=0.2, actions=[GroupAction([G4_74])]),
-                    TimerAction(period=0.2, actions=[GroupAction([G0_4J])]),
-                ],
-            ),
             # The primary camera should be launched at last
         ]
     )
 
     return ld
+

orbbec_camera/src/ob_camera_node.cpp

@@ -782,7 +782,42 @@ void OBCameraNode::updateImageConfig(const stream_index_pair &stream_index) {
     unit_step_size_[stream_index] = sizeof(uint16_t);
   }
 }
+int OBCameraNode::init_interleave_hdr_param() {
+  device_->setIntProperty(OB_PROP_FRAME_INTERLEAVE_CONFIG_INDEX_INT, 1);
+  device_->setIntProperty(OB_PROP_LASER_CONTROL_INT, 1);
+  device_->setIntProperty(OB_PROP_DEPTH_EXPOSURE_INT, 1);
+  device_->setIntProperty(OB_PROP_DEPTH_GAIN_INT, 16);
+  device_->setIntProperty(OB_PROP_IR_BRIGHTNESS_INT, 20);
+  device_->setIntProperty(OB_PROP_IR_AE_MAX_EXPOSURE_INT, 2000);
 
+  // set interleaveae
+  device_->setIntProperty(OB_PROP_FRAME_INTERLEAVE_CONFIG_INDEX_INT, 0);
+  device_->setIntProperty(OB_PROP_LASER_CONTROL_INT, 1);
+  device_->setIntProperty(OB_PROP_DEPTH_EXPOSURE_INT, 7500);
+  device_->setIntProperty(OB_PROP_DEPTH_GAIN_INT, 16);
+  device_->setIntProperty(OB_PROP_IR_BRIGHTNESS_INT, 60);
+  device_->setIntProperty(OB_PROP_IR_AE_MAX_EXPOSURE_INT, 10000);
+  return 0;
+}
+
+int OBCameraNode::init_interleave_laser_param() {
+  device_->setIntProperty(OB_PROP_FRAME_INTERLEAVE_CONFIG_INDEX_INT, 1);
+  device_->setIntProperty(OB_PROP_LASER_CONTROL_INT, 0);
+  device_->setIntProperty(OB_PROP_DEPTH_EXPOSURE_INT, 3000);
+  device_->setIntProperty(OB_PROP_DEPTH_GAIN_INT, 16);
+  device_->setIntProperty(OB_PROP_IR_BRIGHTNESS_INT, 60);
+  // device_->setIntProperty(OB_PROP_IR_AE_MAX_EXPOSURE_INT, 30000);
+  device_->setIntProperty(OB_PROP_IR_AE_MAX_EXPOSURE_INT, 17000);
+
+  // set interleaveae
+  device_->setIntProperty(OB_PROP_FRAME_INTERLEAVE_CONFIG_INDEX_INT, 0);
+  device_->setIntProperty(OB_PROP_LASER_CONTROL_INT, 1);
+  device_->setIntProperty(OB_PROP_DEPTH_EXPOSURE_INT, 3000);
+  device_->setIntProperty(OB_PROP_DEPTH_GAIN_INT, 16);
+  device_->setIntProperty(OB_PROP_IR_BRIGHTNESS_INT, 60);
+  device_->setIntProperty(OB_PROP_IR_AE_MAX_EXPOSURE_INT, 30000);
+  return 0;
+}
 void OBCameraNode::startStreams() {
   if (pipeline_ != nullptr) {
     pipeline_.reset();
@@ -791,6 +826,19 @@ void OBCameraNode::startStreams() {
 
   try {
     setupPipelineConfig();
+    // set interleave mode
+    if (interleave_ae_mode_ == "hdr" && interleave_frame_enable_) {
+      RCLCPP_INFO_STREAM(logger_, "Setting interleave mode to hdr");
+      device_->loadFrameInterleave("hdr interleave");
+      init_interleave_hdr_param();
+    } else if (interleave_ae_mode_ == "laser" && interleave_frame_enable_) {
+      RCLCPP_INFO_STREAM(logger_, "Setting interleave mode to laser");
+      device_->loadFrameInterleave("laser interleave");
+      init_interleave_laser_param();
+    } else {
+      RCLCPP_INFO_STREAM(logger_, "Setting interleave mode to nothing");
+    }
+
     pipeline_->start(pipeline_config_, [this](const std::shared_ptr<ob::FrameSet> &frame_set) {
       onNewFrameSetCallback(frame_set);
     });
@@ -816,6 +864,25 @@ void OBCameraNode::startStreams() {
     RCLCPP_INFO_STREAM(logger_, "Disable frame sync");
     TRY_EXECUTE_BLOCK(pipeline_->disableFrameSync());
   }
+  // enable interleave frame
+  if ((interleave_ae_mode_ == "hdr") || (interleave_ae_mode_ == "laser")) {
+    RCLCPP_INFO_STREAM(logger_, "current interleave_ae_mode_: " << interleave_ae_mode_);
+    if (device_->isPropertySupported(OB_PROP_FRAME_INTERLEAVE_ENABLE_BOOL, OB_PERMISSION_WRITE)) {
+      TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_FRAME_INTERLEAVE_ENABLE_BOOL,
+                          interleave_frame_enable_);
+      RCLCPP_INFO_STREAM(logger_, "Enable enable_interleave_depth_frame to "
+                                      << (interleave_frame_enable_ ? "true" : "false"));
+    }
+  }
+  // set interleave larse PATTERN_SYNC_DELAY
+  if ((interleave_ae_mode_ == "laser") && interleave_frame_enable_ &&
+      device_->isPropertySupported(OB_PROP_FRAME_INTERLEAVE_LASER_PATTERN_SYNC_DELAY_INT,
+                                   OB_PERMISSION_READ_WRITE) &&
+      (sync_mode_str_ == "PRIMARY" || sync_mode_str_ == "SOFTWARE_TRIGGERING")) {
+    std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+    TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_FRAME_INTERLEAVE_LASER_PATTERN_SYNC_DELAY_INT, 0);
+    RCLCPP_INFO_STREAM(logger_, "Setting OB_PROP_FRAME_INTERLEAVE_LASER_PATTERN_SYNC_DELAY_INT 0 ");
+  }
   pipeline_started_.store(true);
 }
 
@@ -892,6 +959,17 @@ void OBCameraNode::stopStreams() {
   }
   try {
     pipeline_->stop();
+    // disable interleave frame
+    if ((interleave_ae_mode_ == "hdr") || (interleave_ae_mode_ == "laser")) {
+      RCLCPP_INFO_STREAM(logger_, "current interleave_ae_mode_: " << interleave_ae_mode_);
+      if (device_->isPropertySupported(OB_PROP_FRAME_INTERLEAVE_ENABLE_BOOL, OB_PERMISSION_WRITE)) {
+        interleave_frame_enable_ = false;
+        RCLCPP_INFO_STREAM(logger_, "Enable enable_interleave_depth_frame to "
+                                        << (interleave_frame_enable_ ? "true" : "false"));
+        TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_FRAME_INTERLEAVE_ENABLE_BOOL,
+                            interleave_frame_enable_);
+      }
+    }
   } catch (const ob::Error &e) {
     RCLCPP_ERROR_STREAM(logger_, "Failed to stop pipeline: " << e.getMessage());
   } catch (...) {
@@ -1204,6 +1282,10 @@ void OBCameraNode::getParameters() {
   software_trigger_period_ = std::chrono::milliseconds(software_trigger_period);
   setAndGetNodeParameter<int>(gmsl_trigger_fps_, "gmsl_trigger_fps", 3000);
   setAndGetNodeParameter<bool>(enable_gmsl_trigger_, "enable_gmsl_trigger", false);
+  setAndGetNodeParameter<std::string>(interleave_ae_mode_, "interleave_ae_mode", "hdr");
+  setAndGetNodeParameter<bool>(interleave_frame_enable_, "interleave_frame_enable", false);
+  setAndGetNodeParameter<bool>(interleave_skip_enable_, "interleave_skip_enable", false);
+  setAndGetNodeParameter<int>(interleave_skip_index_, "interleave_skip_index", 1);
 }
 
 void OBCameraNode::setupTopics() {
@@ -1968,6 +2050,17 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
   } else if (frame->getType() == OB_FRAME_IR || frame->getType() == OB_FRAME_IR_LEFT ||
              frame->getType() == OB_FRAME_IR_RIGHT) {
     video_frame = frame->as<ob::IRFrame>();
+
+    // interleave filter speckle or flood ir
+    if (interleave_skip_enable_) {
+      RCLCPP_DEBUG(logger_, "interleave filter skip interleave_skip_index_: %d",
+                   interleave_skip_index_);
+      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());
+        return;
+      }
+    }
   } else {
     RCLCPP_ERROR(logger_, "Unsupported frame type: %d", frame->getType());
     return;

orbbec_camera/src/ros_service.cpp

@@ -811,6 +811,7 @@ void OBCameraNode::setRESETTimestampCallback(
   (void)request;
   try {
     device_->setBoolProperty(OB_PROP_TIMER_RESET_TRIGGER_OUT_ENABLE_BOOL, true);
+    device_->setBoolProperty(OB_PROP_TIMER_RESET_SIGNAL_BOOL, true);
     response->success = true;
   } catch (const ob::Error& e) {
     response->message = e.getMessage();