From e20b90fe988ae9a47bdc80ac572b624a9f26134c Mon Sep 17 00:00:00 2001 From: ob-yalian Date: Thu, 18 Jun 2026 16:11:16 +0800 Subject: [PATCH] feat: add GetDeviceConfig service and related functionality --- .../include/orbbec_camera/ob_camera_node.h | 6 + orbbec_camera/src/ros_service.cpp | 235 ++++++++++++++++++ orbbec_camera_msgs/CMakeLists.txt | 1 + orbbec_camera_msgs/srv/GetDeviceConfig.srv | 27 ++ 4 files changed, 269 insertions(+) create mode 100644 orbbec_camera_msgs/srv/GetDeviceConfig.srv diff --git a/orbbec_camera/include/orbbec_camera/ob_camera_node.h b/orbbec_camera/include/orbbec_camera/ob_camera_node.h index 9fd9c2a4..2d55bd3b 100644 --- a/orbbec_camera/include/orbbec_camera/ob_camera_node.h +++ b/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& request, std::shared_ptr& response); + void getDeviceConfigCallback(const std::shared_ptr& request, + std::shared_ptr& response); + void getSDKVersion(const std::shared_ptr& request, std::shared_ptr& response); @@ -658,6 +663,7 @@ class OBCameraNode { set_auto_exposure_srv_; std::map::SharedPtr> set_ae_roi_srv_; rclcpp::Service::SharedPtr get_device_srv_; + rclcpp::Service::SharedPtr get_device_config_srv_; rclcpp::Service::SharedPtr set_laser_enable_srv_; rclcpp::Service::SharedPtr set_ldp_enable_srv_; rclcpp::Service::SharedPtr get_ldp_status_srv_; diff --git a/orbbec_camera/src/ros_service.cpp b/orbbec_camera/src/ros_service.cpp index ab37d889..7790dfa8 100644 --- a/orbbec_camera/src/ros_service.cpp +++ b/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 response) { getDeviceInfoCallback(request, response); }); + get_device_config_srv_ = node_->create_service( + "get_device_config", [this](const std::shared_ptr request, + std::shared_ptr response) { + getDeviceConfigCallback(request, response); + }); get_sdk_version_srv_ = node_->create_service( "get_sdk_version", [this](const std::shared_ptr request, @@ -1118,6 +1176,183 @@ void OBCameraNode::getDeviceInfoCallback(const std::shared_ptr& request, + std::shared_ptr& response) { + (void)request; + std::lock_guard 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& request, std::shared_ptr& response) { (void)request; diff --git a/orbbec_camera_msgs/CMakeLists.txt b/orbbec_camera_msgs/CMakeLists.txt index 09dfbf50..0fdae6a6 100644 --- a/orbbec_camera_msgs/CMakeLists.txt +++ b/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" diff --git a/orbbec_camera_msgs/srv/GetDeviceConfig.srv b/orbbec_camera_msgs/srv/GetDeviceConfig.srv new file mode 100644 index 00000000..e7cdb8ab --- /dev/null +++ b/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