From 41200eaff56c7bcd3df298969ecc5808144b8c26 Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Fri, 16 Feb 2024 19:34:19 +0530 Subject: [PATCH] skip updating exp 1,2 & gain 1,2 params when HDR is disabled --- README.md | 26 +++++---- realsense2_camera/include/dynamic_params.h | 5 +- realsense2_camera/src/dynamic_params.cpp | 8 +++ realsense2_camera/src/ros_sensor.cpp | 68 +++++++++++++++------- 4 files changed, 74 insertions(+), 33 deletions(-) diff --git a/README.md b/README.md index 52af27c039..08fcfc99fe 100644 --- a/README.md +++ b/README.md @@ -527,17 +527,21 @@ The following post processing filters are available: ros2 param set /camera/camera pointcloud.enable 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. - - 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` - - `depth_module.gain.1` - - `depth_module.exposure.2` - - `depth_module.gain.2` - - For in-depth review of the subject please read the accompanying [white paper](https://dev.intelrealsense.com/docs/high-dynamic-range-with-stereoscopic-depth-cameras). + - `depth_module.hdr_enabled`: to enable/disable HDR. The way to set exposure and gain values for each sequence: + - during 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. + - Disable the HDR first using `depth_module.hdr_enabled` parameter and then, update the required presets. + - during Launch time of the node: + - is by setting below parameters + - `depth_module.exposure.1` + - `depth_module.gain.1` + - `depth_module.exposure.2` + - `depth_module.gain.2` + - Make sure to set `depth_module.hdr_enabled` to true, otherwise these parameters won't be considered. + - To view the effect on the infrared image for each sequence id use the `filter_by_sequence_id.sequence_id` parameter. + - For in-depth review of the subject please read the accompanying [white paper](https://dev.intelrealsense.com/docs/high-dynamic-range-with-stereoscopic-depth-cameras). + - **Note**: Auto exposure functionality is not supported when HDR is enabled. i.e., Auto exposure will be auto-disabled if HDR is enabled. - The following filters have detailed descriptions in : https://github.com/IntelRealSense/librealsense/blob/master/doc/post-processing-filters.md - ```disparity_filter``` - convert depth to disparity before applying other filters and back. diff --git a/realsense2_camera/include/dynamic_params.h b/realsense2_camera/include/dynamic_params.h index b87caea5c0..d438ad937b 100644 --- a/realsense2_camera/include/dynamic_params.h +++ b/realsense2_camera/include/dynamic_params.h @@ -46,7 +46,10 @@ namespace realsense2_camera template void queueSetRosValue(const std::string& param_name, const T value); - + + template + T getParam(std::string param_name); + private: void monitor_update_functions(); diff --git a/realsense2_camera/src/dynamic_params.cpp b/realsense2_camera/src/dynamic_params.cpp index 45db52d90a..52a521240d 100644 --- a/realsense2_camera/src/dynamic_params.cpp +++ b/realsense2_camera/src/dynamic_params.cpp @@ -267,6 +267,12 @@ namespace realsense2_camera _param_functions.erase(param_name); } + template + T Parameters::getParam(std::string param_name) + { + return _node.get_parameter(param_name).get_value(); + } + template void Parameters::setParamT(std::string param_name, bool& param, std::function func, rcl_interfaces::msg::ParameterDescriptor descriptor); template void Parameters::setParamT(std::string param_name, int& param, std::function func, rcl_interfaces::msg::ParameterDescriptor descriptor); template void Parameters::setParamT(std::string param_name, double& param, std::function func, rcl_interfaces::msg::ParameterDescriptor descriptor); @@ -284,4 +290,6 @@ namespace realsense2_camera template void Parameters::queueSetRosValue(const std::string& param_name, const int value); template int Parameters::readAndDeleteParam(std::string param_name, const int& initial_value); + + template bool Parameters::getParam(std::string param_name); } diff --git a/realsense2_camera/src/ros_sensor.cpp b/realsense2_camera/src/ros_sensor.cpp index dfd5daed99..35a4ef8ca3 100644 --- a/realsense2_camera/src/ros_sensor.cpp +++ b/realsense2_camera/src/ros_sensor.cpp @@ -110,8 +110,35 @@ void RosSensor::UpdateSequenceIdCallback() if (!supports(RS2_OPTION_SEQUENCE_ID)) return; + std::string module_name = create_graph_resource_name(rs2_to_ros(get_info(RS2_CAMERA_INFO_NAME))); + std::string param_name = module_name + "." + create_graph_resource_name(rs2_option_to_string(RS2_OPTION_ENABLE_AUTO_EXPOSURE)); + + bool user_set_enable_ae_value = _params.getParameters()->getParam(param_name); bool is_hdr_enabled = static_cast(get_option(RS2_OPTION_HDR_ENABLED)); + if (is_hdr_enabled && user_set_enable_ae_value) + { + bool is_ae_enabled = static_cast(get_option(RS2_OPTION_ENABLE_AUTO_EXPOSURE)); + + // If AE got auto-disabled, update the Enable_Auto_Exposure ROS paramerter as well accordingly. + if (!is_ae_enabled) + { + ROS_WARN_STREAM("Auto Exposure functionality is not supported when HDR is enabled. " << + "So, disabling the '" << param_name << "' param."); + + try + { + std::vector > funcs; + funcs.push_back([this](){set_sensor_parameter_to_ros(RS2_OPTION_ENABLE_AUTO_EXPOSURE);}); + _params.getParameters()->pushUpdateFunctions(funcs); + } + catch(const std::exception& e) + { + ROS_WARN_STREAM("Failed to set parameter:" << param_name << " : " << e.what()); + } + } + } + // 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) { @@ -121,36 +148,35 @@ void RosSensor::UpdateSequenceIdCallback() } }); - // 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) { + // 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. It will be reverted back by the deleter 'deleter_to_revert_hdr'. 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))); - - // Read initialization parameters and set to sensor: - std::vector options{RS2_OPTION_EXPOSURE, RS2_OPTION_GAIN}; - unsigned int seq_size = get_option(RS2_OPTION_SEQUENCE_SIZE); - for (unsigned int seq_id = 1; seq_id <= seq_size; seq_id++ ) - { - set_option(RS2_OPTION_SEQUENCE_ID, seq_id); - for (rs2_option& option : options) + int original_seq_id = static_cast(get_option(RS2_OPTION_SEQUENCE_ID)); // To Set back to default. + + // Read initialization parameters and set to sensor: + std::vector options{RS2_OPTION_EXPOSURE, RS2_OPTION_GAIN}; + unsigned int seq_size = get_option(RS2_OPTION_SEQUENCE_SIZE); + for (unsigned int seq_id = 1; seq_id <= seq_size; seq_id++ ) { - std::stringstream param_name_str; - param_name_str << module_name << "." << create_graph_resource_name(rs2_option_to_string(option)) << "." << seq_id; - int option_value = get_option(option); - int user_set_option_value = _params.getParameters()->readAndDeleteParam(param_name_str.str(), option_value); - if (option_value != user_set_option_value) + set_option(RS2_OPTION_SEQUENCE_ID, seq_id); + for (rs2_option& option : options) { - ROS_INFO_STREAM("Set " << rs2_option_to_string(option) << "." << seq_id << " to " << user_set_option_value); - set_option(option, user_set_option_value); + std::stringstream param_name_str; + param_name_str << module_name << "." << create_graph_resource_name(rs2_option_to_string(option)) << "." << seq_id; + int option_value = get_option(option); + int user_set_option_value = _params.getParameters()->readAndDeleteParam(param_name_str.str(), option_value); + if (option_value != user_set_option_value) + { + ROS_INFO_STREAM("Set " << rs2_option_to_string(option) << "." << seq_id << " to " << user_set_option_value); + set_option(option, user_set_option_value); + } } } + set_option(RS2_OPTION_SEQUENCE_ID, original_seq_id); // Set back to default. } - set_option(RS2_OPTION_SEQUENCE_ID, original_seq_id); // Set back to default. // Set callback to update ros parameters to gain and exposure matching the selected sequence_id: const std::string option_name(module_name + "." + create_graph_resource_name(rs2_option_to_string(RS2_OPTION_SEQUENCE_ID)));