Merge branch 'feat/get_preset_service' into merge/sdk_2.9.0

orbbec_camera/include/orbbec_camera/ob_camera_node.h

@@ -51,6 +51,7 @@
 #include "orbbec_camera_msgs/msg/depth_filter_param.hpp"
 #include "orbbec_camera_msgs/msg/depth_filter_state.hpp"
 #include "orbbec_camera_msgs/msg/depth_filters_status.hpp"
+#include "orbbec_camera_msgs/srv/get_device_config.hpp"
 #include "orbbec_camera_msgs/srv/get_device_info.hpp"
 #include "orbbec_camera_msgs/msg/extrinsics.hpp"
 #include "orbbec_camera_msgs/msg/metadata.hpp"
@@ -110,6 +111,7 @@
 #define DEVICE_PATH "/dev/camsync"
 
 namespace orbbec_camera {
+using GetDeviceConfig = orbbec_camera_msgs::srv::GetDeviceConfig;
 using GetDeviceInfo = orbbec_camera_msgs::srv::GetDeviceInfo;
 using Extrinsics = orbbec_camera_msgs::msg::Extrinsics;
 using SetInt32 = orbbec_camera_msgs::srv::SetInt32;
@@ -378,6 +380,9 @@ class OBCameraNode {
   void getDeviceInfoCallback(const std::shared_ptr<GetDeviceInfo::Request>& request,
                              std::shared_ptr<GetDeviceInfo::Response>& response);
 
+  void getDeviceConfigCallback(const std::shared_ptr<GetDeviceConfig::Request>& request,
+                               std::shared_ptr<GetDeviceConfig::Response>& response);
+
   void getSDKVersion(const std::shared_ptr<GetString::Request>& request,
                      std::shared_ptr<GetString::Response>& response);
 
@@ -658,6 +663,7 @@ class OBCameraNode {
       set_auto_exposure_srv_;
   std::map<stream_index_pair, rclcpp::Service<SetArrays>::SharedPtr> set_ae_roi_srv_;
   rclcpp::Service<GetDeviceInfo>::SharedPtr get_device_srv_;
+  rclcpp::Service<GetDeviceConfig>::SharedPtr get_device_config_srv_;
   rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr set_laser_enable_srv_;
   rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr set_ldp_enable_srv_;
   rclcpp::Service<orbbec_camera_msgs::srv::GetBool>::SharedPtr get_ldp_status_srv_;

orbbec_camera/src/ros_service.cpp

@@ -51,6 +51,59 @@ std::string getDisparityResolutionHintByPid(uint32_t pid) {
          "1280x800/1280x720/640x400/424x266";
 }
 
+std::string alignTargetStreamToString(OBStreamType stream_type) {
+  switch (stream_type) {
+    case OB_STREAM_COLOR:
+      return "COLOR";
+    case OB_STREAM_DEPTH:
+      return "DEPTH";
+    default:
+      return "UNKNOWN";
+  }
+}
+
+std::string colorPresetToString(int value) {
+  switch (value) {
+    case 0:
+      return "Default";
+    case 1:
+      return "Warm Biased AWB";
+    default:
+      return "";
+  }
+}
+
+std::string disparityToDepthModeToString(bool hardware_enabled, bool software_enabled) {
+  if (hardware_enabled) {
+    return "HW";
+  }
+  if (software_enabled) {
+    return "SW";
+  }
+  return "disable";
+}
+
+std::string OBSyncModeToString(const OBMultiDeviceSyncMode& mode) {
+  switch (mode) {
+    case OBMultiDeviceSyncMode::OB_MULTI_DEVICE_SYNC_MODE_FREE_RUN:
+      return "FREE_RUN";
+    case OBMultiDeviceSyncMode::OB_MULTI_DEVICE_SYNC_MODE_STANDALONE:
+      return "STANDALONE";
+    case OBMultiDeviceSyncMode::OB_MULTI_DEVICE_SYNC_MODE_PRIMARY:
+      return "PRIMARY";
+    case OBMultiDeviceSyncMode::OB_MULTI_DEVICE_SYNC_MODE_SECONDARY:
+      return "SECONDARY";
+    case OBMultiDeviceSyncMode::OB_MULTI_DEVICE_SYNC_MODE_SECONDARY_SYNCED:
+      return "SECONDARY_SYNCED";
+    case OBMultiDeviceSyncMode::OB_MULTI_DEVICE_SYNC_MODE_SOFTWARE_TRIGGERING:
+      return "SOFTWARE_TRIGGERING";
+    case OBMultiDeviceSyncMode::OB_MULTI_DEVICE_SYNC_MODE_HARDWARE_TRIGGERING:
+      return "HARDWARE_TRIGGERING";
+    default:
+      return "FREE_RUN";
+  }
+}
+
 }  // namespace
 
 void OBCameraNode::setupCameraCtrlServices() {
@@ -213,6 +266,11 @@ void OBCameraNode::setupCameraCtrlServices() {
                                 std::shared_ptr<GetDeviceInfo::Response> response) {
         getDeviceInfoCallback(request, response);
       });
+  get_device_config_srv_ = node_->create_service<GetDeviceConfig>(
+      "get_device_config", [this](const std::shared_ptr<GetDeviceConfig::Request> request,
+                                  std::shared_ptr<GetDeviceConfig::Response> response) {
+        getDeviceConfigCallback(request, response);
+      });
   get_sdk_version_srv_ = node_->create_service<GetString>(
       "get_sdk_version",
       [this](const std::shared_ptr<GetString::Request> request,
@@ -1118,6 +1176,183 @@ void OBCameraNode::getDeviceInfoCallback(const std::shared_ptr<GetDeviceInfo::Re
   }
 }
 
+void OBCameraNode::getDeviceConfigCallback(const std::shared_ptr<GetDeviceConfig::Request>& request,
+                                           std::shared_ptr<GetDeviceConfig::Response>& response) {
+  (void)request;
+  std::lock_guard<decltype(device_lock_)> lock(device_lock_);
+  response->schema_version = "1";
+  response->color_preset = color_preset_;
+  response->align_mode = align_mode_;
+  response->align_target_stream = alignTargetStreamToString(align_target_stream_);
+  response->time_domain = time_domain_;
+  response->frame_aggregate_mode = frame_aggregate_mode_;
+  response->disparity_to_depth_mode = disparity_to_depth_mode_;
+  response->sync_mode = OBSyncModeToString(sync_mode_);
+  response->depth_precision = depth_precision_str_;
+  response->enable_frame_sync = enable_frame_sync_;
+  response->depth_registration = depth_registration_;
+  response->exposure_range_mode = exposure_range_mode_;
+  response->intra_camera_sync_reference = intra_camera_sync_reference_;
+  response->data_json = "";
+
+  auto can_read = [this](OBPropertyID property_id) {
+    return device_->isPropertySupported(property_id, OB_PERMISSION_READ) ||
+           device_->isPropertySupported(property_id, OB_PERMISSION_READ_WRITE);
+  };
+
+  try {
+    if (can_read(OB_PROP_DISPARITY_TO_DEPTH_BOOL) &&
+        can_read(OB_PROP_SDK_DISPARITY_TO_DEPTH_BOOL)) {
+      response->disparity_to_depth_mode = disparityToDepthModeToString(
+          device_->getBoolProperty(OB_PROP_DISPARITY_TO_DEPTH_BOOL),
+          device_->getBoolProperty(OB_PROP_SDK_DISPARITY_TO_DEPTH_BOOL));
+    }
+  } catch (const ob::Error& e) {
+    RCLCPP_DEBUG_STREAM(logger_, "Failed to get disparity to depth mode: "
+                                     << orbbec_camera::formatObErrorWithStatus(e));
+  } catch (const std::exception& e) {
+    RCLCPP_DEBUG_STREAM(logger_, "Failed to get disparity to depth mode: " << e.what());
+  } catch (...) {
+    RCLCPP_DEBUG_STREAM(logger_, "Failed to get disparity to depth mode");
+  }
+
+  try {
+    if (can_read(OB_PROP_DEPTH_ALIGN_HARDWARE_BOOL)) {
+      const bool hardware_align_enabled =
+          device_->getBoolProperty(OB_PROP_DEPTH_ALIGN_HARDWARE_BOOL);
+      if (hardware_align_enabled) {
+        response->align_mode = "HW";
+        response->align_target_stream = "COLOR";
+        response->depth_registration = true;
+      } else if (align_mode_ == "HW") {
+        response->depth_registration = false;
+      }
+    }
+  } catch (const ob::Error& e) {
+    RCLCPP_DEBUG_STREAM(logger_, "Failed to get hardware depth alignment status: "
+                                     << orbbec_camera::formatObErrorWithStatus(e));
+  } catch (const std::exception& e) {
+    RCLCPP_DEBUG_STREAM(logger_, "Failed to get hardware depth alignment status: " << e.what());
+  } catch (...) {
+    RCLCPP_DEBUG_STREAM(logger_, "Failed to get hardware depth alignment status");
+  }
+
+  try {
+    response->sync_mode = OBSyncModeToString(device_->getMultiDeviceSyncConfig().syncMode);
+  } catch (const ob::Error& e) {
+    RCLCPP_DEBUG_STREAM(logger_, "Failed to get multi-device sync mode: "
+                                     << orbbec_camera::formatObErrorWithStatus(e));
+  } catch (const std::exception& e) {
+    RCLCPP_DEBUG_STREAM(logger_, "Failed to get multi-device sync mode: " << e.what());
+  } catch (...) {
+    RCLCPP_DEBUG_STREAM(logger_, "Failed to get multi-device sync mode");
+  }
+
+  try {
+    if (can_read(OB_PROP_DEPTH_PRECISION_LEVEL_INT)) {
+      response->depth_precision =
+          depthPrecisionLevelToString(device_->getIntProperty(OB_PROP_DEPTH_PRECISION_LEVEL_INT));
+    } else if (can_read(OB_PROP_DEPTH_UNIT_FLEXIBLE_ADJUSTMENT_FLOAT)) {
+      response->depth_precision =
+          std::to_string(device_->getFloatProperty(OB_PROP_DEPTH_UNIT_FLEXIBLE_ADJUSTMENT_FLOAT)) +
+          "mm";
+    }
+  } catch (const ob::Error& e) {
+    RCLCPP_DEBUG_STREAM(
+        logger_, "Failed to get depth precision: " << orbbec_camera::formatObErrorWithStatus(e));
+  } catch (const std::exception& e) {
+    RCLCPP_DEBUG_STREAM(logger_, "Failed to get depth precision: " << e.what());
+  } catch (...) {
+    RCLCPP_DEBUG_STREAM(logger_, "Failed to get depth precision");
+  }
+
+  try {
+    if (can_read(OB_PROP_DEVICE_PERFORMANCE_MODE_INT)) {
+      response->exposure_range_mode =
+          exposureRangeModeToString(device_->getIntProperty(OB_PROP_DEVICE_PERFORMANCE_MODE_INT));
+    }
+  } catch (const ob::Error& e) {
+    RCLCPP_DEBUG_STREAM(logger_, "Failed to get exposure range mode: "
+                                     << orbbec_camera::formatObErrorWithStatus(e));
+  } catch (const std::exception& e) {
+    RCLCPP_DEBUG_STREAM(logger_, "Failed to get exposure range mode: " << e.what());
+  } catch (...) {
+    RCLCPP_DEBUG_STREAM(logger_, "Failed to get exposure range mode");
+  }
+
+  try {
+    if (can_read(OB_PROP_INTRA_CAMERA_SYNC_REFERENCE_INT)) {
+      response->intra_camera_sync_reference = intraCameraSyncReferenceToString(
+          device_->getIntProperty(OB_PROP_INTRA_CAMERA_SYNC_REFERENCE_INT));
+    }
+  } catch (const ob::Error& e) {
+    RCLCPP_DEBUG_STREAM(logger_, "Failed to get intra-camera sync reference: "
+                                     << orbbec_camera::formatObErrorWithStatus(e));
+  } catch (const std::exception& e) {
+    RCLCPP_DEBUG_STREAM(logger_, "Failed to get intra-camera sync reference: " << e.what());
+  } catch (...) {
+    RCLCPP_DEBUG_STREAM(logger_, "Failed to get intra-camera sync reference");
+  }
+
+  try {
+    response->device_preset = device_->getCurrentPresetName();
+    if (response->device_preset == "Custom") {
+      try {
+        const char* current_depth_mode_name = device_->getCurrentDepthModeName();
+        if (current_depth_mode_name != nullptr) {
+          std::string current_depth_mode(current_depth_mode_name);
+          if (!current_depth_mode.empty()) {
+            response->device_preset = "Custom(from " + current_depth_mode + ")";
+          }
+        }
+      } catch (const ob::Error& e) {
+        RCLCPP_DEBUG_STREAM(logger_, "Failed to get current depth mode name: "
+                                         << orbbec_camera::formatObErrorWithStatus(e));
+      } catch (const std::exception& e) {
+        RCLCPP_DEBUG_STREAM(logger_, "Failed to get current depth mode name: " << e.what());
+      } catch (...) {
+        RCLCPP_DEBUG_STREAM(logger_, "Failed to get current depth mode name");
+      }
+    }
+  } catch (const ob::Error& e) {
+    RCLCPP_DEBUG_STREAM(
+        logger_, "Failed to get current preset: " << orbbec_camera::formatObErrorWithStatus(e));
+  } catch (const std::exception& e) {
+    RCLCPP_DEBUG_STREAM(logger_, "Failed to get current preset: " << e.what());
+  } catch (...) {
+    RCLCPP_DEBUG_STREAM(logger_, "Failed to get current preset");
+  }
+
+  try {
+    const auto color_preset = device_->getIntProperty(OB_PROP_COLOR_PRESET_PRIORITY_INT);
+    const auto color_preset_name = colorPresetToString(color_preset);
+    if (!color_preset_name.empty()) {
+      response->color_preset = color_preset_name;
+    }
+  } catch (const ob::Error& e) {
+    RCLCPP_DEBUG_STREAM(
+        logger_, "Failed to get color preset: " << orbbec_camera::formatObErrorWithStatus(e));
+  } catch (const std::exception& e) {
+    RCLCPP_DEBUG_STREAM(logger_, "Failed to get color preset: " << e.what());
+  } catch (...) {
+    RCLCPP_DEBUG_STREAM(logger_, "Failed to get color preset");
+  }
+
+  try {
+    response->preset_version = device_->getExtensionInfo("PresetVer");
+  } catch (const ob::Error& e) {
+    RCLCPP_DEBUG_STREAM(
+        logger_, "Failed to get preset version: " << orbbec_camera::formatObErrorWithStatus(e));
+  } catch (const std::exception& e) {
+    RCLCPP_DEBUG_STREAM(logger_, "Failed to get preset version: " << e.what());
+  } catch (...) {
+    RCLCPP_DEBUG_STREAM(logger_, "Failed to get preset version");
+  }
+
+  response->success = true;
+  response->message = "OK";
+}
+
 void OBCameraNode::getSDKVersion(const std::shared_ptr<GetString::Request>& request,
                                  std::shared_ptr<GetString::Response>& response) {
   (void)request;

orbbec_camera_msgs/CMakeLists.txt

@@ -26,6 +26,7 @@ rosidl_generate_interfaces(
     "msg/IMUInfo.msg"
     "msg/RGBD.msg"
     "srv/GetBool.srv"
+    "srv/GetDeviceConfig.srv"
     "srv/GetDeviceInfo.srv"
     "srv/GetCameraInfo.srv"
     "srv/GetInt32.srv"

orbbec_camera_msgs/srv/GetDeviceConfig.srv

@@ -0,0 +1,27 @@
+---
+# Schema version for this service response. Increment only when field semantics change.
+string schema_version
+
+# Effective device configuration state that is not provided by existing device/version services.
+string device_preset
+string preset_version
+string color_preset
+string depth_precision
+string disparity_to_depth_mode
+string exposure_range_mode
+
+bool depth_registration
+string align_mode
+string align_target_stream
+string frame_aggregate_mode
+bool enable_frame_sync
+
+string time_domain
+string sync_mode
+string intra_camera_sync_reference
+
+# Reserved for future non-breaking, device-specific extensions.
+string data_json
+
+bool success
+string message