Merge remote-tracking branch 'origin/v2-main_feature/test_interleave_mode' into test_interleave_mode_opdk

orbbec_camera/SDK/include/libobsensor/h/Filter.h

@@ -245,6 +245,16 @@ OB_EXPORT uint32_t ob_filter_config_schema_list_get_count(const ob_filter_config
 OB_EXPORT ob_filter_config_schema_item ob_filter_config_schema_list_get_item(const ob_filter_config_schema_list *config_schema_list, uint32_t index,
                                                                              ob_error **error);
 
+/**
+ * @brief Set the align to stream profile for the align filter.
+ * @breif It is useful when the align target stream dose not started (without any frame to get intrinsics and extrinsics).
+ *
+ * @param filter A filter object.
+ * @param align_to_stream_profile The align target stream profile.
+ * @param error Pointer to an error object that will be set if an error occurs.
+ */
+OB_EXPORT void ob_align_filter_set_align_to_stream_profile(ob_filter *filter, const ob_stream_profile *align_to_stream_profile, ob_error **error);
+
 // The following interfaces are deprecated and are retained here for compatibility purposes.
 #define ob_get_filter ob_filter_list_get_filter
 #define ob_get_filter_name ob_filter_get_name

orbbec_camera/SDK/include/libobsensor/h/ObTypes.h

@@ -1,9 +1,6 @@
 // Copyright (c) Orbbec Inc. All Rights Reserved.
 // Licensed under the MIT License.
 
-// License: Apache 2.0. See LICENSE file in root directory.
-// Copyright(c) 2020 Orbbec  Corporation. All Rights Reserved.
-
 /**
  * @file ObTypes.h
  * @brief Provide structs commonly used in the SDK, enumerating constant definitions.

orbbec_camera/SDK/include/libobsensor/h/Property.h

@@ -97,7 +97,7 @@ typedef enum {
     /**
      * @brief Software filter switch
      */
-    OB_PROP_DEPTH_SOFT_FILTER_BOOL = 24,
+    OB_PROP_DEPTH_NOISE_REMOVAL_FILTER_BOOL = 24,
 
     /**
      * @brief LDP status
@@ -105,14 +105,14 @@ typedef enum {
     OB_PROP_LDP_STATUS_BOOL = 32,
 
     /**
-     * @brief soft filter maxdiff param
+     * @brief maxdiff for depth noise removal filter
      */
-    OB_PROP_DEPTH_MAX_DIFF_INT = 40,
+    OB_PROP_DEPTH_NOISE_REMOVAL_FILTER_MAX_DIFF_INT = 40,
 
     /**
-     * @brief soft filter maxSpeckleSize
+     * @brief maxSpeckleSize for depth noise removal filter
      */
-    OB_PROP_DEPTH_MAX_SPECKLE_SIZE_INT = 41,
+    OB_PROP_DEPTH_NOISE_REMOVAL_FILTER_MAX_SPECKLE_SIZE_INT = 41,
 
     /**
      * @brief Hardware d2c is on
@@ -759,7 +759,9 @@ typedef enum {
 #define OB_PROP_LASER_ENERGY_LEVEL_INT OB_PROP_LASER_POWER_LEVEL_CONTROL_INT
 #define OB_PROP_LASER_HW_ENERGY_LEVEL_INT OB_PROP_LASER_POWER_ACTUAL_LEVEL_INT
 #define OB_PROP_DEVICE_USB3_REPEAT_IDENTIFY_BOOL OB_PROP_DEVICE_USB2_REPEAT_IDENTIFY_BOOL
-#define OB_PROP_DEPTH_NOISE_REMOVAL_FILTER_BOOL OB_PROP_DEPTH_SOFT_FILTER_BOOL
+#define OB_PROP_DEPTH_SOFT_FILTER_BOOL OB_PROP_DEPTH_NOISE_REMOVAL_FILTER_BOOL
+#define OB_PROP_DEPTH_MAX_DIFF_INT OB_PROP_DEPTH_NOISE_REMOVAL_FILTER_MAX_DIFF_INT
+#define OB_PROP_DEPTH_MAX_SPECKLE_SIZE_INT OB_PROP_DEPTH_NOISE_REMOVAL_FILTER_MAX_SPECKLE_SIZE_INT
 
 /**
  * @brief The data type used to describe all property settings

orbbec_camera/SDK/include/libobsensor/hpp/Filter.hpp

@@ -440,6 +440,18 @@ public:
     void setMatchTargetResolution(bool state) {
         setConfigValue("MatchTargetRes", state);
     }
+
+    /**
+     * @brief Set the Align To Stream Profile
+     * @brief  It is useful when the align target stream dose not started (without any frame to get intrinsics and extrinsics).
+     *
+     * @param profile The Align To Stream Profile.
+     */
+    void setAlignToStreamProfile(std::shared_ptr<const StreamProfile> profile) {
+        ob_error *error = nullptr;
+        ob_align_filter_set_align_to_stream_profile(impl_, profile->getImpl(), &error);
+        Error::handle(&error);
+    }
 };
 
 /**

orbbec_camera/include/orbbec_camera/constants.h

@@ -23,7 +23,7 @@
 
 #define OB_ROS_MAJOR_VERSION 2
 #define OB_ROS_MINOR_VERSION 0
-#define OB_ROS_PATCH_VERSION 8
+#define OB_ROS_PATCH_VERSION 9
 
 #ifndef STRINGIFY
 #define STRINGIFY(arg) #arg

orbbec_camera/include/orbbec_camera/ob_camera_node.h

@@ -312,6 +312,8 @@ class OBCameraNode {
                                  std::shared_ptr<std_srvs::srv::SetBool::Response>& response);
   void setSYNCInterleaveLaserCallback(const std::shared_ptr<SetInt32 ::Request>& request,
                                       std::shared_ptr<SetInt32 ::Response>& response);
+  void setSYNCHostimeCallback(const std::shared_ptr<std_srvs::srv::SetBool::Request>& request,
+                                 std::shared_ptr<std_srvs::srv::SetBool::Response>& response);
   bool toggleSensor(const stream_index_pair& stream_index, bool enabled, std::string& msg);
 
   void saveImageCallback(const std::shared_ptr<std_srvs::srv::Empty::Request>& request,
@@ -465,6 +467,7 @@ class OBCameraNode {
   rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr set_sync_immediately_srv_;
   rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr set_reset_timestamp_srv_;
   rclcpp::Service<SetInt32>::SharedPtr set_interleaver_laser_sync_srv_;
+  rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr set_sync_host_time_srv_;
 
   bool enable_sync_output_accel_gyro_ = false;
   bool publish_tf_ = false;

orbbec_camera/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>orbbec_camera</name>
-  <version>2.0.8</version>
+  <version>2.0.9</version>
   <description>Orbbec Camera package</description>
   <maintainer email="[email protected]">Joe Dong</maintainer>
   <license>Apache-2.0</license>

orbbec_camera/src/ob_camera_node.cpp

@@ -545,14 +545,14 @@ void OBCameraNode::setupDepthPostProcessFilter() {
     } else if (filter_name == "NoiseRemovalFilter" && enable_noise_removal_filter_) {
       auto noise_removal_filter = filter->as<ob::NoiseRemovalFilter>();
       OBNoiseRemovalFilterParams params = noise_removal_filter->getFilterParams();
-      RCLCPP_INFO_STREAM(logger_, "Default noise removal filter params: "
+      RCLCPP_INFO_STREAM(
-                                      << "disp_diff: " << params.disp_diff
+          logger_, "Default noise removal filter params: " << "disp_diff: " << params.disp_diff
-                                      << ", max_size: " << params.max_size);
+                                                           << ", max_size: " << params.max_size);
       params.disp_diff = noise_removal_filter_min_diff_;
       params.max_size = noise_removal_filter_max_size_;
-      RCLCPP_INFO_STREAM(logger_, "Set noise removal filter params: "
+      RCLCPP_INFO_STREAM(logger_,
-                                      << "disp_diff: " << params.disp_diff
+                         "Set noise removal filter params: " << "disp_diff: " << params.disp_diff
-                                      << ", max_size: " << params.max_size);
+                                                             << ", max_size: " << params.max_size);
       if (noise_removal_filter_min_diff_ != -1 && noise_removal_filter_max_size_ != -1) {
         noise_removal_filter->setFilterParams(params);
       }
@@ -561,11 +561,11 @@ void OBCameraNode::setupDepthPostProcessFilter() {
           hdr_merge_gain_2_ != -1) {
         auto hdr_merge_filter = filter->as<ob::HdrMerge>();
         hdr_merge_filter->enable(true);
-        RCLCPP_INFO_STREAM(logger_, "Set HDR merge filter params: "
+        RCLCPP_INFO_STREAM(
-                                        << "exposure_1: " << hdr_merge_exposure_1_
+            logger_, "Set HDR merge filter params: " << "exposure_1: " << hdr_merge_exposure_1_
-                                        << ", gain_1: " << hdr_merge_gain_1_
+                                                     << ", gain_1: " << hdr_merge_gain_1_
-                                        << ", exposure_2: " << hdr_merge_exposure_2_
+                                                     << ", exposure_2: " << hdr_merge_exposure_2_
-                                        << ", gain_2: " << hdr_merge_gain_2_);
+                                                     << ", gain_2: " << hdr_merge_gain_2_);
         auto config = OBHdrConfig();
         config.enable = true;
         config.exposure_1 = hdr_merge_exposure_1_;
@@ -860,24 +860,6 @@ void OBCameraNode::startStreams() {
     colorFrameThread_ = std::make_shared<std::thread>([this]() { onNewColorFrameCallback(); });
   }
 
-  // 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)) {
-      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_);
-    }
-  }
-  // set interleave larse PATTERN_SYNC_DELAY
-  if ((interleave_ae_mode_ == "laser") &&
-      device_->isPropertySupported(OB_PROP_FRAME_INTERLEAVE_LASER_PATTERN_SYNC_DELAY_INT,
-                                   OB_PERMISSION_READ_WRITE)) {
-    RCLCPP_INFO_STREAM(logger_, "Setting OB_PROP_FRAME_INTERLEAVE_LASER_PATTERN_SYNC_DELAY_INT 0 ");
-    TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_FRAME_INTERLEAVE_LASER_PATTERN_SYNC_DELAY_INT, 0);
-  }
-
   if (enable_frame_sync_) {
     RCLCPP_INFO_STREAM(logger_, "Enable frame sync");
     TRY_EXECUTE_BLOCK(pipeline_->enableFrameSync());
@@ -885,6 +867,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);
 }
 
@@ -2241,7 +2242,6 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
   } else {
     memcpy(image.data, video_frame->getData(), video_frame->getDataSize());
   }
-
   if (stream_index == DEPTH) {
     auto depth_scale = video_frame->as<ob::DepthFrame>()->getValueScale();
     image = image * depth_scale;

orbbec_camera/src/ros_service.cpp

@@ -184,6 +184,11 @@ void OBCameraNode::setupCameraCtrlServices() {
                                           std::shared_ptr<SetInt32::Response> response) {
         setSYNCInterleaveLaserCallback(request, response);
       });
+    set_sync_host_time_srv_ = node_->create_service<SetBool>(
+      "set_sync_hosttime", [this](const std::shared_ptr<SetBool::Request> request,
+                                    std::shared_ptr<SetBool::Response> response) {
+        setSYNCHostimeCallback(request, response);
+      });
 }
 
 void OBCameraNode::setExposureCallback(const std::shared_ptr<SetInt32::Request>& request,
@@ -825,7 +830,25 @@ void OBCameraNode::setSYNCInterleaveLaserCallback(
     std::shared_ptr<SetInt32 ::Response>& response) {
   (void)request;
   try {
-    device_->setIntProperty(OB_PROP_TIMER_RESET_TRIGGER_OUT_ENABLE_BOOL, request->data);
+    device_->setIntProperty(OB_PROP_FRAME_INTERLEAVE_LASER_PATTERN_SYNC_DELAY_INT, request->data);
+    response->success = true;
+  } catch (const ob::Error& e) {
+    response->message = e.getMessage();
+    response->success = false;
+  } catch (const std::exception& e) {
+    response->message = e.what();
+    response->success = false;
+  } catch (...) {
+    response->message = "unknown error";
+    response->success = false;
+  }
+}
+void OBCameraNode::setSYNCHostimeCallback(
+    const std::shared_ptr<std_srvs::srv::SetBool::Request>& request,
+    std::shared_ptr<std_srvs::srv::SetBool::Response>& response) {
+  (void)request;
+  try {
+    device_->timerSyncWithHost();
     response->success = true;
   } catch (const ob::Error& e) {
     response->message = e.getMessage();