From e09414dc09fca43341c3c317e8df3e22084dd9d0 Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Wed, 22 Nov 2023 06:04:50 +0530 Subject: [PATCH 1/2] 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..73b9cac11b 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 && *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) From 8384c891b22a74ccd8f0e1ea32a2aca11c3d3d21 Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Thu, 14 Dec 2023 21:44:48 +0530 Subject: [PATCH 2/2] Return failure in SetParam callback in case of error --- README.md | 2 +- realsense2_camera/src/dynamic_params.cpp | 12 +++++++----- realsense2_camera/src/sensor_params.cpp | 9 +-------- 3 files changed, 9 insertions(+), 14 deletions(-) diff --git a/README.md b/README.md index ee99a27473..a77621972e 100644 --- a/README.md +++ b/README.md @@ -505,7 +505,7 @@ The following post processing filters are available: - `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. + - 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/dynamic_params.cpp b/realsense2_camera/src/dynamic_params.cpp index acce9038ca..e51dfb1818 100644 --- a/realsense2_camera/src/dynamic_params.cpp +++ b/realsense2_camera/src/dynamic_params.cpp @@ -25,6 +25,8 @@ namespace realsense2_camera _params_backend.add_on_set_parameters_callback( [this](const std::vector & parameters) { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; for (const auto & parameter : parameters) { try @@ -43,15 +45,15 @@ namespace realsense2_camera } } } - catch(const std::out_of_range& e) - {} catch(const std::exception& e) { - std::cerr << e.what() << ":" << parameter.get_name() << '\n'; + result.successful = false; + result.reason = e.what(); + ROS_WARN_STREAM("Set parameter {" << parameter.get_name() + << "} failed: " << e.what()); } } - rcl_interfaces::msg::SetParametersResult result; - result.successful = true; + return result; }); monitor_update_functions(); // Start parameters update thread diff --git a/realsense2_camera/src/sensor_params.cpp b/realsense2_camera/src/sensor_params.cpp index ff0cf151e9..c347a1d979 100644 --- a/realsense2_camera/src/sensor_params.cpp +++ b/realsense2_camera/src/sensor_params.cpp @@ -70,14 +70,7 @@ std::map get_enum_method(rs2::options sensor, rs2_option optio template void param_set_option(rs2::options sensor, rs2_option option, const rclcpp::Parameter& parameter) { - try - { - sensor.set_option(option, parameter.get_value()); - } - catch(const std::exception& e) - { - std::cout << "Failed to set value: " << e.what() << std::endl; - } + sensor.set_option(option, parameter.get_value()); } void SensorParams::clearParameters()