i/OrbbecSDK_ROS2
1
0
Fork 0
mirror of https://github.com/orbbec/OrbbecSDK_ROS2.git synced 2026-09-11 02:40:18 +08:00
Code Issues Packages Projects Releases Wiki Activity

feat: add sync IO voltage level configuration and service

Browse Source
This commit is contained in:
ob-yalian
2026-06-18 18:18:47 +08:00
parent 541575257a
commit dd9c0dcd42
5 changed files with 69 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/config/gemini305_dual_color.yaml
+1
View File
@@ -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: ""
orbbec_camera/include/orbbec_camera/ob_camera_node.h
+4
View File
@@ -424,6 +424,8 @@ class OBCameraNode {
std::shared_ptr<SetInt32::Response>& response);
void setDisparitySearchOffsetCallback(const std::shared_ptr<SetInt32::Request>& request,
std::shared_ptr<SetInt32::Response>& response);
void setSyncIoVoltageLevelCallback(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);
void sendSoftwareTriggerCallback(const std::shared_ptr<std_srvs::srv::SetBool::Request>& request,
@@ -683,6 +685,7 @@ class OBCameraNode {
rclcpp::Service<GetInt32>::SharedPtr get_point_cloud_decimation_srv_;
rclcpp::Service<SetInt32>::SharedPtr set_disparity_range_mode_srv_;
rclcpp::Service<SetInt32>::SharedPtr set_disparity_search_offset_srv_;
rclcpp::Service<SetInt32>::SharedPtr set_sync_io_voltage_level_srv_;
rclcpp::Service<orbbec_camera_msgs::srv::GetBool>::SharedPtr get_streams_enable_srv_;
rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr set_streams_enable_srv_;
rclcpp::Service<GetUserCalibParams>::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
orbbec_camera/launch/gemini_301_series.launch.py
+1
View File
@@ -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.
orbbec_camera/src/ob_camera_node.cpp
+14
View File
@@ -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<bool>(disparity_offset_config_, "disparity_offset_config", false);
setAndGetNodeParameter<int>(offset_index0_, "offset_index0", -1);
setAndGetNodeParameter<int>(offset_index1_, "offset_index1", -1);
setAndGetNodeParameter<int>(sync_io_voltage_level_, "sync_io_voltage_level", -1);
setAndGetNodeParameter<std::string>(frame_aggregate_mode_, "frame_aggregate_mode", "ANY");
frame_aggregate_mode_ =
orbbec_camera/src/ros_service.cpp
+49
View File
@@ -387,6 +387,11 @@ void OBCameraNode::setupCameraCtrlServices() {
std::shared_ptr<SetInt32::Response> response) {
setDisparitySearchOffsetCallback(request, response);
});
set_sync_io_voltage_level_srv_ = node_->create_service<SetInt32>(
"set_sync_io_voltage_level", [this](const std::shared_ptr<SetInt32::Request> request,
std::shared_ptr<SetInt32::Response> response) {
setSyncIoVoltageLevelCallback(request, response);
});
}
void OBCameraNode::getPointCloudDecimationCallback(
@@ -569,6 +574,50 @@ void OBCameraNode::setDisparitySearchOffsetCallback(
}
}
void OBCameraNode::setSyncIoVoltageLevelCallback(const std::shared_ptr<SetInt32::Request>& request,
std::shared_ptr<SetInt32::Response>& response) {
if (!request) {
response->success = false;
response->message = "Invalid request";
return;
}
std::lock_guard<decltype(device_lock_)> 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<std_srvs::srv::SetBool::Request> request,
std::shared_ptr<std_srvs::srv::SetBool::Response> response) {