i/OrbbecSDK_ROS2
1
0
Fork 0
mirror of https://github.com/orbbec/OrbbecSDK_ROS2.git synced 2026-10-05 04:27:46 +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/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) {