diff --git a/realsense2_camera/test/d455/test_d455_basic_tests.py b/realsense2_camera/test/d455/test_d455_basic_tests.py new file mode 100644 index 0000000000..b2b8d0cf31 --- /dev/null +++ b/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() + + diff --git a/realsense2_camera/test/utils/pytest_rs_utils.py b/realsense2_camera/test/utils/pytest_rs_utils.py index 0fd4563381..1983d3f647 100644 --- a/realsense2_camera/test/utils/pytest_rs_utils.py +++ b/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)