From 407da9af9daac17efaf6c65a8fa39581aa51f021 Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Wed, 22 Nov 2023 06:04:50 +0530 Subject: [PATCH] Disabling hdr while updating exposure & gain values --- README.md | 3 +++ realsense2_camera/src/ros_sensor.cpp | 19 ++++++++++++++++++- 2 files changed, 21 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 0311e9ed2b..6c0bbf9615 100644 --- a/README.md +++ b/README.md @@ -531,7 +531,10 @@ The following post processing filters are available: * The depth FOV and the texture FOV are not similar. By default, pointcloud is limited to the section of depth containing the texture. You can have a full depth to pointcloud, coloring the regions beyond the texture with zeros, by setting `pointcloud.allow_no_texture_points` to true. * pointcloud is of an unordered format by default. This can be changed by setting `pointcloud.ordered_pc` to true. - ```hdr_merge```: Allows depth image to be created by merging the information from 2 consecutive frames, taken with different exposure and gain values. + - `depth_module.hdr_enabled`: to enable/disable HDR - The way to set exposure and gain values for each sequence in runtime is by first selecting the sequence id, using the `depth_module.sequence_id` parameter and then modifying the `depth_module.gain`, and `depth_module.exposure`. + - From FW versions 5.14.x.x and above, if HDR is enabled, the preset configs (like exposure, gain, etc.,) cannot be updated. + - The user should disable the HDR first using `depth_module.hdr_enabled` parameter and then, update the required presets. - To view the effect on the infrared image for each sequence id use the `filter_by_sequence_id.sequence_id` parameter. - To initialize these parameters in start time use the following parameters: - `depth_module.exposure.1` diff --git a/realsense2_camera/src/ros_sensor.cpp b/realsense2_camera/src/ros_sensor.cpp index 635424d8c5..4a29f658d5 100644 --- a/realsense2_camera/src/ros_sensor.cpp +++ b/realsense2_camera/src/ros_sensor.cpp @@ -102,6 +102,24 @@ void RosSensor::UpdateSequenceIdCallback() if (!supports(RS2_OPTION_SEQUENCE_ID)) return; + bool is_hdr_enabled = static_cast(get_option(RS2_OPTION_HDR_ENABLED)); + + // Deleter to revert back the RS2_OPTION_HDR_ENABLED value at the end. + auto deleter_to_revert_hdr = std::unique_ptr>(&is_hdr_enabled, + [&](bool* enable_back_hdr) { + if (*enable_back_hdr) + { + set_option(RS2_OPTION_HDR_ENABLED, true); + } + }); + + // From FW version 5.14.x.x, if HDR is enabled, updating UVC controls like exposure, gain , etc are restricted. + // So, disable it before updating. + if (is_hdr_enabled) + { + set_option(RS2_OPTION_HDR_ENABLED, false); + } + int original_seq_id = static_cast(get_option(RS2_OPTION_SEQUENCE_ID)); // To Set back to default. std::string module_name = create_graph_resource_name(rs2_to_ros(get_info(RS2_CAMERA_INFO_NAME))); @@ -146,7 +164,6 @@ void RosSensor::UpdateSequenceIdCallback() ROS_WARN_STREAM("Setting alternative callback: Failed to set parameter:" << option_name << " : " << e.what()); return; } - } void RosSensor::set_sensor_parameter_to_ros(rs2_option option)