i/OrbbecSDK_ROS2
1
0
Fork 0
mirror of https://github.com/orbbec/OrbbecSDK_ROS2.git synced 2026-09-12 11:10:19 +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/include/orbbec_camera/ob_camera_node.h
+8
View File
@@ -77,6 +77,7 @@ using Extrinsics = orbbec_camera_msgs::msg::Extrinsics;
using SetInt32 = orbbec_camera_msgs::srv::SetInt32;
using GetInt32 = orbbec_camera_msgs::srv::GetInt32;
using GetString = orbbec_camera_msgs::srv::GetString;
using SetBool = std_srvs::srv::SetBool;
typedef std::pair<ob_stream_type, int> stream_index_pair;
@@ -193,6 +194,12 @@ class OBCameraNode {
void getSDKVersion(const std::shared_ptr<GetString::Request>& request,
std::shared_ptr<GetString::Response>& response);
void toggleSensorCallback(const std::shared_ptr<SetBool::Request>& request,
std::shared_ptr<SetBool::Response>& response,
const stream_index_pair& stream_index);
bool toggleSensor(const stream_index_pair& stream_index, bool enabled, std::string& msg);
void publishPointCloud(std::shared_ptr<ob::FrameSet> frame_set);
void publishDepthPointCloud(std::shared_ptr<ob::FrameSet> frame_set);
@@ -256,6 +263,7 @@ class OBCameraNode {
std::map<stream_index_pair, rclcpp::Service<SetInt32>::SharedPtr> set_exposure_srv_;
std::map<stream_index_pair, rclcpp::Service<GetInt32>::SharedPtr> get_gain_srv_;
std::map<stream_index_pair, rclcpp::Service<SetInt32>::SharedPtr> set_gain_srv_;
std::map<stream_index_pair, rclcpp::Service<SetBool>::SharedPtr> toggle_sensor_srv_;
rclcpp::Service<GetInt32>::SharedPtr get_white_balance_srv_; // only rgb
rclcpp::Service<SetInt32>::SharedPtr set_white_balance_srv_;
rclcpp::Service<GetString>::SharedPtr get_sdk_version_srv_;
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