diff --git a/orbbec_camera/include/orbbec_camera/ob_camera_node.h b/orbbec_camera/include/orbbec_camera/ob_camera_node.h index 0e56756d..f8386048 100644 --- a/orbbec_camera/include/orbbec_camera/ob_camera_node.h +++ b/orbbec_camera/include/orbbec_camera/ob_camera_node.h @@ -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 stream_index_pair; @@ -256,6 +258,10 @@ class OBCameraNode { std::shared_ptr& response, const stream_index_pair& stream_index); + void setAeRoiCallback(const std::shared_ptr& request, + std::shared_ptr& response, + const stream_index_pair& stream_index); + void setLaserEnableCallback(const std::shared_ptr& request_header, const std::shared_ptr& request, std::shared_ptr& response); @@ -445,6 +451,7 @@ class OBCameraNode { rclcpp::Service::SharedPtr set_ir_long_exposure_srv_; std::map::SharedPtr> set_auto_exposure_srv_; + std::map::SharedPtr> set_ae_roi_srv_; rclcpp::Service::SharedPtr get_device_srv_; rclcpp::Service::SharedPtr set_laser_enable_srv_; rclcpp::Service::SharedPtr set_ldp_enable_srv_; diff --git a/orbbec_camera/src/ros_service.cpp b/orbbec_camera/src/ros_service.cpp index 2c46cd31..8c4f4791 100644 --- a/orbbec_camera/src/ros_service.cpp +++ b/orbbec_camera/src/ros_service.cpp @@ -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( + service_name, + [this, stream_index = stream_index](const std::shared_ptr request, + std::shared_ptr response) { + setAeRoiCallback(request, response, stream_index); + }); + service_name = "toggle_" + stream_name; toggle_sensor_srv_[stream_index] = node_->create_service( @@ -301,6 +309,59 @@ void OBCameraNode::setGainCallback(const std::shared_ptr& re } } +void OBCameraNode::setAeRoiCallback(const std::shared_ptr& request, + std::shared_ptr& 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(request->data_param[0]); + config.x1_right = static_cast(request->data_param[1]); + config.y0_top = static_cast(request->data_param[2]); + config.y1_bottom = static_cast(request->data_param[3]); + device_->setStructuredData(OB_STRUCT_DEPTH_AE_ROI, + reinterpret_cast(&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(request->data_param[0]); + config.x1_right = static_cast(request->data_param[1]); + config.y0_top = static_cast(request->data_param[2]); + config.y1_bottom = static_cast(request->data_param[3]); + device_->setStructuredData(OB_STRUCT_COLOR_AE_ROI, + reinterpret_cast(&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& request, std::shared_ptr& response) { (void)request; diff --git a/orbbec_camera_msgs/CMakeLists.txt b/orbbec_camera_msgs/CMakeLists.txt index e0ff1125..1d403f03 100644 --- a/orbbec_camera_msgs/CMakeLists.txt +++ b/orbbec_camera_msgs/CMakeLists.txt @@ -28,6 +28,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "srv/SetFilter.srv" "srv/SetInt32.srv" "srv/SetString.srv" + "srv/SetArrays.srv" DEPENDENCIES sensor_msgs std_msgs) diff --git a/orbbec_camera_msgs/srv/SetArrays.srv b/orbbec_camera_msgs/srv/SetArrays.srv new file mode 100644 index 00000000..79d458a6 --- /dev/null +++ b/orbbec_camera_msgs/srv/SetArrays.srv @@ -0,0 +1,5 @@ +bool enable +float32[] data_param +--- +bool success +string message \ No newline at end of file