From c5e5d3b47687099976506d4658cdab6daf843f21 Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Thu, 30 Nov 2023 02:43:18 +0530 Subject: [PATCH 1/2] Fixing the data_type of ROS Params exposure & gain --- realsense2_camera/include/ros_sensor.h | 1 + realsense2_camera/src/ros_sensor.cpp | 7 ++++--- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/realsense2_camera/include/ros_sensor.h b/realsense2_camera/include/ros_sensor.h index c4430bfd4f..57d224300e 100644 --- a/realsense2_camera/include/ros_sensor.h +++ b/realsense2_camera/include/ros_sensor.h @@ -105,6 +105,7 @@ namespace realsense2_camera void set_sensor_auto_exposure_roi(); void registerAutoExposureROIOptions(); void UpdateSequenceIdCallback(); + template void set_sensor_parameter_to_ros(rs2_option option); private: diff --git a/realsense2_camera/src/ros_sensor.cpp b/realsense2_camera/src/ros_sensor.cpp index 73b9cac11b..2eae865995 100644 --- a/realsense2_camera/src/ros_sensor.cpp +++ b/realsense2_camera/src/ros_sensor.cpp @@ -154,8 +154,8 @@ void RosSensor::UpdateSequenceIdCallback() { set_option(RS2_OPTION_SEQUENCE_ID, parameter.get_value()); std::vector > 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(RS2_OPTION_GAIN);}); + funcs.push_back([this](){set_sensor_parameter_to_ros(RS2_OPTION_EXPOSURE);}); _params.getParameters()->pushUpdateFunctions(funcs); }); } @@ -166,11 +166,12 @@ void RosSensor::UpdateSequenceIdCallback() } } +template 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(get_option(option)); _params.getParameters()->setRosParamValue(option_name, &value); } From 25ceeaec89f9528ba7c163845c061ba16a456348 Mon Sep 17 00:00:00 2001 From: Arun-Prasad-V Date: Sat, 16 Dec 2023 01:21:05 +0530 Subject: [PATCH 2/2] Added live camera test for dynamic seq_id update --- .../test/live_camera/test_d455_basic_tests.py | 53 +++++++++++++++++++ .../test/utils/pytest_rs_utils.py | 23 ++++++++ 2 files changed, 76 insertions(+) diff --git a/realsense2_camera/test/live_camera/test_d455_basic_tests.py b/realsense2_camera/test/live_camera/test_d455_basic_tests.py index 7848ff4986..fb2f58cab6 100644 --- a/realsense2_camera/test/live_camera/test_d455_basic_tests.py +++ b/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() + diff --git a/realsense2_camera/test/utils/pytest_rs_utils.py b/realsense2_camera/test/utils/pytest_rs_utils.py index db9c09c3c0..0a0069115f 100644 --- a/realsense2_camera/test/utils/pytest_rs_utils.py +++ b/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