PR #2934 from Arun-Prasad-V: Disabling hdr while updating exposure & …

…gain values
IntelRealSense · Dec 15, 2023 · 734f92a · 734f92a
2 parents 91c0f35 + 8384c89
commit 734f92a
Show file tree

Hide file tree

Showing 4 changed files with 29 additions and 14 deletions.
diff --git a/README.md b/README.md
@@ -502,7 +502,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.
+      - 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
@@ -25,6 +25,8 @@ namespace realsense2_camera
         _params_backend.add_on_set_parameters_callback(
             [this](const std::vector<rclcpp::Parameter> & 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/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<bool>(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<bool, std::function<void(bool*)>>(&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<int>(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)

diff --git a/realsense2_camera/src/sensor_params.cpp b/realsense2_camera/src/sensor_params.cpp
@@ -70,14 +70,7 @@ std::map<std::string, int> get_enum_method(rs2::options sensor, rs2_option optio
 template<class T>
 void param_set_option(rs2::options sensor, rs2_option option, const rclcpp::Parameter& parameter)
 { 
-    try
-    {
-        sensor.set_option(option, parameter.get_value<T>());
-    }
-    catch(const std::exception& e)
-    {
-        std::cout << "Failed to set value: " << e.what() << std::endl;
-    }
+    sensor.set_option(option, parameter.get_value<T>());
 }
 
 void SensorParams::clearParameters()