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

Fix range validation and error handling in setGainCallback and setAutoExposureCallback

Browse Source
This commit is contained in:
Joe Dong
2024-04-29 09:56:02 +08:00
parent dfe0ef803d
commit 99f56eaaca
1 changed files with 43 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/src/ros_service.cpp
+43 -11
View File
@@ -240,23 +240,34 @@ void OBCameraNode::setGainCallback(const std::shared_ptr<SetInt32 ::Request>& re
std::shared_ptr<SetInt32::Response>& response, std::shared_ptr<SetInt32::Response>& response,
const stream_index_pair& stream_index) { const stream_index_pair& stream_index) {
auto stream = stream_index.first; auto stream = stream_index.first;
OBPropertyID prop_id = OB_PROP_IR_GAIN_INT;
try { try {
switch (stream) { switch (stream) {
case OB_STREAM_IR_LEFT: case OB_STREAM_IR_LEFT:
case OB_STREAM_IR_RIGHT: case OB_STREAM_IR_RIGHT:
case OB_STREAM_IR: case OB_STREAM_IR:
device_->setIntProperty(OB_PROP_IR_GAIN_INT, request->data); prop_id = OB_PROP_IR_GAIN_INT;
break; break;
case OB_STREAM_DEPTH: case OB_STREAM_DEPTH:
device_->setIntProperty(OB_PROP_DEPTH_GAIN_INT, request->data); prop_id = OB_PROP_DEPTH_GAIN_INT;
break; break;
case OB_STREAM_COLOR: case OB_STREAM_COLOR:
device_->setIntProperty(OB_PROP_COLOR_GAIN_INT, request->data); prop_id = OB_PROP_COLOR_GAIN_INT;
break; break;
default: default:
RCLCPP_ERROR(logger_, "%s NOT a video stream", __FUNCTION__); RCLCPP_ERROR(logger_, "%s NOT a video stream", __FUNCTION__);
break; response->success = false;
response->message = "NOT a video stream";
return;
} }
auto range = device_->getIntPropertyRange(prop_id);
if (request->data < range.min || request->data > range.max) {
response->success = false;
RCLCPP_INFO_STREAM(logger_, "set gain value out of range");
response->message = "value out of range";
return;
}
device_->setIntProperty(prop_id, request->data);
response->success = true; response->success = true;
} catch (const ob::Error& e) { } catch (const ob::Error& e) {
response->success = false; response->success = false;
@@ -291,6 +302,20 @@ void OBCameraNode::getWhiteBalanceCallback(const std::shared_ptr<GetInt32::Reque
void OBCameraNode::setWhiteBalanceCallback(const std::shared_ptr<SetInt32 ::Request>& request, void OBCameraNode::setWhiteBalanceCallback(const std::shared_ptr<SetInt32 ::Request>& request,
std::shared_ptr<SetInt32 ::Response>& response) { std::shared_ptr<SetInt32 ::Response>& response) {
try { try {
auto range = device_->getIntPropertyRange(OB_PROP_COLOR_WHITE_BALANCE_INT);
if (request->data < range.min || request->data > range.max) {
response->success = false;
RCLCPP_INFO_STREAM(logger_, "set white balance value out of range");
response->message = "value out of range";
return;
}
bool auto_white_balance = device_->getBoolProperty(OB_PROP_COLOR_AUTO_WHITE_BALANCE_BOOL);
if (auto_white_balance) {
RCLCPP_WARN(logger_, "auto white balance is enabled, set white balance will be ignored");
response->success = false;
response->message = "auto white balance is enabled";
return;
}
device_->setIntProperty(OB_PROP_COLOR_WHITE_BALANCE_INT, request->data); device_->setIntProperty(OB_PROP_COLOR_WHITE_BALANCE_INT, request->data);
response->success = true; response->success = true;
} catch (const ob::Error& e) { } catch (const ob::Error& e) {
@@ -342,28 +367,35 @@ void OBCameraNode::setAutoExposureCallback(
std::shared_ptr<std_srvs::srv::SetBool::Response>& response, std::shared_ptr<std_srvs::srv::SetBool::Response>& response,
const stream_index_pair& stream_index) { const stream_index_pair& stream_index) {
auto stream = stream_index.first; auto stream = stream_index.first;
OBPropertyID prop_id = OB_PROP_IR_AUTO_EXPOSURE_BOOL;
try { try {
switch (stream) { switch (stream) {
case OB_STREAM_IR_LEFT: case OB_STREAM_IR_LEFT:
case OB_STREAM_IR_RIGHT: case OB_STREAM_IR_RIGHT:
case OB_STREAM_IR: case OB_STREAM_IR:
response->success = true; prop_id = OB_PROP_IR_AUTO_EXPOSURE_BOOL;
device_->setIntProperty(OB_PROP_IR_AUTO_EXPOSURE_BOOL, request->data);
break; break;
case OB_STREAM_DEPTH: case OB_STREAM_DEPTH:
device_->setIntProperty(OB_PROP_DEPTH_AUTO_EXPOSURE_BOOL, request->data); prop_id = OB_PROP_DEPTH_AUTO_EXPOSURE_BOOL;
response->success = true;
break; break;
case OB_STREAM_COLOR: case OB_STREAM_COLOR:
device_->setIntProperty(OB_PROP_COLOR_AUTO_EXPOSURE_BOOL, request->data); prop_id = OB_PROP_COLOR_AUTO_EXPOSURE_BOOL;
response->success = true;
break; break;
default: default:
RCLCPP_ERROR(logger_, "%s NOT a video stream", __FUNCTION__); RCLCPP_ERROR(logger_, "%s NOT a video stream", __FUNCTION__);
response->success = false; response->success = false;
response->message = "NOT a video stream"; response->message = "NOT a video stream";
break; return;
} }
auto range = device_->getIntPropertyRange(prop_id);
if (request->data < range.min || request->data > range.max) {
response->success = false;
RCLCPP_INFO_STREAM(logger_, "set auto exposure value out of range");
response->message = "value out of range";
return;
}
device_->setIntProperty(prop_id, request->data);
response->success = true;
} catch (const ob::Error& e) { } catch (const ob::Error& e) {
response->success = false; response->success = false;
response->message = e.getMessage(); response->message = e.getMessage();