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

Add service to set color and depth ROI

Browse Source
This commit is contained in:
jj
2025-03-21 09:28:42 +08:00
parent f380fc56bc
commit a14ac42178
4 changed files with 74 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
orbbec_camera/include/orbbec_camera/ob_camera_node.h
+7
View File
@@ -57,6 +57,7 @@
#include "orbbec_camera_msgs/srv/get_bool.hpp"
#include "orbbec_camera_msgs/srv/set_string.hpp"
#include "orbbec_camera_msgs/srv/set_filter.hpp"
#include "orbbec_camera_msgs/srv/set_arrays.hpp"
#include "orbbec_camera/constants.h"
#include "orbbec_camera/dynamic_params.h"
#include "orbbec_camera/d2c_viewer.h"
@@ -109,6 +110,7 @@ using SetString = orbbec_camera_msgs::srv::SetString;
using SetBool = std_srvs::srv::SetBool;
using GetBool = orbbec_camera_msgs::srv::GetBool;
using SetFilter = orbbec_camera_msgs::srv::SetFilter;
using SetArrays = orbbec_camera_msgs::srv::SetArrays;
typedef std::pair<ob_stream_type, int> stream_index_pair;
@@ -256,6 +258,10 @@ class OBCameraNode {
std::shared_ptr<std_srvs::srv::SetBool::Response>& response,
const stream_index_pair& stream_index);
void setAeRoiCallback(const std::shared_ptr<SetArrays::Request>& request,
std::shared_ptr<SetArrays::Response>& response,
const stream_index_pair& stream_index);
void setLaserEnableCallback(const std::shared_ptr<rmw_request_id_t>& request_header,
const std::shared_ptr<std_srvs::srv::SetBool::Request>& request,
std::shared_ptr<std_srvs::srv::SetBool::Response>& response);
@@ -445,6 +451,7 @@ class OBCameraNode {
rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr set_ir_long_exposure_srv_;
std::map<stream_index_pair, rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr>
set_auto_exposure_srv_;
std::map<stream_index_pair, rclcpp::Service<SetArrays>::SharedPtr> set_ae_roi_srv_;
rclcpp::Service<GetDeviceInfo>::SharedPtr get_device_srv_;
rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr set_laser_enable_srv_;
rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr set_ldp_enable_srv_;
orbbec_camera/src/ros_service.cpp
+61
View File
@@ -67,6 +67,14 @@ void OBCameraNode::setupCameraCtrlServices() {
setAutoExposureCallback(request, response, stream_index);
});
service_name = "set_" + stream_name + "_ae_roi";
set_ae_roi_srv_[stream_index] = node_->create_service<SetArrays>(
service_name,
[this, stream_index = stream_index](const std::shared_ptr<SetArrays::Request> request,
std::shared_ptr<SetArrays::Response> response) {
setAeRoiCallback(request, response, stream_index);
});
service_name = "toggle_" + stream_name;
toggle_sensor_srv_[stream_index] = node_->create_service<SetBool>(
@@ -301,6 +309,59 @@ void OBCameraNode::setGainCallback(const std::shared_ptr<SetInt32 ::Request>& re
}
}
void OBCameraNode::setAeRoiCallback(const std::shared_ptr<SetArrays ::Request>& request,
std::shared_ptr<SetArrays::Response>& response,
const stream_index_pair& stream_index) {
auto stream = stream_index.first;
auto config = OBRegionOfInterest();
try {
switch (stream) {
case OB_STREAM_IR_LEFT:
case OB_STREAM_IR_RIGHT:
case OB_STREAM_IR:
case OB_STREAM_DEPTH:
config.x0_left = static_cast<short int>(request->data_param[0]);
config.x1_right = static_cast<short int>(request->data_param[1]);
config.y0_top = static_cast<short int>(request->data_param[2]);
config.y1_bottom = static_cast<short int>(request->data_param[3]);
device_->setStructuredData(OB_STRUCT_DEPTH_AE_ROI,
reinterpret_cast<const uint8_t*>(&config), sizeof(config));
RCLCPP_INFO_STREAM(logger_,
"set depth AE ROI : " << "[Left: " << config.x0_left << ", Right: "
<< config.x1_right << ", Top: " << config.y0_top
<< ", Bottom: " << config.y1_bottom << " ]");
break;
case OB_STREAM_COLOR:
config.x0_left = static_cast<short int>(request->data_param[0]);
config.x1_right = static_cast<short int>(request->data_param[1]);
config.y0_top = static_cast<short int>(request->data_param[2]);
config.y1_bottom = static_cast<short int>(request->data_param[3]);
device_->setStructuredData(OB_STRUCT_COLOR_AE_ROI,
reinterpret_cast<const uint8_t*>(&config), sizeof(config));
RCLCPP_INFO_STREAM(logger_,
"set color AE ROI : " << "[Left: " << config.x0_left << ", Right: "
<< config.x1_right << ", Top: " << config.y0_top
<< ", Bottom: " << config.y1_bottom << " ]");
break;
default:
RCLCPP_ERROR(logger_, "%s NOT a video stream", __FUNCTION__);
response->success = false;
response->message = "NOT a video stream";
return;
}
response->success = true;
} 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::getWhiteBalanceCallback(const std::shared_ptr<GetInt32::Request>& request,
std::shared_ptr<GetInt32::Response>& response) {
(void)request;