Skip to content

Commit

Permalink
chore: Update color and IR camera settings in OBCameraNode
Browse files Browse the repository at this point in the history
This commit updates the OBCameraNode class to include new properties for color and IR camera settings. Specifically, it adds support for setting the color brightness, color AE max exposure, IR brightness, and IR AE max exposure. These settings can now be configured through the launch file and applied to the camera device.
  • Loading branch information
jian-dong committed Sep 18, 2024
1 parent e285382 commit 4007e9f
Show file tree
Hide file tree
Showing 4 changed files with 37 additions and 1 deletion.
5 changes: 5 additions & 0 deletions README.MD
Original file line number Diff line number Diff line change
Expand Up @@ -281,6 +281,7 @@ The following are the launch parameters available:
- `enable_laser`: Enables the laser. The default value is `true`.
- `laser_on_off_mode`: Laser on/off alternate mode, 0: off, 1: on-off alternate, 2: off-on alternate. The default value
is `0`.
- `laser_energy_level`: Laser energy level.
- `device_preset`: The default value is `Default`. Only the G330 series is supported. For more information, refer to
the [G330 documentation](https://www.orbbec.com/docs/g330-use-depth-presets/). Please refer to the table below to set
the `device_preset` value based on your use case. The value should be one of the preset names
Expand Down Expand Up @@ -321,6 +322,10 @@ the default parameters from the launch file will be used. If you want to use a c
the firmware, and if hardware logging is desired, it should also be set to `true`.
- `log_level` : SDK log level, the default value is `info`, the optional values are `debug`, `info`, `warn`, `error`, `fatal`.
- `enable_color_undistortion`: Enables color undistortion, the default value is `false`. Note that our color cameras exhibit minimal distortion, and typically, undistortion is not necessary.
- `color_brightness`: Color brightness.
- `ir_brightness`: IR brightness.
- `color_ae_max_exposure`: Color auto exposure maximum exposure.
- `ir_ae_max_exposure`: IR auto exposure maximum exposure.

**IMPORTANT**: *Please carefully read the instructions regarding software filtering settings
at [this link](https://www.orbbec.com/docs/g330-use-depth-post-processing-blocks/). If you are uncertain, do not modify
Expand Down
6 changes: 5 additions & 1 deletion orbbec_camera/include/orbbec_camera/ob_camera_node.h
Original file line number Diff line number Diff line change
Expand Up @@ -115,7 +115,7 @@ const stream_index_pair INFRA2{OB_STREAM_IR_RIGHT, 0};
const stream_index_pair GYRO{OB_STREAM_GYRO, 0};
const stream_index_pair ACCEL{OB_STREAM_ACCEL, 0};

const std::vector<stream_index_pair> IMAGE_STREAMS = {COLOR,DEPTH, INFRA0, INFRA1, INFRA2};
const std::vector<stream_index_pair> IMAGE_STREAMS = {COLOR, DEPTH, INFRA0, INFRA1, INFRA2};

const std::vector<stream_index_pair> HID_STREAMS = {GYRO, ACCEL};

Expand Down Expand Up @@ -463,8 +463,12 @@ class OBCameraNode {
int color_exposure_ = -1;
int color_gain_ = -1;
int color_white_balance_ = -1;
int color_ae_max_exposure_ = -1;
int color_brightness_ = -1;
int ir_exposure_ = -1;
int ir_gain_ = -1;
int ir_ae_max_exposure_ = -1;
int ir_brightness_ = -1;
int soft_filter_max_diff_ = -1;
int soft_filter_speckle_size_ = -1;
bool enable_frame_sync_ = false;
Expand Down
4 changes: 4 additions & 0 deletions orbbec_camera/launch/gemini_330_series.launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -73,6 +73,8 @@ def generate_launch_description():
DeclareLaunchArgument('color_gain', default_value='-1'),
DeclareLaunchArgument('enable_color_auto_white_balance', default_value='true'),
DeclareLaunchArgument('color_white_balance', default_value='-1'),
DeclareLaunchArgument('color_ae_max_exposure', default_value='-1'),
DeclareLaunchArgument('color_brightness', default_value='-1'),
DeclareLaunchArgument('depth_width', default_value='0'),
DeclareLaunchArgument('depth_height', default_value='0'),
DeclareLaunchArgument('depth_fps', default_value='0'),
Expand All @@ -97,6 +99,8 @@ def generate_launch_description():
DeclareLaunchArgument('enable_ir_auto_exposure', default_value='true'),
DeclareLaunchArgument('ir_exposure', default_value='-1'),
DeclareLaunchArgument('ir_gain', default_value='-1'),
DeclareLaunchArgument('ir_ae_max_exposure', default_value='-1'),
DeclareLaunchArgument('ir_brightness', default_value='-1'),
DeclareLaunchArgument('enable_sync_output_accel_gyro', default_value='false'),
DeclareLaunchArgument('enable_accel', default_value='false'),
DeclareLaunchArgument('accel_rate', default_value='200hz'),
Expand Down
23 changes: 23 additions & 0 deletions orbbec_camera/src/ob_camera_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -380,6 +380,25 @@ void OBCameraNode::setupDevices() {
}
}

if (device_->isPropertySupported(OB_PROP_COLOR_AE_MAX_EXPOSURE_INT, OB_PERMISSION_WRITE)) {
RCLCPP_INFO_STREAM(logger_, "Setting color AE max exposure to " << color_ae_max_exposure_);
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_AE_MAX_EXPOSURE_INT, color_ae_max_exposure_);
}
if (device_->isPropertySupported(OB_PROP_COLOR_BRIGHTNESS_INT, OB_PERMISSION_WRITE)) {
RCLCPP_INFO_STREAM(logger_, "Setting color brightness to " << color_brightness_);
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_COLOR_BRIGHTNESS_INT, color_brightness_);
}
// ir ae max
if (device_->isPropertySupported(OB_PROP_IR_AE_MAX_EXPOSURE_INT, OB_PERMISSION_WRITE)) {
RCLCPP_INFO_STREAM(logger_, "Setting IR AE max exposure to " << ir_ae_max_exposure_);
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_IR_AE_MAX_EXPOSURE_INT, ir_ae_max_exposure_);
}
// ir brightness
if (device_->isPropertySupported(OB_PROP_IR_BRIGHTNESS_INT, OB_PERMISSION_WRITE)) {
RCLCPP_INFO_STREAM(logger_, "Setting IR brightness to " << ir_brightness_);
TRY_TO_SET_PROPERTY(setIntProperty, OB_PROP_IR_BRIGHTNESS_INT, ir_brightness_);
}
// ir auto exposure
if (device_->isPropertySupported(OB_PROP_IR_AUTO_EXPOSURE_BOOL, OB_PERMISSION_WRITE)) {
RCLCPP_INFO_STREAM(logger_,
"Setting IR auto exposure to " << (enable_ir_auto_exposure_ ? "ON" : "OFF"));
Expand Down Expand Up @@ -1011,9 +1030,13 @@ void OBCameraNode::getParameters() {
setAndGetNodeParameter<int>(color_exposure_, "color_exposure", -1);
setAndGetNodeParameter<int>(color_gain_, "color_gain", -1);
setAndGetNodeParameter<int>(color_white_balance_, "color_white_balance", -1);
setAndGetNodeParameter<int>(color_ae_max_exposure_, "color_ae_max_exposure", -1);
setAndGetNodeParameter<int>(color_brightness_, "color_brightness", -1);
setAndGetNodeParameter(enable_ir_auto_exposure_, "enable_ir_auto_exposure", true);
setAndGetNodeParameter<int>(ir_exposure_, "ir_exposure", -1);
setAndGetNodeParameter<int>(ir_gain_, "ir_gain", -1);
setAndGetNodeParameter<int>(ir_ae_max_exposure_, "ir_ae_max_exposure", -1);
setAndGetNodeParameter<int>(ir_brightness_, "ir_brightness", -1);
setAndGetNodeParameter(enable_ir_long_exposure_, "enable_ir_long_exposure", true);
setAndGetNodeParameter<std::string>(depth_work_mode_, "depth_work_mode", "");
setAndGetNodeParameter<std::string>(sync_mode_str_, "sync_mode", "");
Expand Down

0 comments on commit 4007e9f

Please sign in to comment.