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

catch error when ctrl camera

Browse Source
This commit is contained in:
Joe Dong
2022-06-07 18:09:17 +08:00
parent 669755bf63
commit b51f8bf2b3
10 changed files with 298 additions and 109 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/src/device_ctrl_service.cpp
+179 -92
View File
@@ -9,7 +9,7 @@ void OBCameraNode::setupCameraCtrlServices() {
for (auto stream_index : IMAGE_STREAMS) {
auto stream_name = stream_name_[stream_index.first];
if (enable_[stream_index]) {
std::string service_name = "get/" + stream_name + "/exposure";
std::string service_name = "get_" + stream_name + "_exposure";
get_exposure_srv_[stream_index] = node_->create_service<GetInt32>(
service_name, [this, stream_index = stream_index](
const std::shared_ptr<rmw_request_id_t> request_header,
@@ -18,7 +18,7 @@ void OBCameraNode::setupCameraCtrlServices() {
getExposureCallback(request, response, stream_index);
});
service_name = "set/" + stream_name + "/exposure";
service_name = "set_" + stream_name + "_exposure";
set_exposure_srv_[stream_index] = node_->create_service<SetInt32>(
service_name, [this, stream_index = stream_index](
const std::shared_ptr<rmw_request_id_t> request_header,
@@ -26,7 +26,7 @@ void OBCameraNode::setupCameraCtrlServices() {
std::shared_ptr<SetInt32::Response> response) {
setExposureCallback(request, response, stream_index);
});
service_name = "get/" + stream_name + "/gain";
service_name = "get_" + stream_name + "_gain";
get_gain_srv_[stream_index] = node_->create_service<GetInt32>(
service_name, [this, stream_index = stream_index](
const std::shared_ptr<rmw_request_id_t> request_header,
@@ -35,7 +35,7 @@ void OBCameraNode::setupCameraCtrlServices() {
getGainCallback(request, response, stream_index);
});
service_name = "set/" + stream_name + "/gain";
service_name = "set_" + stream_name + "_gain";
set_gain_srv_[stream_index] = node_->create_service<SetInt32>(
service_name, [this, stream_index = stream_index](
const std::shared_ptr<rmw_request_id_t> request_header,
@@ -44,9 +44,7 @@ void OBCameraNode::setupCameraCtrlServices() {
setGainCallback(request, response, stream_index);
});
if (stream_index.first == OB_STREAM_COLOR || stream_index.first == OB_STREAM_DEPTH) {
service_name = "set/" + stream_name +
"/"
"auto_exposure";
service_name = "set_" + stream_name + "_auto_exposure";
set_auto_exposure_srv_[stream_index] = node_->create_service<SetBool>(
service_name, [this, stream_index = stream_index](
const std::shared_ptr<rmw_request_id_t> request_header,
@@ -59,40 +57,40 @@ void OBCameraNode::setupCameraCtrlServices() {
}
set_fan_mode_srv_ = node_->create_service<SetInt32>(
"set_fan_mode", [this](const std::shared_ptr<rmw_request_id_t> request_header,
const std::shared_ptr<SetInt32::Request> request,
std::shared_ptr<SetInt32::Response> response) {
const std::shared_ptr<SetInt32::Request> request,
std::shared_ptr<SetInt32::Response> response) {
setFanModeCallback(request_header, request, response);
});
set_floor_enable_srv_ = node_->create_service<SetBool>(
"set_floor_enable", [this](const std::shared_ptr<rmw_request_id_t> request_header,
const std::shared_ptr<SetBool::Request> request,
std::shared_ptr<SetBool::Response> response) {
const std::shared_ptr<SetBool::Request> request,
std::shared_ptr<SetBool::Response> response) {
setFloorEnableCallback(request_header, request, response);
});
set_laser_enable_srv_ = node_->create_service<SetBool>(
"set_laser_enable", [this](const std::shared_ptr<rmw_request_id_t> request_header,
const std::shared_ptr<SetBool::Request> request,
std::shared_ptr<SetBool::Response> response) {
const std::shared_ptr<SetBool::Request> request,
std::shared_ptr<SetBool::Response> response) {
setLaserEnableCallback(request_header, request, response);
});
set_ldp_enable_srv_ = node_->create_service<SetBool>(
"set_ldp_enable", [this](const std::shared_ptr<rmw_request_id_t> request_header,
const std::shared_ptr<SetBool::Request> request,
std::shared_ptr<SetBool::Response> response) {
const std::shared_ptr<SetBool::Request> request,
std::shared_ptr<SetBool::Response> response) {
setLdpEnableCallback(request_header, request, response);
});
get_white_balance_srv_ = node_->create_service<GetInt32>(
"get/white_balance", [this](const std::shared_ptr<rmw_request_id_t> request_header,
const std::shared_ptr<GetInt32::Request> request,
std::shared_ptr<GetInt32::Response> response) {
"get_white_balance", [this](const std::shared_ptr<rmw_request_id_t> request_header,
const std::shared_ptr<GetInt32::Request> request,
std::shared_ptr<GetInt32::Response> response) {
getWhiteBalanceCallback(request_header, request, response);
});
set_white_balance_srv_ = node_->create_service<SetInt32>(
"set/white_balance", [this](const std::shared_ptr<rmw_request_id_t> request_header,
const std::shared_ptr<SetInt32::Request> request,
std::shared_ptr<SetInt32::Response> response) {
"set_white_balance", [this](const std::shared_ptr<rmw_request_id_t> request_header,
const std::shared_ptr<SetInt32::Request> request,
std::shared_ptr<SetInt32::Response> response) {
setWhiteBalanceCallback(request_header, request, response);
});
}
@@ -101,19 +99,28 @@ void OBCameraNode::setExposureCallback(const std::shared_ptr<SetInt32::Request>&
std::shared_ptr<SetInt32::Response>& response,
const stream_index_pair& stream_index) {
auto stream = stream_index.first;
switch (stream) {
case OB_STREAM_IR:
device_->setIntProperty(OB_PROP_IR_EXPOSURE_INT, request->data);
break;
case OB_STREAM_DEPTH:
device_->setIntProperty(OB_PROP_DEPTH_EXPOSURE_INT, request->data);
break;
case OB_STREAM_COLOR:
device_->setIntProperty(OB_PROP_COLOR_EXPOSURE_INT, request->data);
break;
default:
RCLCPP_ERROR(logger_, "%s NOT a video stream", __FUNCTION__);
break;
try {
switch (stream) {
case OB_STREAM_IR:
device_->setIntProperty(OB_PROP_IR_EXPOSURE_INT, request->data);
break;
case OB_STREAM_DEPTH:
device_->setIntProperty(OB_PROP_DEPTH_EXPOSURE_INT, request->data);
break;
case OB_STREAM_COLOR:
device_->setIntProperty(OB_PROP_COLOR_EXPOSURE_INT, request->data);
break;
default:
RCLCPP_ERROR(logger_, "%s NOT a video stream", __FUNCTION__);
break;
}
} catch (const ob::Error& e) {
response->message = e.getMessage();
} catch (const std::exception& e) {
response->message = e.what();
} catch (...) {
RCLCPP_ERROR(logger_, "%s unknown error %d", __FUNCTION__, __LINE__);
response->message = "unknown error";
}
}
@@ -121,19 +128,27 @@ void OBCameraNode::getGainCallback(const std::shared_ptr<GetInt32::Request>& req
std::shared_ptr<GetInt32::Response>& response,
const stream_index_pair& stream_index) {
auto stream = stream_index.first;
switch (stream) {
case OB_STREAM_IR:
response->data = device_->getIntProperty(OB_PROP_IR_GAIN_INT);
break;
case OB_STREAM_DEPTH:
response->data = device_->getIntProperty(OB_PROP_DEPTH_GAIN_INT);
break;
case OB_STREAM_COLOR:
response->data = device_->getIntProperty(OB_PROP_COLOR_GAIN_INT);
break;
default:
RCLCPP_ERROR(logger_, " %s NOT a video stream", __FUNCTION__);
break;
try {
switch (stream) {
case OB_STREAM_IR:
response->data = device_->getIntProperty(OB_PROP_IR_GAIN_INT);
break;
case OB_STREAM_DEPTH:
response->data = device_->getIntProperty(OB_PROP_DEPTH_GAIN_INT);
break;
case OB_STREAM_COLOR:
response->data = device_->getIntProperty(OB_PROP_COLOR_GAIN_INT);
break;
default:
RCLCPP_ERROR(logger_, " %s NOT a video stream", __FUNCTION__);
break;
}
} catch (ob::Error& e) {
response->message = e.getMessage();
} catch (const std::exception& e) {
response->message = e.what();
} catch (...) {
response->message = "unknown error";
}
}
@@ -141,32 +156,56 @@ void OBCameraNode::setGainCallback(const std::shared_ptr<SetInt32 ::Request>& re
std::shared_ptr<SetInt32::Response>& response,
const stream_index_pair& stream_index) {
auto stream = stream_index.first;
switch (stream) {
case OB_STREAM_IR:
device_->setIntProperty(OB_PROP_IR_GAIN_INT, request->data);
break;
case OB_STREAM_DEPTH:
device_->setIntProperty(OB_PROP_DEPTH_GAIN_INT, request->data);
break;
case OB_STREAM_COLOR:
device_->setIntProperty(OB_PROP_COLOR_GAIN_INT, request->data);
break;
default:
RCLCPP_ERROR(logger_, "%s NOT a video stream", __FUNCTION__);
break;
try {
switch (stream) {
case OB_STREAM_IR:
device_->setIntProperty(OB_PROP_IR_GAIN_INT, request->data);
break;
case OB_STREAM_DEPTH:
device_->setIntProperty(OB_PROP_DEPTH_GAIN_INT, request->data);
break;
case OB_STREAM_COLOR:
device_->setIntProperty(OB_PROP_COLOR_GAIN_INT, request->data);
break;
default:
RCLCPP_ERROR(logger_, "%s NOT a video stream", __FUNCTION__);
break;
}
} catch (const ob::Error& e) {
response->message = e.getMessage();
} catch (const std::exception& e) {
response->message = e.what();
} catch (...) {
response->message = "unknown error";
}
}
void OBCameraNode::getWhiteBalanceCallback(const std::shared_ptr<rmw_request_id_t>& request_header,
const std::shared_ptr<GetInt32::Request>& request,
std::shared_ptr<GetInt32::Response>& response) {
response->data = device_->getIntProperty(OB_PROP_COLOR_WHITE_BALANCE_INT);
try {
response->data = device_->getIntProperty(OB_PROP_COLOR_WHITE_BALANCE_INT);
} catch (const ob::Error& e) {
response->message = e.getMessage();
} catch (const std::exception& e) {
response->message = e.what();
} catch (...) {
response->message = "unknown error";
}
}
void OBCameraNode::setWhiteBalanceCallback(const std::shared_ptr<rmw_request_id_t>& request_header,
const std::shared_ptr<SetInt32 ::Request>& request,
std::shared_ptr<SetInt32 ::Response>& response) {
device_->setIntProperty(OB_PROP_COLOR_WHITE_BALANCE_INT, request->data);
try {
device_->setIntProperty(OB_PROP_COLOR_WHITE_BALANCE_INT, request->data);
} catch (const ob::Error& e) {
response->message = e.getMessage();
} catch (const std::exception& e) {
response->message = e.what();
} catch (...) {
response->message = "unknown error";
}
}
void OBCameraNode::setAutoExposureCallback(
@@ -174,19 +213,27 @@ void OBCameraNode::setAutoExposureCallback(
std::shared_ptr<std_srvs::srv::SetBool::Response>& response,
const stream_index_pair& stream_index) {
auto stream = stream_index.first;
switch (stream) {
case OB_STREAM_IR:
response->message = "IR not support set auto exposure";
break;
case OB_STREAM_DEPTH:
device_->setIntProperty(OB_PROP_DEPTH_AUTO_EXPOSURE_BOOL, request->data);
break;
case OB_STREAM_COLOR:
device_->setIntProperty(OB_PROP_COLOR_AUTO_EXPOSURE_BOOL, request->data);
break;
default:
RCLCPP_ERROR(logger_, "%s NOT a video stream", __FUNCTION__);
break;
try {
switch (stream) {
case OB_STREAM_IR:
response->message = "IR not support set auto exposure";
break;
case OB_STREAM_DEPTH:
device_->setIntProperty(OB_PROP_DEPTH_AUTO_EXPOSURE_BOOL, request->data);
break;
case OB_STREAM_COLOR:
device_->setIntProperty(OB_PROP_COLOR_AUTO_EXPOSURE_BOOL, request->data);
break;
default:
RCLCPP_ERROR(logger_, "%s NOT a video stream", __FUNCTION__);
break;
}
} catch (const ob::Error& e) {
response->message = e.getMessage();
} catch (const std::exception& e) {
response->message = e.what();
} catch (...) {
response->message = "unknown error";
}
}
@@ -196,7 +243,15 @@ void OBCameraNode::setFanModeCallback(const std::shared_ptr<rmw_request_id_t>& r
(void)request_header;
(void)response;
bool fan_mode = request->data;
device_->setBoolProperty(OB_PROP_FAN_WORK_MODE_INT, fan_mode);
try {
device_->setBoolProperty(OB_PROP_FAN_WORK_MODE_INT, fan_mode);
} catch (const ob::Error& e) {
response->message = e.getMessage();
} catch (const std::exception& e) {
response->message = e.what();
} catch (...) {
response->message = "unknown error";
}
}
void OBCameraNode::setFloorEnableCallback(
@@ -206,7 +261,15 @@ void OBCameraNode::setFloorEnableCallback(
(void)request_header;
(void)response;
bool floor_enable = request->data;
device_->setBoolProperty(OB_PROP_FLOOD_BOOL, floor_enable);
try {
device_->setBoolProperty(OB_PROP_FLOOD_BOOL, floor_enable);
} catch (const ob::Error& e) {
response->message = e.getMessage();
} catch (const std::exception& e) {
response->message = e.what();
} catch (...) {
response->message = "unknown error";
}
}
void OBCameraNode::setLaserEnableCallback(
@@ -216,7 +279,15 @@ void OBCameraNode::setLaserEnableCallback(
(void)request_header;
(void)response;
bool laser_enable = request->data;
device_->setBoolProperty(OB_PROP_LASER_BOOL, laser_enable);
try {
device_->setBoolProperty(OB_PROP_LASER_BOOL, laser_enable);
} catch (const ob::Error& e) {
response->message = e.getMessage();
} catch (const std::exception& e) {
response->message = e.what();
} catch (...) {
response->message = "unknown error";
}
}
void OBCameraNode::setLdpEnableCallback(
@@ -226,25 +297,41 @@ void OBCameraNode::setLdpEnableCallback(
(void)request_header;
(void)response;
bool ldp_enable = request->data;
device_->setBoolProperty(OB_PROP_LDP_BOOL, ldp_enable);
try {
device_->setBoolProperty(OB_PROP_LDP_BOOL, ldp_enable);
} catch (const ob::Error& e) {
response->message = e.getMessage();
} catch (const std::exception& e) {
response->message = e.what();
} catch (...) {
response->message = "unknown error";
}
}
void OBCameraNode::getExposureCallback(const std::shared_ptr<GetInt32::Request>& request,
std::shared_ptr<GetInt32 ::Response>& response,
const stream_index_pair& stream_index) {
auto stream = stream_index.first;
switch (stream) {
case OB_STREAM_IR:
response->data = device_->getIntProperty(OB_PROP_IR_EXPOSURE_INT);
break;
case OB_STREAM_DEPTH:
response->data = device_->getIntProperty(OB_PROP_DEPTH_EXPOSURE_INT);
break;
case OB_STREAM_COLOR:
response->data = device_->getIntProperty(OB_PROP_COLOR_EXPOSURE_INT);
break;
default:
RCLCPP_ERROR(logger_, " %s NOT a video stream", __FUNCTION__);
break;
try {
switch (stream) {
case OB_STREAM_IR:
response->data = device_->getIntProperty(OB_PROP_IR_EXPOSURE_INT);
break;
case OB_STREAM_DEPTH:
response->data = device_->getIntProperty(OB_PROP_DEPTH_EXPOSURE_INT);
break;
case OB_STREAM_COLOR:
response->data = device_->getIntProperty(OB_PROP_COLOR_EXPOSURE_INT);
break;
default:
RCLCPP_ERROR(logger_, " %s NOT a video stream", __FUNCTION__);
break;
}
} catch (const ob::Error& e) {
response->message = e.getMessage();
} catch (const std::exception& e) {
response->message = e.what();
} catch (...) {
response->message = "unknown error";
}
}
} // namespace orbbec_camera