Skip updating Exp 1,2 & Gain 1,2 when HDR is disabled

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.

realsense2_camera/include/dynamic_params.h

@@ -46,7 +46,10 @@ namespace realsense2_camera
 
             template <class T>
             void queueSetRosValue(const std::string& param_name, const T value);
-
+
+            template <class T>
+            T getParam(std::string param_name);
+
         private:
             void monitor_update_functions();
 

realsense2_camera/src/dynamic_params.cpp

@@ -267,6 +267,12 @@ namespace realsense2_camera
         _param_functions.erase(param_name);
     }
 
+    template <class T>
+    T Parameters::getParam(std::string param_name)
+    {
+        return _node.get_parameter(param_name).get_value<T>();
+    }
+
     template void Parameters::setParamT<bool>(std::string param_name, bool& param, std::function<void(const rclcpp::Parameter&)> func, rcl_interfaces::msg::ParameterDescriptor descriptor);
     template void Parameters::setParamT<int>(std::string param_name, int& param, std::function<void(const rclcpp::Parameter&)> func, rcl_interfaces::msg::ParameterDescriptor descriptor);
     template void Parameters::setParamT<double>(std::string param_name, double& param, std::function<void(const rclcpp::Parameter&)> func, rcl_interfaces::msg::ParameterDescriptor descriptor);
@@ -284,4 +290,6 @@ namespace realsense2_camera
     template void Parameters::queueSetRosValue<int>(const std::string& param_name, const int value);
 
     template int Parameters::readAndDeleteParam<int>(std::string param_name, const int& initial_value);
+
+    template bool Parameters::getParam<bool>(std::string param_name);
 }

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<bool>(param_name);
     bool is_hdr_enabled = static_cast<bool>(get_option(RS2_OPTION_HDR_ENABLED));
 
+    if (is_hdr_enabled && user_set_enable_ae_value)
+    {
+        bool is_ae_enabled = static_cast<bool>(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<std::function<void()> > funcs;
+                funcs.push_back([this](){set_sensor_parameter_to_ros<bool>(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<bool, std::function<void(bool*)>>(&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<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)));
-
-    // Read initialization parameters and set to sensor:
-    std::vector<rs2_option> 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<int>(get_option(RS2_OPTION_SEQUENCE_ID));   // To Set back to default.
+
+        // Read initialization parameters and set to sensor:
+        std::vector<rs2_option> 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)));