i/OrbbecSDK_ROS2
1
0
Fork 0
mirror of https://github.com/orbbec/OrbbecSDK_ROS2.git synced 2026-10-04 20:17:47 +08:00
Code Issues Packages Projects Releases Wiki Activity

feat: add set_disparity_range_mode service and callback to OBCameraNode

Browse Source
This commit is contained in:
ob-yalian
2026-03-16 15:33:56 +08:00
parent 49b5af99a0
commit 6fe7ad8e27
2 changed files with 57 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/src/ros_service.cpp
+54
View File
@@ -283,6 +283,11 @@ void OBCameraNode::setupCameraCtrlServices() {
std::shared_ptr<GetInt32::Response> response) {
getPointCloudDecimationCallback(request, response);
});
set_disparity_range_mode_srv_ = node_->create_service<SetInt32>(
"set_disparity_range_mode", [this](const std::shared_ptr<SetInt32::Request> request,
std::shared_ptr<SetInt32::Response> response) {
setDisparityRangeModeCallback(request, response);
});
}
void OBCameraNode::getPointCloudDecimationCallback(
@@ -330,6 +335,55 @@ void OBCameraNode::setPointCloudDecimationCallback(
}
}
void OBCameraNode::setDisparityRangeModeCallback(const std::shared_ptr<SetInt32::Request>& request,
std::shared_ptr<SetInt32::Response>& response) {
if (!request) {
response->success = false;
response->message = "Invalid request";
return;
}
try {
if (!device_->isPropertySupported(OB_PROP_DISP_SEARCH_RANGE_MODE_INT, OB_PERMISSION_WRITE)) {
response->success = false;
response->message = "OB_PROP_DISP_SEARCH_RANGE_MODE_INT is not supported";
return;
}
const bool allow_set = isGemini435LePID(pid_) || enable_stream_[DEPTH];
if (!allow_set) {
response->success = false;
response->message = "Disparity range mode can only be set when depth stream is enabled";
return;
}
auto range = device_->getIntPropertyRange(OB_PROP_DISP_SEARCH_RANGE_MODE_INT);
if (hw_mode_index < range.min || hw_mode_index > range.max) {
response->success = false;
response->message =
"Invalid disparity range mode. Allowed values:" + std::to_string(range.min) + " to " +
std::to_string(range.max);
return;
}
device_->setIntProperty(OB_PROP_DISP_SEARCH_RANGE_MODE_INT, hw_mode_index);
disparity_range_mode_ = requested_mode_value;
RCLCPP_INFO_STREAM(logger_, "Set disparity_range_mode to " << requested_mode_value);
response->success = true;
response->message = "disparity_range_mode updated";
} catch (const ob::Error& e) {
response->success = false;
response->message = e.getMessage();
} 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) {