add template for d455 live camera with dynamic parameter change

realsense2_camera/test/d455/test_d455_basic_tests.py

@@ -0,0 +1,88 @@
+# Copyright 2023 Intel Corporation. All Rights Reserved.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+
+import os
+import sys
+import itertools
+
+
+import pytest
+import rclpy
+
+from sensor_msgs.msg import Image as msg_Image
+from sensor_msgs.msg import Imu as msg_Imu
+from sensor_msgs.msg import PointCloud2 as msg_PointCloud2
+
+import numpy as np
+
+sys.path.append(os.path.abspath(os.path.dirname(__file__)+"/../utils"))
+import pytest_rs_utils
+from pytest_rs_utils import launch_descr_with_parameters
+from pytest_rs_utils import delayed_launch_descr_with_parameters
+from pytest_rs_utils import get_rosbag_file_path
+
+from rclpy.parameter import Parameter
+from rcl_interfaces.msg import ParameterValue
+from rcl_interfaces.srv import SetParameters, GetParameters, ListParameters
+
+test_params_depth_avg_1 = {
+    'camera_name': 'D455',
+    'device_type': 'D455',
+    }
+'''
+This test was implemented as a template to set the parameters and run the test.
+This directory is not added to the CMakeLists.txt so as to avoid the colcon failure in the
+machines that don't have the D455 connected.
+1. Only a subset of parameter types are implemented in py_rs_utils, it has to be extended for others
+2. After setting the param, rclpy.spin_once may be needed.Test passes even without this though.
+'''
+@pytest.mark.parametrize("launch_descr_with_parameters", [test_params_depth_avg_1],indirect=True)
+@pytest.mark.launch(fixture=launch_descr_with_parameters)
+class TestD455_Change_Resolution(pytest_rs_utils.RsTestBaseClass):
+    def test_D455_Change_Resolution(self,launch_descr_with_parameters):
+        params = launch_descr_with_parameters[1]
+        themes = [
+        {'topic':'/'+params['camera_name']+'/color/image_raw',
+         'msg_type':msg_Image,
+         'expected_data_chunks':1,
+         'width':640,
+         'height':480,
+         #'data':data
+        }
+        ]
+        try:
+            ''' 
+            initialize, run and check the data 
+            '''
+            self.init_test("RsTest"+params['camera_name'])
+            self.create_param_ifs(params['camera_name'] + '/' + params['camera_name'])
+            self.set_string_param('rgb_camera.profile', '640x480x30')
+            self.set_bool_param('enable_color', True)
+            ret = self.run_test(themes)
+            assert ret[0], ret[1]
+            assert self.process_data(themes)
+            self.set_string_param('rgb_camera.profile', '1280x720x15')
+            self.set_bool_param('enable_color', True)
+            themes[0]['width'] = 1280
+            themes[0]['height'] = 720
+
+            ret = self.run_test(themes)
+            assert self.process_data(themes)
+        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

@@ -33,6 +33,14 @@
 import struct
 import requests
 
+#from rclpy.parameter import Parameter
+from rcl_interfaces.msg import Parameter
+from rcl_interfaces.msg import ParameterValue
+from rcl_interfaces.srv import SetParameters, GetParameters, ListParameters
+from rcl_interfaces.msg import ParameterType
+from rcl_interfaces.msg import ParameterValue
+
+
 
 from sensor_msgs.msg import Image as msg_Image
 from sensor_msgs.msg import Imu as msg_Imu
@@ -708,6 +716,26 @@ def create_subscription(self, msg_type, topic, data_type, store_raw_data=False):
             self.node.create_subscription(msg_type, topic, data_type, store_raw_data)
             self.subscribed_topics.append(topic)
 
+    def create_param_ifs(self, camera_name):
+
+        self.set_param_if = self.node.create_client(SetParameters, '/' + camera_name + '/set_parameters')
+        self.get_param_if = self.node.create_client(GetParameters, '/' + camera_name + '/get_parameters')
+        while not self.get_param_if.wait_for_service(timeout_sec=1.0):
+            print('service not available, waiting again...') 
+        while not self.set_param_if.wait_for_service(timeout_sec=1.0):
+            print('service not available, waiting again...') 
+    def set_string_param(self, param_name, param_value):
+        req = SetParameters.Request()
+        new_param_value = ParameterValue(type=ParameterType.PARAMETER_STRING, string_value=param_value)
+        req.parameters = [Parameter(name=param_name, value=new_param_value)]
+        future = self.set_param_if.call_async(req)
+
+    def set_bool_param(self, param_name, param_value):
+        req = SetParameters.Request()
+        new_param_value = ParameterValue(type=ParameterType.PARAMETER_BOOL, bool_value=param_value)
+        req.parameters = [Parameter(name=param_name, value=new_param_value)]
+        future = self.set_param_if.call_async(req)
+
     def spin_for_data(self,themes):
         start = time.time()
         '''
@@ -771,9 +799,9 @@ def process_data(self, themes):
         for theme in themes:
             data = self.node.pop_first_chunk(theme['topic'])
             if 'width' in theme:
-                assert theme['width'] == data['shape'][0][1]  # (get from numpy image the width)
+                assert theme['width'] == data['shape'][0][1], "Width not matched. Expected:" +  str(theme['width']) + " & got: " + str(data['shape'][0][1]) # (get from numpy image the width)
             if 'height' in theme:
-                assert theme['height'] == data['shape'][0][0]  # (get from numpy image the height)
+                assert theme['height'] == data['shape'][0][0], "Height not matched. Expected:" +  str(theme['height']) + " & got: " + str(data['shape'][0][0])  # (get from numpy image the height)
             if 'data' not in theme:
                 print('No data to compare for ' + theme['topic'])
                 #print(data)