From dd9c0dcd42e109a6754dbce1ef613b848fc990f7 Mon Sep 17 00:00:00 2001 From: ob-yalian Date: Thu, 18 Jun 2026 18:18:47 +0800 Subject: [PATCH] feat: add sync IO voltage level configuration and service --- .../config/gemini305_dual_color.yaml | 1 + .../include/orbbec_camera/ob_camera_node.h | 4 ++ .../launch/gemini_301_series.launch.py | 1 + orbbec_camera/src/ob_camera_node.cpp | 14 ++++++ orbbec_camera/src/ros_service.cpp | 49 +++++++++++++++++++ 5 files changed, 69 insertions(+) diff --git a/orbbec_camera/config/gemini305_dual_color.yaml b/orbbec_camera/config/gemini305_dual_color.yaml index 619b2b42..9f029837 100644 --- a/orbbec_camera/config/gemini305_dual_color.yaml +++ b/orbbec_camera/config/gemini305_dual_color.yaml @@ -29,6 +29,7 @@ diagnostic_period: 1.0 device_preset: Dual Color Streams retry_on_usb3_detection_failure: false enable_sync_host_time: true +sync_io_voltage_level: -1 time_sync_period: 6.0 time_domain: global config_file_path: "" diff --git a/orbbec_camera/include/orbbec_camera/ob_camera_node.h b/orbbec_camera/include/orbbec_camera/ob_camera_node.h index 2d55bd3b..55257d0b 100644 --- a/orbbec_camera/include/orbbec_camera/ob_camera_node.h +++ b/orbbec_camera/include/orbbec_camera/ob_camera_node.h @@ -424,6 +424,8 @@ class OBCameraNode { std::shared_ptr& response); void setDisparitySearchOffsetCallback(const std::shared_ptr& request, std::shared_ptr& response); + void setSyncIoVoltageLevelCallback(const std::shared_ptr& request, + std::shared_ptr& response); void setSYNCHostimeCallback(const std::shared_ptr& request, std::shared_ptr& response); void sendSoftwareTriggerCallback(const std::shared_ptr& request, @@ -683,6 +685,7 @@ class OBCameraNode { rclcpp::Service::SharedPtr get_point_cloud_decimation_srv_; rclcpp::Service::SharedPtr set_disparity_range_mode_srv_; rclcpp::Service::SharedPtr set_disparity_search_offset_srv_; + rclcpp::Service::SharedPtr set_sync_io_voltage_level_srv_; rclcpp::Service::SharedPtr get_streams_enable_srv_; rclcpp::Service::SharedPtr set_streams_enable_srv_; rclcpp::Service::SharedPtr get_user_calib_params_srv_; @@ -982,6 +985,7 @@ class OBCameraNode { bool disparity_offset_config_ = false; int offset_index0_ = -1; int offset_index1_ = -1; + int sync_io_voltage_level_ = -1; std::string frame_aggregate_mode_ = "ANY"; // # full_frame, color_frame, ANY or disable diff --git a/orbbec_camera/launch/gemini_301_series.launch.py b/orbbec_camera/launch/gemini_301_series.launch.py index 32d27863..901c6fe8 100644 --- a/orbbec_camera/launch/gemini_301_series.launch.py +++ b/orbbec_camera/launch/gemini_301_series.launch.py @@ -270,6 +270,7 @@ def generate_launch_description(): DeclareLaunchArgument('device_preset', default_value='Default'), # Default, High Accuracy, Close Range High Accuracy, Factory Calib, Dual Color Streams, Custom DeclareLaunchArgument('retry_on_usb3_detection_failure', default_value='false'), DeclareLaunchArgument('enable_sync_host_time', default_value='false'), + DeclareLaunchArgument('sync_io_voltage_level', default_value='-1'), DeclareLaunchArgument('time_sync_period', default_value='6.0'), # seconds DeclareLaunchArgument('time_domain', default_value='global'),# global, device, system DeclareLaunchArgument('timestamp_clock_type', default_value=''),# realtime or monotonic, default is realtime. diff --git a/orbbec_camera/src/ob_camera_node.cpp b/orbbec_camera/src/ob_camera_node.cpp index 930f40b6..d5313742 100644 --- a/orbbec_camera/src/ob_camera_node.cpp +++ b/orbbec_camera/src/ob_camera_node.cpp @@ -890,6 +890,19 @@ void OBCameraNode::setupDevices() { TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_DEVICE_USB3_REPEAT_IDENTIFY_BOOL, retry_on_usb3_detection_failure_); } + if (sync_io_voltage_level_ != -1 && + device_->isPropertySupported(OB_PROP_USB_SYNC_VOLTAGE_LEVEL_INT, OB_PERMISSION_READ_WRITE)) { + auto range = device_->getIntPropertyRange(OB_PROP_USB_SYNC_VOLTAGE_LEVEL_INT); + if (sync_io_voltage_level_ < range.min || sync_io_voltage_level_ > range.max) { + RCLCPP_ERROR_STREAM( + logger_, "sync IO voltage level is out of range " << range.min << " - " << range.max); + } else { + TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_USB_SYNC_VOLTAGE_LEVEL_INT, + sync_io_voltage_level_); + RCLCPP_INFO_STREAM(logger_, "Current sync IO voltage level: " << device_->getIntProperty( + OB_PROP_USB_SYNC_VOLTAGE_LEVEL_INT)); + } + } if (should_apply_launch_config("enable_heartbeat") && device_->isPropertySupported(OB_PROP_HEARTBEAT_BOOL, OB_PERMISSION_READ_WRITE)) { TRY_TO_SET_PROPERTY(setBoolProperty, OB_PROP_HEARTBEAT_BOOL, enable_heartbeat_); @@ -3820,6 +3833,7 @@ void OBCameraNode::getParameters() { setAndGetNodeParameter(disparity_offset_config_, "disparity_offset_config", false); setAndGetNodeParameter(offset_index0_, "offset_index0", -1); setAndGetNodeParameter(offset_index1_, "offset_index1", -1); + setAndGetNodeParameter(sync_io_voltage_level_, "sync_io_voltage_level", -1); setAndGetNodeParameter(frame_aggregate_mode_, "frame_aggregate_mode", "ANY"); frame_aggregate_mode_ = diff --git a/orbbec_camera/src/ros_service.cpp b/orbbec_camera/src/ros_service.cpp index 7790dfa8..29bf9a08 100644 --- a/orbbec_camera/src/ros_service.cpp +++ b/orbbec_camera/src/ros_service.cpp @@ -387,6 +387,11 @@ void OBCameraNode::setupCameraCtrlServices() { std::shared_ptr response) { setDisparitySearchOffsetCallback(request, response); }); + set_sync_io_voltage_level_srv_ = node_->create_service( + "set_sync_io_voltage_level", [this](const std::shared_ptr request, + std::shared_ptr response) { + setSyncIoVoltageLevelCallback(request, response); + }); } void OBCameraNode::getPointCloudDecimationCallback( @@ -569,6 +574,50 @@ void OBCameraNode::setDisparitySearchOffsetCallback( } } +void OBCameraNode::setSyncIoVoltageLevelCallback(const std::shared_ptr& request, + std::shared_ptr& response) { + if (!request) { + response->success = false; + response->message = "Invalid request"; + return; + } + + std::lock_guard lock(device_lock_); + try { + if (!device_->isPropertySupported(OB_PROP_USB_SYNC_VOLTAGE_LEVEL_INT, + OB_PERMISSION_READ_WRITE)) { + response->success = false; + response->message = "Current device does not support sync IO voltage level"; + return; + } + + auto range = device_->getIntPropertyRange(OB_PROP_USB_SYNC_VOLTAGE_LEVEL_INT); + if (request->data < range.min || request->data > range.max) { + response->success = false; + response->message = + "Invalid sync IO voltage level. Allowed values:" + std::to_string(range.min) + " to " + + std::to_string(range.max); + return; + } + + device_->setIntProperty(OB_PROP_USB_SYNC_VOLTAGE_LEVEL_INT, request->data); + sync_io_voltage_level_ = device_->getIntProperty(OB_PROP_USB_SYNC_VOLTAGE_LEVEL_INT); + response->success = true; + response->message = + "sync_io_voltage_level updated to " + std::to_string(sync_io_voltage_level_); + RCLCPP_INFO_STREAM(logger_, response->message); + } catch (const ob::Error& e) { + response->success = false; + response->message = orbbec_camera::formatObErrorWithStatus(e); + } catch (const std::exception& e) { + response->success = false; + response->message = e.what(); + } catch (...) { + response->success = false; + response->message = "unknown error"; + } +} + void OBCameraNode::setStreamsEnableCallback( const std::shared_ptr request, std::shared_ptr response) {