Add AE ROI setting param in gemini_330_series.launch.py

README.MD

@@ -385,6 +385,7 @@ The following are the launch parameters available:
   case-insensitive. These correspond
   to `rmw_qos_profile_system_default`, `rmw_qos_profile_default`, `rmw_qos_profile_parameter_events`, `rmw_qos_profile_services_default`, `rmw_qos_profile_parameters`,
   and `SENSOR_DATA`, respectively.
+- `color_ae_roi_[left|right|top|bottom]`,`depth_ae_roi_[left|right|top|bottom]`:Set Color and Depth auto exposure ROI.
 - `enable_d2c_viewer`: Publishes the D2C overlay image (for testing only).
 - `device_num`: The number of devices. This must be filled in if multiple cameras are required.
 - `uvc_backend`:Optional values: v4l2, libuvc
@@ -470,7 +471,7 @@ these settings.*
   * Imagine we are standing behind of the camera, and looking forward.
   * Always use this point of view when talking about coordinates, left vs right IRs, position of sensor, etc..
 
-![ROS2 and Camera Coordinate System](docs/source/image/image7.png)
+![ROS2 and Camera Coordinate System](docs/images/image13.png)
 
 * ROS2 Coordinate System: (X: Forward, Y:Left, Z: Up)
 * Camera Optical Coordinate System: (X: Right, Y: Down, Z: Forward)
@@ -479,9 +480,9 @@ these settings.*
 
 ## Camera sensor structure
 
-![module in rviz2](docs/source/image/image9.png)
+![module in rviz2](docs/images/image16.png)
 
-![module in rviz2](docs/source/image/image10.png)
+![module in rviz2](docs/images/image14.png)
 
 ## TF from coordinate A to coordinate B:
 
@@ -497,7 +498,7 @@ Example of static TFs of RGB sensor and right infra sensor of Gemini335 module a
 ros2 launch orbbec_description view_model.launch.py model:=gemini_335_336.urdf.xacro
 ```
 
-![module in rviz2](docs/source/image/image8.png)
+![module in rviz2](docs/images/image15.png)
 
 ## Predefined presets
 

orbbec_camera/include/orbbec_camera/ob_camera_node.h

@@ -379,6 +379,10 @@ class OBCameraNode {
   int init_interleave_hdr_param();
   int init_interleave_laser_param();
 
+  // Set ROI
+  void setColorAutoExposureROI();
+  void setDepthAutoExposureROI();
+
   void setDisparitySearchOffset();
 
  private:
@@ -508,6 +512,11 @@ class OBCameraNode {
   bool enable_ir_long_exposure_ = false;
   bool enable_ldp_ = true;
   int ldp_power_level_ = -1;
+  // color ae roi
+  int color_ae_roi_left_ = -1;
+  int color_ae_roi_top_ = -1;
+  int color_ae_roi_right_ = -1;
+  int color_ae_roi_bottom_ = -1;
   int color_exposure_ = -1;
   int color_gain_ = -1;
   int color_white_balance_ = -1;
@@ -521,6 +530,11 @@ class OBCameraNode {
   bool enable_color_backlight_compenstation_ = false;
   bool enable_color_decimation_filter_ = false;
   int color_decimation_filter_scale_ = -1;
+  // depth ae roi
+  int depth_ae_roi_left_ = -1;
+  int depth_ae_roi_top_ = -1;
+  int depth_ae_roi_right_ = -1;
+  int depth_ae_roi_bottom_ = -1;
   int depth_brightness_ = -1;
   int ir_exposure_ = -1;
   int ir_gain_ = -1;

orbbec_camera/launch/gemini_330_series.launch.py

@@ -72,6 +72,10 @@ def generate_launch_description():
         DeclareLaunchArgument('color_camera_info_qos', default_value='default'),
         DeclareLaunchArgument('enable_color_auto_exposure_priority', default_value='false'),
         DeclareLaunchArgument('enable_color_auto_exposure', default_value='true'),
+        DeclareLaunchArgument('color_ae_roi_left', default_value='-1'),
+        DeclareLaunchArgument('color_ae_roi_right', default_value='-1'),
+        DeclareLaunchArgument('color_ae_roi_top', default_value='-1'),
+        DeclareLaunchArgument('color_ae_roi_bottom', default_value='-1'),
         DeclareLaunchArgument('color_exposure', default_value='-1'),
         DeclareLaunchArgument('color_gain', default_value='-1'),
         DeclareLaunchArgument('enable_color_auto_white_balance', default_value='true'),
@@ -94,6 +98,10 @@ def generate_launch_description():
         DeclareLaunchArgument('depth_qos', default_value='default'),
         DeclareLaunchArgument('depth_camera_info_qos', default_value='default'),
         DeclareLaunchArgument('enable_depth_auto_exposure_priority', default_value='false'),
+        DeclareLaunchArgument('depth_ae_roi_left', default_value='-1'),
+        DeclareLaunchArgument('depth_ae_roi_right', default_value='-1'),
+        DeclareLaunchArgument('depth_ae_roi_top', default_value='-1'),
+        DeclareLaunchArgument('depth_ae_roi_bottom', default_value='-1'),
         DeclareLaunchArgument('depth_brightness', default_value='-1'),
         DeclareLaunchArgument('left_ir_width', default_value='0'),
         DeclareLaunchArgument('left_ir_height', default_value='0'),

orbbec_camera/src/ob_camera_node.cpp

@@ -1390,6 +1390,10 @@ void OBCameraNode::getParameters() {
   setAndGetNodeParameter<bool>(enable_color_auto_exposure_, "enable_color_auto_exposure", true);
   setAndGetNodeParameter<bool>(enable_color_auto_white_balance_, "enable_color_auto_white_balance",
                                true);
+  setAndGetNodeParameter<int>(color_ae_roi_left_, "color_ae_roi_left", -1);
+  setAndGetNodeParameter<int>(color_ae_roi_top_, "color_ae_roi_top", -1);
+  setAndGetNodeParameter<int>(color_ae_roi_right_, "color_ae_roi_right", -1);
+  setAndGetNodeParameter<int>(color_ae_roi_bottom_, "color_ae_roi_bottom", -1);
   setAndGetNodeParameter<int>(color_exposure_, "color_exposure", -1);
   setAndGetNodeParameter<int>(color_gain_, "color_gain", -1);
   setAndGetNodeParameter<int>(color_white_balance_, "color_white_balance", -1);
@@ -1407,6 +1411,10 @@ void OBCameraNode::getParameters() {
   setAndGetNodeParameter<int>(color_decimation_filter_scale_, "color_decimation_filter_scale", -1);
   setAndGetNodeParameter<bool>(enable_depth_auto_exposure_priority_,
                                "enable_depth_auto_exposure_priority", false);
+  setAndGetNodeParameter<int>(depth_ae_roi_left_, "depth_ae_roi_left", -1);
+  setAndGetNodeParameter<int>(depth_ae_roi_top_, "depth_ae_roi_top", -1);
+  setAndGetNodeParameter<int>(depth_ae_roi_right_, "depth_ae_roi_right", -1);
+  setAndGetNodeParameter<int>(depth_ae_roi_bottom_, "depth_ae_roi_bottom", -1);
   setAndGetNodeParameter<int>(depth_brightness_, "depth_brightness", -1);
   setAndGetNodeParameter<bool>(enable_ir_auto_exposure_, "enable_ir_auto_exposure", true);
   setAndGetNodeParameter<int>(ir_exposure_, "ir_exposure", -1);
@@ -2122,6 +2130,50 @@ void OBCameraNode::setDisparitySearchOffset() {
   has_run = true;
 }
 
+void OBCameraNode::setDepthAutoExposureROI() {
+  static bool depth_roi_has_run = false;
+  if (depth_roi_has_run) {
+    return;
+  }
+  if (depth_ae_roi_left_ != -1 && depth_ae_roi_top_ != -1 && depth_ae_roi_right_ != -1 &&
+      depth_ae_roi_bottom_ != -1 &&
+      device_->isPropertySupported(OB_STRUCT_DEPTH_AE_ROI, OB_PERMISSION_READ_WRITE)) {
+    RCLCPP_INFO_STREAM(logger_, "Setting depth AE ROI to "
+                                    << depth_ae_roi_left_ << ", " << depth_ae_roi_top_ << ", "
+                                    << depth_ae_roi_right_ << ", " << depth_ae_roi_bottom_);
+    auto config = OBRegionOfInterest();
+    config.x0_left = depth_ae_roi_left_;
+    config.y0_top = depth_ae_roi_top_;
+    config.x1_right = depth_ae_roi_right_;
+    config.y1_bottom = depth_ae_roi_bottom_;
+    device_->setStructuredData(OB_STRUCT_DEPTH_AE_ROI,
+                                   reinterpret_cast<const uint8_t*>(&config), sizeof(config));
+  }
+  depth_roi_has_run = true;
+}
+
+void OBCameraNode::setColorAutoExposureROI() {
+  static bool color_roi_has_run = false;
+  if (color_roi_has_run) {
+    return;
+  }
+  if (color_ae_roi_left_ != -1 && color_ae_roi_top_ != -1 && color_ae_roi_right_ != -1 &&
+      color_ae_roi_bottom_ != -1 &&
+      device_->isPropertySupported(OB_STRUCT_COLOR_AE_ROI, OB_PERMISSION_READ_WRITE)) {
+    RCLCPP_INFO_STREAM(logger_, "Setting color AE ROI to "
+                                    << color_ae_roi_left_ << ", " << color_ae_roi_top_ << ", "
+                                    << color_ae_roi_right_ << ", " << color_ae_roi_bottom_);
+    auto config = OBRegionOfInterest();
+    config.x0_left = color_ae_roi_left_;
+    config.y0_top = color_ae_roi_top_;
+    config.x1_right = color_ae_roi_right_;
+    config.y1_bottom = color_ae_roi_bottom_;
+    device_->setStructuredData(OB_STRUCT_COLOR_AE_ROI,
+                                   reinterpret_cast<const uint8_t*>(&config), sizeof(config));
+  }
+  color_roi_has_run = true;
+}
+
 uint64_t OBCameraNode::getFrameTimestampUs(const std::shared_ptr<ob::Frame> &frame) {
   if (frame == nullptr) {
     RCLCPP_WARN(logger_, "getFrameTimestampUs: frame is nullptr, return 0");
@@ -2154,10 +2206,12 @@ void OBCameraNode::onNewFrameSetCallback(std::shared_ptr<ob::FrameSet> frame_set
     auto color_frame = frame_set->getFrame(OB_FRAME_COLOR);
     if (depth_frame) {
       setDisparitySearchOffset();
+      setDepthAutoExposureROI();
       depth_frame = processDepthFrameFilter(depth_frame);
       frame_set->pushFrame(depth_frame);
     }
     if (color_frame) {
+      setColorAutoExposureROI();
       color_frame = processColorFrameFilter(color_frame);
       frame_set->pushFrame(color_frame);
     }