PR #2940 from Arun-Prasad-V: Fixing the data_type of ROS Params expos…

…ure & gain

realsense2_camera/include/ros_sensor.h

@@ -105,6 +105,7 @@ namespace realsense2_camera
             void set_sensor_auto_exposure_roi();
             void registerAutoExposureROIOptions();
             void UpdateSequenceIdCallback();
+            template<class T> 
             void set_sensor_parameter_to_ros(rs2_option option);
 
         private:

realsense2_camera/src/ros_sensor.cpp

@@ -162,8 +162,8 @@ void RosSensor::UpdateSequenceIdCallback()
             {
                 set_option(RS2_OPTION_SEQUENCE_ID, parameter.get_value<int>());
                 std::vector<std::function<void()> > funcs;
-                funcs.push_back([this](){set_sensor_parameter_to_ros(RS2_OPTION_GAIN);});
-                funcs.push_back([this](){set_sensor_parameter_to_ros(RS2_OPTION_EXPOSURE);});
+                funcs.push_back([this](){set_sensor_parameter_to_ros<int>(RS2_OPTION_GAIN);});
+                funcs.push_back([this](){set_sensor_parameter_to_ros<int>(RS2_OPTION_EXPOSURE);});
                 _params.getParameters()->pushUpdateFunctions(funcs);
             });
     }
@@ -174,11 +174,12 @@ void RosSensor::UpdateSequenceIdCallback()
     }
 }
 
+template<class T> 
 void RosSensor::set_sensor_parameter_to_ros(rs2_option option)
 {
     std::string module_name = create_graph_resource_name(rs2_to_ros(get_info(RS2_CAMERA_INFO_NAME)));
     const std::string option_name(module_name + "." + create_graph_resource_name(rs2_option_to_string(option)));
-    float value = get_option(option);
+    auto value = static_cast<T>(get_option(option));
     _params.getParameters()->setRosParamValue(option_name, &value);
 }
 

realsense2_camera/test/live_camera/test_d455_basic_tests.py

@@ -97,3 +97,56 @@ def test_D455_Change_Resolution(self,launch_descr_with_parameters):
             self.shutdown()
 
 
+test_params_seq_id_update = {
+    'camera_name': 'D455',
+    'device_type': 'D455',
+    'exposure_1' : 2000,
+    'gain_1'     : 20,
+    'exposure_2' : 3000,
+    'gain_2'     : 30,
+    }
+'''
+This test sets the sequence ID param and validates the corresponding Exposure & Gain values.
+'''
+@pytest.mark.d455
+@pytest.mark.parametrize("launch_descr_with_parameters", [test_params_seq_id_update],indirect=True)
+@pytest.mark.launch(fixture=launch_descr_with_parameters)
+class Test_D455_Seq_ID_Update(pytest_rs_utils.RsTestBaseClass):
+    def test_D455_Seq_ID_update(self,launch_descr_with_parameters):
+        params = launch_descr_with_parameters[1]
+        if pytest_live_camera_utils.check_if_camera_connected(params['device_type']) == False:
+            print("Device not found? : " + params['device_type'])
+            return
+
+        try:
+            '''
+            initialize, run and check the data
+            '''
+            print("Starting camera test...")
+            self.init_test("RsTest"+params['camera_name'])
+            self.wait_for_node(params['camera_name'])
+            self.create_param_ifs(get_node_heirarchy(params))
+
+            assert self.set_bool_param('depth_module.hdr_enabled', False)
+
+            assert self.set_integer_param('depth_module.sequence_id', 1)
+            assert self.set_integer_param('depth_module.exposure', params['exposure_1'])
+            assert self.set_integer_param('depth_module.gain', params['gain_1'])
+
+            assert self.set_integer_param('depth_module.sequence_id', 2)
+            assert self.set_integer_param('depth_module.exposure', params['exposure_2'])
+            assert self.set_integer_param('depth_module.gain', params['gain_2'])
+
+            assert self.set_integer_param('depth_module.sequence_id', 1)
+            assert self.get_integer_param('depth_module.exposure') == params['exposure_1']
+            assert self.get_integer_param('depth_module.gain') == params['gain_1']
+
+            assert self.set_integer_param('depth_module.sequence_id', 2)
+            assert self.get_integer_param('depth_module.exposure') == params['exposure_2']
+            assert self.get_integer_param('depth_module.gain') == params['gain_2']
+
+        finally:
+            #this step is important because the test will fail next time
+            pytest_rs_utils.kill_realsense2_camera_node()
+            self.shutdown()
+

realsense2_camera/test/utils/pytest_rs_utils.py

@@ -808,6 +808,20 @@ def send_param(self, req):
                     pass
                 return False
 
+    def get_param(self, req):
+        future = self.get_param_if.call_async(req)
+        while rclpy.ok():
+            rclpy.spin_once(self.node)
+            if future.done():
+                try:
+                    response = future.result()
+                    return response.values[0]
+                except Exception as e:
+                    print("exception raised:")
+                    print(e)
+                    pass
+                return None
+
     def set_string_param(self, param_name, param_value):
         req = SetParameters.Request()
         new_param_value = ParameterValue(type=ParameterType.PARAMETER_STRING, string_value=param_value)
@@ -826,6 +840,15 @@ def set_integer_param(self, param_name, param_value):
         req.parameters = [Parameter(name=param_name, value=new_param_value)]
         return self.send_param(req)
 
+    def get_integer_param(self, param_name):
+        req = GetParameters.Request()
+        req.names = [param_name]
+        value = self.get_param(req)
+        if (value == None) or (value.type == ParameterType.PARAMETER_NOT_SET):
+            return None
+        else:
+            return value.integer_value
+
     def spin_for_data(self,themes, timeout=5.0):
         '''
         timeout value varies depending upon the system, it needs to be more if