Add service to set color and depth ROI

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<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

@@ -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;

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)

orbbec_camera_msgs/srv/SetArrays.srv

@@ -0,0 +1,5 @@
+bool enable
+float32[] data_param
+---
+bool success
+string message