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

add toggle sesror service

Browse Source
This commit is contained in:
Joe Dong
2022-06-13 16:02:49 +08:00
parent 008aea027a
commit 60482e86be
3 changed files with 101 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/src/ob_camera_node.cpp
+6
View File
@@ -105,6 +105,9 @@ void OBCameraNode::setupDevices() {
}
void OBCameraNode::setupProfiles() {
if (config_ != nullptr) {
config_.reset();
}
config_ = std::make_shared<ob::Config>();
for (const auto& elem : IMAGE_STREAMS) {
if (enable_[elem]) {
@@ -166,6 +169,9 @@ void OBCameraNode::startPipeline() {
config_->setAlignMode(ALIGN_DISABLE);
align_depth_ = false;
}
if (pipeline_ != nullptr) {
pipeline_.reset();
}
pipeline_ = std::make_unique<ob::Pipeline>(device_);
pipeline_->start(config_, [this](std::shared_ptr<ob::FrameSet> frame_set) {
frameSetCallback(std::move(frame_set));
orbbec_camera/src/ros_service.cpp
+87 -36
View File
@@ -22,47 +22,53 @@ void OBCameraNode::setupCameraCtrlServices() {
using std_srvs::srv::SetBool;
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";
get_exposure_srv_[stream_index] = node_->create_service<GetInt32>(
service_name,
[this, stream_index = stream_index](const std::shared_ptr<GetInt32::Request> request,
std::shared_ptr<GetInt32::Response> response) {
getExposureCallback(request, response, stream_index);
});
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<GetInt32::Request> request,
std::shared_ptr<GetInt32::Response> response) {
getExposureCallback(request, response, stream_index);
});
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<SetInt32::Request> request,
std::shared_ptr<SetInt32::Response> response) {
setExposureCallback(request, response, stream_index);
});
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<GetInt32::Request> request,
std::shared_ptr<GetInt32::Response> response) {
getGainCallback(request, response, stream_index);
});
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<SetInt32::Request> request,
std::shared_ptr<SetInt32::Response> response) {
setExposureCallback(request, response, stream_index);
});
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<GetInt32::Request> request,
std::shared_ptr<GetInt32::Response> response) {
getGainCallback(request, response, stream_index);
});
service_name = "set_" + stream_name + "_gain";
set_gain_srv_[stream_index] = node_->create_service<SetInt32>(
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<SetInt32::Request> request,
std::shared_ptr<SetInt32::Response> response) {
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";
set_auto_exposure_srv_[stream_index] = node_->create_service<SetBool>(
service_name,
[this, stream_index = stream_index](const std::shared_ptr<SetInt32::Request> request,
std::shared_ptr<SetInt32::Response> response) {
setGainCallback(request, response, stream_index);
[this, stream_index = stream_index](const std::shared_ptr<SetBool::Request> request,
std::shared_ptr<SetBool::Response> response) {
setAutoExposureCallback(request, response, stream_index);
});
if (stream_index.first == OB_STREAM_COLOR || stream_index.first == OB_STREAM_DEPTH) {
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<SetBool::Request> request,
std::shared_ptr<SetBool::Response> response) {
setAutoExposureCallback(request, response, stream_index);
});
}
}
service_name = "toggle_" + stream_name;
toggle_sensor_srv_[stream_index] = node_->create_service<SetBool>(
service_name,
[this, stream_index = stream_index](const std::shared_ptr<SetBool::Request> request,
std::shared_ptr<SetBool::Response> response) {
toggleSensorCallback(request, response, stream_index);
});
}
set_fan_mode_srv_ = node_->create_service<SetInt32>(
"set_fan_mode", [this](const std::shared_ptr<SetInt32::Request> request,
@@ -443,4 +449,49 @@ void OBCameraNode::getSDKVersion(const std::shared_ptr<GetString::Request>& requ
response->message = "unknown error";
}
}
void OBCameraNode::toggleSensorCallback(const std::shared_ptr<SetBool::Request>& request,
std::shared_ptr<SetBool::Response>& response,
const stream_index_pair& stream_index) {
std::string msg;
if (request->data) {
if (enable_[stream_index]) {
msg = stream_name_[stream_index.first] + " Already ON";
}
RCLCPP_INFO_STREAM(logger_, "toggling sensor " << stream_name_[stream_index.first] << " ON");
} else {
if (!enable_[stream_index]) {
msg = stream_name_[stream_index.first] + " Already OFF";
}
RCLCPP_INFO_STREAM(logger_, "toggling sensor " << stream_name_[stream_index.first] << " OFF");
}
if (!msg.empty()) {
RCLCPP_ERROR_STREAM(logger_, msg);
response->success = false;
response->message = msg;
return;
}
response->success = toggleSensor(stream_index, request->data, response->message);
}
bool OBCameraNode::toggleSensor(const stream_index_pair& stream_index, bool enabled,
std::string& msg) {
try {
pipeline_->stop();
enable_[stream_index] = enabled;
setupProfiles();
startPipeline();
return true;
} catch (const ob::Error& e) {
msg = e.getMessage();
return false;
} catch (const std::exception& e) {
msg = e.what();
return false;
} catch (...) {
msg = "unknown error";
return false;
}
}
} // namespace orbbec_camera