From 66ba4ee573eeef1b1fbd880a8b94c1650df8f55c Mon Sep 17 00:00:00 2001 From: PrasRsRos Date: Wed, 30 Aug 2023 20:42:53 +0530 Subject: [PATCH 01/18] Added live camera tests --- realsense2_camera/CMakeLists.txt | 35 +++ realsense2_camera/package.xml | 3 +- .../test_camera_all_profile_tests.py | 264 +++++++++++++++++ .../live_camera/test_camera_failing_tests.py | 268 ++++++++++++++++++ .../test/live_camera/test_camera_fps_tests.py | 102 +++++++ .../test/live_camera/test_camera_imu_tests.py | 126 ++++++++ .../test/live_camera/test_d415_basic_tests.py | 150 ++++++++++ .../test/live_camera/test_d455_basic_tests.py | 93 ++++++ .../test/utils/pytest_live_camera_utils.py | 190 +++++++++++++ .../test/utils/pytest_rs_utils.py | 163 +++++++++-- 10 files changed, 1371 insertions(+), 23 deletions(-) create mode 100644 realsense2_camera/test/live_camera/test_camera_all_profile_tests.py create mode 100644 realsense2_camera/test/live_camera/test_camera_failing_tests.py create mode 100644 realsense2_camera/test/live_camera/test_camera_fps_tests.py create mode 100644 realsense2_camera/test/live_camera/test_camera_imu_tests.py create mode 100644 realsense2_camera/test/live_camera/test_d415_basic_tests.py create mode 100644 realsense2_camera/test/live_camera/test_d455_basic_tests.py create mode 100644 realsense2_camera/test/utils/pytest_live_camera_utils.py diff --git a/realsense2_camera/CMakeLists.txt b/realsense2_camera/CMakeLists.txt index 4eeda43bdf..bb2d19a47d 100644 --- a/realsense2_camera/CMakeLists.txt +++ b/realsense2_camera/CMakeLists.txt @@ -314,6 +314,41 @@ if(BUILD_TESTING) ) endforeach() endforeach() + + unset(_pytest_folders) + + set(rs_query_cmd "rs-enumerate-devices -s") + execute_process(COMMAND bash -c ${rs_query_cmd} + WORKING_DIRECTORY ${PROJECT_SOURCE_DIR} + RESULT_VARIABLE rs_result + OUTPUT_VARIABLE RS_DEVICE_INFO) + message(STATUS "rs_device_info:") + message(STATUS "${RS_DEVICE_INFO}") + if(RS_DEVICE_INFO MATCHES "D455") + message(STATUS "D455 device found") + set(_pytest_live_folders + test/live_camera + ) + elseif(RS_DEVICE_INFO MATCHES "D415") + message(STATUS "D415 device found") + set(_pytest_live_folders + test/live_camera + ) + endif() + + foreach(test_folder ${_pytest_live_folders}) + file(GLOB files "${test_folder}/test_*.py") + foreach(file ${files}) + + get_filename_component(_test_name ${file} NAME_WE) + ament_add_pytest_test(${_test_name} ${file} + APPEND_ENV PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR}:${CMAKE_SOURCE_DIR}/test/utils:${CMAKE_SOURCE_DIR}/launch:${CMAKE_SOURCE_DIR}/scripts + TIMEOUT 500 + WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} + ) + endforeach() + endforeach() + endif() # Ament exports diff --git a/realsense2_camera/package.xml b/realsense2_camera/package.xml index faedde2dd2..3afb91e596 100644 --- a/realsense2_camera/package.xml +++ b/realsense2_camera/package.xml @@ -38,7 +38,8 @@ sensor_msgs_py python3-requests tf2_ros_py - + ros2topic + launch_ros ros_environment diff --git a/realsense2_camera/test/live_camera/test_camera_all_profile_tests.py b/realsense2_camera/test/live_camera/test_camera_all_profile_tests.py new file mode 100644 index 0000000000..274ee67625 --- /dev/null +++ b/realsense2_camera/test/live_camera/test_camera_all_profile_tests.py @@ -0,0 +1,264 @@ +# 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 pytest_rs_utils import get_node_heirarchy +import pytest_live_camera_utils +from rclpy.parameter import Parameter +from rcl_interfaces.msg import ParameterValue +from rcl_interfaces.srv import SetParameters, GetParameters, ListParameters +from pytest_live_camera_utils import debug_print +def check_if_skip_test(profile, format): + ''' + if profile == 'Color': + if "BGRA8" == format: + return True + if "RGBA8" == format: + return True + if "Y8" == format: + return True + elif profile == 'Depth': + if "Z16" == format: + return True + el + if profile == 'Infrared': + if "Y8" == format: + return True + if "Y16" == format: + return True + if "BGRA8" == format: + return True + if "RGBA8" == format: + return True + if "Y10BPACK" == format: + return True + if "UYVY" == format: + return True + if "BGR8" == format: + return True + if "RGB8" == format: + return True + if "RAW10" == format: + return True + elif profile == 'Infrared1': + if "Y8" ==format: + return True + if "Y16" ==format: + return True + if "Y10BPACK" == format: + return True + if "UYVY" ==format: + return True + if "BGR8" ==format: + return True + if "RGB8" ==format: + return True + if "RAW10" ==format: + return True + if profile == 'Infrared2': + if "Y8" == format: + return True + if "Y16" == format: + return True + if "Y10BPACK" == format: + return True + if "UYVY" == format: + return True + if "BGR8" == format: + return True + if "RGB8" == format: + return True + if "RAW10" == format: + return True + ''' + if profile == 'Infrared': + if "Y8" == format: + return True + if "Y16" == format: + return True + if profile == 'Infrared1': + if "Y8" ==format: + return True + if "Y16" ==format: + return True + if profile == 'Infrared2': + if "Y8" == format: + return True + if "Y16" == format: + return True + return False + + +test_params_all_profiles_d455 = { + 'camera_name': 'D455', + 'device_type': 'D455', + } +test_params_all_profiles_d415 = { + 'camera_name': 'D415', + 'device_type': 'D415', + } +test_params_all_profiles_d435 = { + 'camera_name': 'D435', + 'device_type': 'D435', + } + + +''' +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", [ + pytest.param(test_params_all_profiles_d455, marks=pytest.mark.d455), + pytest.param(test_params_all_profiles_d415, marks=pytest.mark.d415), + pytest.param(test_params_all_profiles_d435, marks=pytest.mark.d435),] + ,indirect=True) +@pytest.mark.launch(fixture=launch_descr_with_parameters) +class TestLiveCamera_Change_Resolution(pytest_rs_utils.RsTestBaseClass): + def test_LiveCamera_Change_Resolution(self,launch_descr_with_parameters): + skipped_tests = [] + failed_tests = [] + num_passed = 0 + num_failed = 0 + params = launch_descr_with_parameters[1] + themes = [{'topic':get_node_heirarchy(params)+'/color/image_raw', 'msg_type':msg_Image,'expected_data_chunks':1}] + config = pytest_live_camera_utils.get_profile_config(get_node_heirarchy(params)) + try: + ''' + initialize, run and check the data + ''' + self.init_test("RsTest"+params['camera_name']) + self.spin_for_time(wait_time=1.0) + cap = pytest_live_camera_utils.get_camera_capabilities(params['device_type']) + if cap == None: + debug_print("Device not found? : " + params['device_type']) + return + self.create_param_ifs(get_node_heirarchy(params)) + config["Color"]["default_profile1"],config["Color"]["default_profile2"] = pytest_live_camera_utils.get_default_profiles(cap["color_profile"], "Color") + config["Depth"]["default_profile1"],config["Depth"]["default_profile2"] = pytest_live_camera_utils.get_default_profiles(cap["depth_profile"], "Depth") + config["Infrared"]["default_profile1"],config["Infrared"]["default_profile2"] = pytest_live_camera_utils.get_default_profiles(cap["depth_profile"], "Infrared") + config["Infrared1"]["default_profile1"],config["Infrared1"]["default_profile2"] = pytest_live_camera_utils.get_default_profiles(cap["depth_profile"], "Infrared1") + config["Infrared2"]["default_profile1"],config["Infrared2"]["default_profile2"] = pytest_live_camera_utils.get_default_profiles(cap["depth_profile"], "Infrared2") + for key in cap["color_profile"]: + profile_type = key[0] + profile = key[1] + format = key[2] + if check_if_skip_test(profile_type, format): + skipped_tests.append(" ".join(key)) + continue + print("Testing " + " ".join(key)) + themes[0]['topic'] = config[profile_type]['topic'] + themes[0]['width'] = int(profile.split('x')[0]) + themes[0]['height'] = int(profile.split('x')[1]) + if themes[0]['width'] == int(config[profile_type]["default_profile2"].split('x')[0]): + self.set_string_param(config[profile_type]["profile"], config[profile_type]["default_profile1"]) + else: + self.set_string_param(config[profile_type]["profile"], config[profile_type]["default_profile2"]) + self.set_bool_param(config[profile_type]["param"], True) + self.disable_all_params() + self.spin_for_time(wait_time=0.2) + self.set_string_param(config[profile_type]["profile"], profile) + self.set_string_param(config[profile_type]["format"], format) + self.set_bool_param(config[profile_type]["param"], True) + try: + ret = self.run_test(themes) + assert ret[0], ret[1] + assert self.process_data(themes), " ".join(key) + " failed" + num_passed += 1 + except Exception as e: + exc_type, exc_obj, exc_tb = sys.exc_info() + fname = os.path.split(exc_tb.tb_frame.f_code.co_filename)[1] + print("Test failed") + print(e) + print(exc_type, fname, exc_tb.tb_lineno) + num_failed += 1 + failed_tests.append(" ".join(key)) + debug_print("Color tests completed") + for key in cap["depth_profile"]: + profile_type = key[0] + profile = key[1] + format = key[2] + if check_if_skip_test(profile_type, format): + skipped_tests.append(" ".join(key)) + continue + print("Testing " + " ".join(key)) + + themes[0]['topic'] = config[profile_type]['topic'] + themes[0]['width'] = int(profile.split('x')[0]) + themes[0]['height'] = int(profile.split('x')[1]) + if themes[0]['width'] == int(config[profile_type]["default_profile2"].split('x')[0]): + self.set_string_param(config[profile_type]["profile"], config[profile_type]["default_profile1"]) + else: + self.set_string_param(config[profile_type]["profile"], config[profile_type]["default_profile2"]) + self.set_bool_param(config[profile_type]["param"], True) + + + self.disable_all_params() + self.spin_for_time(wait_time=0.2) + self.set_string_param(config[profile_type]["profile"], profile) + self.set_string_param(config[profile_type]["format"], format) + self.set_bool_param(config[profile_type]["param"], True) + try: + ret = self.run_test(themes) + assert ret[0], ret[1] + assert self.process_data(themes), " ".join(key) + " failed" + num_passed += 1 + except Exception as e: + print("Test failed") + print(e) + num_failed += 1 + failed_tests.append(" ".join(key)) + debug_print("Depth tests completed") + finally: + #this step is important because the test will fail next time + pytest_rs_utils.kill_realsense2_camera_node() + self.shutdown() + print("Tests passed " + str(num_passed)) + print("Tests skipped " + str(len(skipped_tests))) + if len(skipped_tests): + debug_print("\nSkipped tests:" + params['device_type']) + debug_print("\n".join(skipped_tests)) + print("Tests failed " + str(num_failed)) + if len(failed_tests): + print("\nFailed tests:" + params['device_type']) + print("\n".join(failed_tests)) + + def disable_all_params(self): + self.set_bool_param('enable_color', False) + self.set_bool_param('enable_depth', False) + self.set_bool_param('enable_infra', False) + self.set_bool_param('enable_infra1', False) + self.set_bool_param('enable_infra2', False) + diff --git a/realsense2_camera/test/live_camera/test_camera_failing_tests.py b/realsense2_camera/test/live_camera/test_camera_failing_tests.py new file mode 100644 index 0000000000..3bdc87b19b --- /dev/null +++ b/realsense2_camera/test/live_camera/test_camera_failing_tests.py @@ -0,0 +1,268 @@ +# 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 pytest_rs_utils import get_node_heirarchy +import pytest_live_camera_utils +from rclpy.parameter import Parameter +from rcl_interfaces.msg import ParameterValue +from rcl_interfaces.srv import SetParameters, GetParameters, ListParameters +from pytest_live_camera_utils import debug_print + +test_params_all_profiles_d455 = { + 'camera_name': 'D455', + 'device_type': 'D455', + } +test_params_all_profiles_d415 = { + 'camera_name': 'D415', + 'device_type': 'D415', + } +''' +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_all_profiles_d415],indirect=True) +@pytest.mark.launch(fixture=launch_descr_with_parameters) +class TestLiveCamera_Change_Resolution(pytest_rs_utils.RsTestBaseClass): + def test_LiveCamera_Change_Resolution(self,launch_descr_with_parameters): + passed_tests = [] + failed_tests = [] + num_passed = 0 + num_failed = 0 + wait_time_s = 1.2 + cap = {} + ''' + need two configurations with different profiles(height & width) for each profile, + this is to ensure the test sets a different profile first, before testing the + actual profile to be tested. + ''' + cap['color_profile'] = [ + ['Color', '1920x1080x30','RGB8'], + ['Color', '1280x720x30','RGB8'], + ] + cap['depth_profile'] = [ + ['Infrared1', '1920x1080x25', 'Y8'], + ['Infrared1', '1920x1080x25', 'Y16'], + ['Infrared1', '1920x1080x15', 'Y16'], + ['Infrared', '848x100x100', 'BGRA8'], + ['Infrared', '640x480x60', 'BGRA8'], + ['Infrared', '640x480x60', 'RGBA8'], + ['Infrared', '640x480x60', 'RGB8'], + ['Infrared', '640x360x60', 'RGBA8'], + ['Infrared', '640x360x60', 'RGB8'], + ['Infrared', '640x360x30', 'UYVY'], + ['Infrared', '480x270x15', 'RGB8'], + ['Infrared', '424x240x60', 'BGRA8'], + ['Infrared', '424x240x30', 'UYVY'], + ['Infrared1', '1920x1080x15', 'Y8'], + ['Infrared1', '1280x720x30', 'Y8'], + ['Infrared1', '1280x720x15', 'Y8'], + ['Infrared1', '1280x720x6', 'Y8'], + ['Infrared1', '960x540x25', 'Y16'], + ['Infrared1', '960x540x15', 'Y16'], + ['Infrared1', '848x480x90', 'Y8'], + ['Infrared1', '848x480x60', 'Y8'], + ['Infrared1', '848x480x30', 'Y8'], + ['Infrared1', '848x480x15', 'Y8'], + ['Infrared1', '848x480x6', 'Y8'], + ['Infrared1', '848x100x100', 'Y8'], + ['Infrared1', '640x480x90', 'Y8'], + ['Infrared1', '640x480x60', 'Y8'], + ['Infrared1', '640x480x30', 'Y8'], + ['Infrared1', '640x480x15', 'Y8'], + ['Infrared1', '640x480x6', 'Y8'], + ['Infrared1', '640x360x90', 'Y8'], + ['Infrared1', '640x360x60', 'Y8'], + ['Infrared1', '640x360x30', 'Y8'], + ['Infrared1', '640x360x15', 'Y8'], + ['Infrared1', '640x360x6', 'Y8'], + ['Infrared1', '480x270x90', 'Y8'], + ['Infrared1', '480x270x60', 'Y8'], + ['Infrared1', '480x270x30', 'Y8'], + ['Infrared1', '480x270x15', 'Y8'], + ['Infrared1', '480x270x6', 'Y8'], + ['Infrared1', '424x240x90', 'Y8'], + ['Infrared1', '424x240x60', 'Y8'], + ['Infrared1', '424x240x30', 'Y8'], + ['Infrared1', '424x240x15', 'Y8'], + ['Infrared1', '424x240x6', 'Y8'], + ['Infrared2', '1920x1080x25', 'Y16'], + ['Infrared2', '1920x1080x25', 'Y8'], + ['Infrared2', '1920x1080x15', 'Y16'], + ['Infrared2', '1920x1080x15', 'Y8'], + ['Infrared2', '1280x720x30', 'Y8'], + ['Infrared2', '1280x720x15', 'Y8'], + ['Infrared2', '1280x720x6', 'Y8'], + ['Infrared2', '960x540x25', 'Y16'], + ['Infrared2', '960x540x15', 'Y16'], + ['Infrared2', '848x480x90', 'Y8'], + ['Infrared2', '848x480x60', 'Y8'], + ['Infrared2', '848x480x30', 'Y8'], + ['Infrared2', '848x480x15', 'Y8'], + ['Infrared2', '848x480x6', 'Y8'], + ['Infrared2', '848x100x100', 'Y8'], + ['Infrared2', '640x480x90', 'Y8'], + ['Infrared2', '640x480x60', 'Y8'], + ['Infrared2', '640x480x30', 'Y8'], + ['Infrared2', '640x480x15', 'Y8'], + ['Infrared2', '640x480x6', 'Y8'], + ['Infrared2', '640x360x90', 'Y8'], + ['Infrared2', '640x360x60', 'Y8'], + ['Infrared2', '640x360x30', 'Y8'], + ['Infrared2', '640x360x15', 'Y8'], + ['Infrared2', '640x360x6', 'Y8'], + ['Infrared2', '480x270x90', 'Y8'], + ['Infrared2', '480x270x60', 'Y8'], + ['Infrared2', '480x270x30', 'Y8'], + ['Infrared2', '480x270x15', 'Y8'], + ['Infrared2', '480x270x6', 'Y8'], + ['Infrared2', '424x240x90', 'Y8'], + ['Infrared2', '424x240x60', 'Y8'], + ['Infrared2', '424x240x30', 'Y8'], + ['Infrared2', '424x240x15', 'Y8'], + ['Infrared2', '424x240x6', 'Y8'], + ['Depth', '640x360x30', 'Z16'], + ['Depth', '480x270x30', 'Z16'], + ['Depth', '424x240x30', 'Z16'], + ] + params = launch_descr_with_parameters[1] + themes = [{'topic':get_node_heirarchy(params)+'/color/image_raw', 'msg_type':msg_Image,'expected_data_chunks':1,'initial_reset':True}] + config = pytest_live_camera_utils.get_profile_config(get_node_heirarchy(params)) + try: + ''' + initialize, run and check the data + ''' + serial_no = None + if 'serial_no' in params: + serial_no = params['serial_no'] + self.init_test("RsTest"+params['camera_name']) + self.spin_for_time(wait_time=1.0) + if cap == None: + debug_print("Device not found? : " + params['device_type']) + return + self.create_param_ifs(get_node_heirarchy(params)) + config["Color"]["default_profile1"],config["Color"]["default_profile2"] = pytest_live_camera_utils.get_default_profiles(cap["color_profile"], "Color") + config["Depth"]["default_profile1"],config["Depth"]["default_profile2"] = pytest_live_camera_utils.get_default_profiles(cap["depth_profile"], "Depth") + config["Infrared"]["default_profile1"],config["Infrared"]["default_profile2"] = pytest_live_camera_utils.get_default_profiles(cap["depth_profile"], "Infrared") + config["Infrared1"]["default_profile1"],config["Infrared1"]["default_profile2"] = pytest_live_camera_utils.get_default_profiles(cap["depth_profile"], "Infrared1") + config["Infrared2"]["default_profile1"],config["Infrared2"]["default_profile2"] = pytest_live_camera_utils.get_default_profiles(cap["depth_profile"], "Infrared2") + for key in cap["color_profile"]: + profile_type = key[0] + profile = key[1] + format = key[2] + print("Testing " + " ".join(key)) + themes[0]['topic'] = config[profile_type]['topic'] + themes[0]['width'] = int(profile.split('x')[0]) + themes[0]['height'] = int(profile.split('x')[1]) + if themes[0]['width'] == int(config[profile_type]["default_profile2"].split('x')[0]): + self.set_string_param(config[profile_type]["profile"], config[profile_type]["default_profile1"]) + else: + self.set_string_param(config[profile_type]["profile"], config[profile_type]["default_profile2"]) + self.set_bool_param(config[profile_type]["param"], True) + self.disable_all_params() + self.spin_for_time(wait_time=wait_time_s) + self.set_string_param(config[profile_type]["profile"], profile) + self.set_string_param(config[profile_type]["format"], format) + self.set_bool_param(config[profile_type]["param"], True) + self.spin_for_time(wait_time=wait_time_s) + try: + ret = self.run_test(themes) + assert ret[0], ret[1] + assert self.process_data(themes), " ".join(key) + " failed" + num_passed += 1 + passed_tests.append(" ".join(key)) + except Exception as e: + exc_type, exc_obj, exc_tb = sys.exc_info() + fname = os.path.split(exc_tb.tb_frame.f_code.co_filename)[1] + print("Test failed") + print(e) + print(exc_type, fname, exc_tb.tb_lineno) + num_failed += 1 + failed_tests.append(" ".join(key)) + debug_print("Color tests completed") + for key in cap["depth_profile"]: + profile_type = key[0] + profile = key[1] + format = key[2] + print("Testing " + " ".join(key)) + + themes[0]['topic'] = config[profile_type]['topic'] + themes[0]['width'] = int(profile.split('x')[0]) + themes[0]['height'] = int(profile.split('x')[1]) + if themes[0]['width'] == int(config[profile_type]["default_profile2"].split('x')[0]): + self.set_string_param(config[profile_type]["profile"], config[profile_type]["default_profile1"]) + else: + self.set_string_param(config[profile_type]["profile"], config[profile_type]["default_profile2"]) + self.set_bool_param(config[profile_type]["param"], True) + + + self.disable_all_params() + self.spin_for_time(wait_time=wait_time_s) + self.set_string_param(config[profile_type]["profile"], profile) + self.set_string_param(config[profile_type]["format"], format) + self.set_bool_param(config[profile_type]["param"], True) + self.spin_for_time(wait_time=wait_time_s) + try: + ret = self.run_test(themes) + assert ret[0], ret[1] + assert self.process_data(themes), " ".join(key) + " failed" + num_passed += 1 + passed_tests.append(" ".join(key)) + except Exception as e: + print("Test failed") + print(e) + num_failed += 1 + failed_tests.append(" ".join(key)) + debug_print("Depth tests completed") + finally: + #this step is important because the test will fail next time + pytest_rs_utils.kill_realsense2_camera_node() + self.shutdown() + print("Tests passed " + str(num_passed)) + if len(passed_tests): + debug_print("\nPassed tests:" + params['device_type']) + debug_print("\n".join(passed_tests)) + print("Tests failed " + str(num_failed)) + if len(failed_tests): + print("\nFailed tests:" + params['device_type']) + print("\n".join(failed_tests)) + + def disable_all_params(self): + self.set_bool_param('enable_color', False) + self.set_bool_param('enable_depth', False) + self.set_bool_param('enable_infra', False) + self.set_bool_param('enable_infra1', False) + self.set_bool_param('enable_infra2', False) \ No newline at end of file diff --git a/realsense2_camera/test/live_camera/test_camera_fps_tests.py b/realsense2_camera/test/live_camera/test_camera_fps_tests.py new file mode 100644 index 0000000000..cff5cadbb4 --- /dev/null +++ b/realsense2_camera/test/live_camera/test_camera_fps_tests.py @@ -0,0 +1,102 @@ +# 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 get_rosbag_file_path +from pytest_rs_utils import get_node_heirarchy + +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', + } +''' +The test was implemented to check the fps of Depth and Color frames. The RosTopicHz had to be +modified to make it work, see py_rs_utils for more details. +To check the fps, a value 'expected_fps_in_hz' has to be added to the corresponding theme +''' +@pytest.mark.parametrize("launch_descr_with_parameters", [test_params_depth_avg_1],indirect=True) +@pytest.mark.launch(fixture=launch_descr_with_parameters) +class TestCamera_TestFPS(pytest_rs_utils.RsTestBaseClass): + def test_camera_test_fps(self,launch_descr_with_parameters): + params = launch_descr_with_parameters[1] + 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('enable_color', False) + + themes = [ + {'topic':get_node_heirarchy(params)+'/depth/image_rect_raw', + 'msg_type':msg_Image, + 'expected_data_chunks':100, + } + ] + profiles = ['640x480x5','640x480x15', '640x480x30', '640x480x90'] + for profile in profiles: + print("Testing profile: ", profile) + themes[0]['expected_fps_in_hz'] = float(profile.split('x')[2]) + assert self.set_string_param('depth_module.profile', profile) + assert self.set_bool_param('enable_depth', True) + ret = self.run_test(themes, timeout=25.0) + assert ret[0], ret[1] + assert self.process_data(themes) + + themes = [ + {'topic':get_node_heirarchy(params)+'/color/image_raw', + 'msg_type':msg_Image, + 'expected_data_chunks':100, + } + ] + profiles = ['640x480x5','640x480x15', '640x480x30', '1280x720x30'] + for profile in profiles: + print("Testing profile: ", profile) + themes[0]['expected_fps_in_hz'] = float(profile.split('x')[2]) + assert self.set_string_param('rgb_camera.profile', profile) + assert self.set_bool_param('enable_color', True) + ret = self.run_test(themes, timeout=25.0) + assert ret[0], ret[1] + 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/live_camera/test_camera_imu_tests.py b/realsense2_camera/test/live_camera/test_camera_imu_tests.py new file mode 100644 index 0000000000..fa173e0a8e --- /dev/null +++ b/realsense2_camera/test/live_camera/test_camera_imu_tests.py @@ -0,0 +1,126 @@ +# 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 pytest_rs_utils import get_node_heirarchy + +import pytest_live_camera_utils +from rclpy.parameter import Parameter +from rcl_interfaces.msg import ParameterValue +from rcl_interfaces.srv import SetParameters, GetParameters, ListParameters +from pytest_live_camera_utils import debug_print + +test_params_all_profiles_d455 = { + 'camera_name': 'D455', + 'device_type': 'D455', + 'enable_accel':True, + 'enable_gyro':True, + 'unite_imu_method':1, + } + +@pytest.mark.parametrize("launch_descr_with_parameters", [ + pytest.param(test_params_all_profiles_d455, marks=pytest.mark.d455) + ],indirect=True) +@pytest.mark.launch(fixture=launch_descr_with_parameters) +class TestLiveCamera_TestMotionSensor(pytest_rs_utils.RsTestBaseClass): + def test_LiveCamera_check_motion_sensor(self,launch_descr_with_parameters): + params = launch_descr_with_parameters[1] + themes = [{'topic':get_node_heirarchy(params)+'/imu', 'msg_type':msg_Imu,'expected_data_chunks':1}, + {'topic':get_node_heirarchy(params)+'/gyro/sample', 'msg_type':msg_Imu,'expected_data_chunks':1}, + {'topic':get_node_heirarchy(params)+'/accel/sample', 'msg_type':msg_Imu,'expected_data_chunks':1}] + IMU_TOPIC = 0 + GYRO_TOPIC = 1 + ACCEL_TOPIC = 2 + if params['unite_imu_method'] == '0': + themes[IMU_TOPIC]['expected_data_chunks'] = 0 + try: + ''' + initialize, run and check the data + ''' + msg = "Test with the default params " + self.init_test("RsTest"+params['camera_name']) + self.create_param_ifs(get_node_heirarchy(params)) + print(msg) + ret = self.run_test(themes) + assert ret[0], msg + str(ret[1]) + assert self.process_data(themes), msg + " failed" + + + msg = "Test with the accel false " + self.set_bool_param('enable_accel', False) + self.set_bool_param('enable_gyro', True) + ''' + This step fails if the next line is commented + ''' + self.set_integer_param('unite_imu_method', 0) #this shouldn't matter because the unite_imu_method cannot be changed + themes[ACCEL_TOPIC]['expected_data_chunks'] = 0 + print(msg) + ret = self.run_test(themes) + assert ret[0], msg + str(ret[1]) + assert self.process_data(themes), msg + " failed" + ''' + Test fails + ''' + + msg = "Test with the gyro false " + self.set_bool_param('enable_accel', True) + self.set_bool_param('enable_gyro', False) + self.set_integer_param('unite_imu_method', 0) #this shouldn't matter because the unite_imu_method cannot be changed + themes[IMU_TOPIC]['expected_data_chunks'] = 1 + themes[ACCEL_TOPIC]['expected_data_chunks'] = 1 + themes[GYRO_TOPIC]['expected_data_chunks'] = 0 + print(msg) + ret = self.run_test(themes) + assert ret[0], msg + str(ret[1]) + assert self.process_data(themes), msg + " failed" + + ''' + Test fails, both gyro and accel data is available, not imu + ''' + msg = "Test with both gyro and accel false " + self.set_bool_param('enable_accel', False) + self.set_bool_param('enable_gyro', False) + self.set_integer_param('unite_imu_method', 1) #this shouldn't matter because the unite_imu_method cannot be changed + + themes[ACCEL_TOPIC]['expected_data_chunks'] = 0 + themes[GYRO_TOPIC]['expected_data_chunks'] = 0 + themes[IMU_TOPIC]['expected_data_chunks'] = 1 #Seems that imu data is available even if gyro and accel is disabled + print(msg) + ret = self.run_test(themes, initial_wait_time=1.0) #wait_time added as test doesn't have to wait for any data + assert ret[0], msg + " failed" + assert self.process_data(themes), msg +" failed" + + 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/live_camera/test_d415_basic_tests.py b/realsense2_camera/test/live_camera/test_d415_basic_tests.py new file mode 100644 index 0000000000..8b88a30e67 --- /dev/null +++ b/realsense2_camera/test/live_camera/test_d415_basic_tests.py @@ -0,0 +1,150 @@ +# 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 get_rosbag_file_path +from pytest_rs_utils import get_node_heirarchy +import pytest_live_camera_utils + +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': 'D415', + 'device_type': 'D415', + } +''' +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 D415 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 TestD415_Change_Resolution(pytest_rs_utils.RsTestBaseClass): + def test_D415_Change_Resolution(self,launch_descr_with_parameters): + params = launch_descr_with_parameters[1] + failed_tests = [] + num_passed = 0 + num_failed = 0 + themes = [{'topic':get_node_heirarchy(params)+'/color/image_raw', 'msg_type':msg_Image,'expected_data_chunks':1}] + config = pytest_live_camera_utils.get_profile_config(get_node_heirarchy(params)) + config["Color"]["default_profile1"] = "640x480x6" + config["Color"]["default_profile2"] = "1280x720x6" + config["Depth"]["default_profile1"] = "640x480x6" + config["Depth"]["default_profile2"] = "1280x720x6" + config["Infrared"]["default_profile1"] = "640x480x6" + config["Infrared"]["default_profile2"] = "1280x720x6" + config["Infrared1"]["default_profile1"] = "640x480x6" + config["Infrared1"]["default_profile2"] = "1280x720x6" + config["Infrared2"]["default_profile1"] = "640x480x6" + config["Infrared2"]["default_profile2"] = "1280x720x6" + + cap = [ + #['Infrared1', '1920x1080x25', 'Y8'], + #['Infrared1', '1920x1080x15', 'Y16'], + ['Infrared', '848x100x100', 'BGRA8'], + ['Infrared', '848x480x60', 'RGBA8'], + ['Infrared', '640x480x60', 'RGBA8'], + ['Infrared', '640x480x60', 'BGR8'], + ['Infrared', '640x360x60', 'BGRA8'], + ['Infrared', '640x360x60', 'BGR8'], + ['Infrared', '640x360x30', 'UYVY'], + ['Infrared', '480x270x60', 'BGRA8'], + ['Infrared', '480x270x60', 'RGB8'], + ['Infrared', '424x240x60', 'BGRA8'], + + ] + try: + ''' + initialize, run and check the data + ''' + self.init_test("RsTest"+params['camera_name']) + self.spin_for_time(wait_time=1.0) + self.create_param_ifs(get_node_heirarchy(params)) + + for key in cap: + profile_type = key[0] + profile = key[1] + format = key[2] + print("Testing " + " ".join(key)) + themes[0]['topic'] = config[profile_type]['topic'] + themes[0]['width'] = int(profile.split('x')[0]) + themes[0]['height'] = int(profile.split('x')[1]) + #''' + if themes[0]['width'] == int(config[profile_type]["default_profile2"].split('x')[0]): + self.set_string_param(config[profile_type]["profile"], config[profile_type]["default_profile1"]) + else: + self.set_string_param(config[profile_type]["profile"], config[profile_type]["default_profile2"]) + self.set_bool_param(config[profile_type]["param"], True) + self.disable_all_params() + #self.set_string_param("depth_profile", "640x480x6") + #self.set_bool_param("enable_depth", True) + #''' + self.spin_for_time(wait_time=0.2) + self.set_string_param(config[profile_type]["profile"], profile) + self.set_string_param(config[profile_type]["format"], format) + self.set_bool_param(config[profile_type]["param"], True) + try: + ret = self.run_test(themes, timeout=5.0) + assert ret[0], ret[1] + assert self.process_data(themes), " ".join(key) + " failed" + num_passed += 1 + except Exception as e: + exc_type, exc_obj, exc_tb = sys.exc_info() + fname = os.path.split(exc_tb.tb_frame.f_code.co_filename)[1] + print("Test failed") + print(e) + print(exc_type, fname, exc_tb.tb_lineno) + num_failed += 1 + failed_tests.append(" ".join(key)) + finally: + #this step is important because the test will fail next time + pytest_rs_utils.kill_realsense2_camera_node() + self.shutdown() + if num_failed != 0: + print("Failed tests:") + print("\n".join(failed_tests)) + assert False, " Tests failed" + + + def disable_all_params(self): + ''' + self.set_bool_param('enable_color', False) + self.set_bool_param('enable_depth', False) + self.set_bool_param('enable_infra', False) + self.set_bool_param('enable_infra1', False) + self.set_bool_param('enable_infra2', False) + ''' + pass + diff --git a/realsense2_camera/test/live_camera/test_d455_basic_tests.py b/realsense2_camera/test/live_camera/test_d455_basic_tests.py new file mode 100644 index 0000000000..fcf4561714 --- /dev/null +++ b/realsense2_camera/test/live_camera/test_d455_basic_tests.py @@ -0,0 +1,93 @@ +# 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 get_rosbag_file_path +from pytest_rs_utils import get_node_heirarchy + +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':get_node_heirarchy(params)+'/color/image_raw', + 'msg_type':msg_Image, + 'expected_data_chunks':1, + 'width':640, + 'height':480, + #'data':data + } + ] + 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('enable_color', False) + assert self.set_string_param('rgb_camera.profile', '640x480x30') + assert 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', '1280x800x5') + self.set_bool_param('enable_color', True) + themes[0]['width'] = 1280 + themes[0]['height'] = 800 + + ret = self.run_test(themes) + assert ret[0], ret[1] + 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_live_camera_utils.py b/realsense2_camera/test/utils/pytest_live_camera_utils.py new file mode 100644 index 0000000000..3a5ab6f600 --- /dev/null +++ b/realsense2_camera/test/utils/pytest_live_camera_utils.py @@ -0,0 +1,190 @@ +# 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 time +import ctypes +import struct +import requests +import json + +from pytest_rs_utils import debug_print + + +def get_profile_config(camera_name): + config = { + "Color":{"profile":"rgb_camera.profile", "format":'rgb_camera.color_format', "param":"enable_color", "topic":camera_name+'/color/image_raw',}, + "Depth":{"profile":"depth_module.profile", "format":'depth_module.depth_format', "param":"enable_depth", 'topic':camera_name+'/depth/image_rect_raw'}, + "Infrared":{"profile":"depth_module.profile", "format":'depth_module.infra_format', "param":"enable_infra", 'topic':camera_name+'/infra/image_rect_raw'}, + "Infrared1":{"profile":"depth_module.profile", "format":'depth_module.infra1_format',"param":"enable_infra1", 'topic':camera_name+'/infra/image_rect_raw'}, + "Infrared2":{"profile":"depth_module.profile", "format":'depth_module.infra2_format',"param":"enable_infra2", 'topic':camera_name+'/infra/image_rect_raw'}, + } + return config + + +def get_default_profiles(cap, profile): + profile1 = "" + profile2 = "" + for profiles in cap: + if profiles[0] == profile and int(profiles[1].split('x')[0]) != 640: + profile1 = profiles[1] + break + for profiles in cap: + if profiles[0] == profile and int(profiles[1].split('x')[0]) != int(profile1.split('x')[0]): + profile2 = profiles[1] + break + debug_print(profile + " default profile1:" + profile1) + debug_print(profile + " default profile2:" + profile2) + return profile1,profile2 + +def get_camera_capabilities_short(device_type, serial_no=None): + short_data = os.popen("rs-enumerate-devices -s").read().splitlines() + print(serial_no) + for line in short_data: + print(line) + if device_type in line: + if serial_no is None or serial_no == "" : + print(device_type+ " found in " + line) + return + if serial_no in line: + print(device_type + " with serial_no " + serial_no +" found in " + line) + return + print(device_type + " not found") + + +device_info_string = "Device info:" + +def get_device_info_location(long_data, index=0): + for line_no in range(index, len(long_data)): + if device_info_string in long_data[line_no]: + return line_no + return len(long_data) + +stream_profile_string = "Stream Profiles supported by" +def get_stream_profile_location(long_data, start_index, end_index, profile_string): + for line_no in range(start_index, end_index): + if stream_profile_string in long_data[line_no]: + if profile_string in long_data[line_no]: + return line_no + return None + +def get_depth_profiles(long_data, start_index, end_index): + cap = [] + for line_no in range(start_index, end_index): + if len(long_data[line_no]) == 0: + break + debug_print("depth profile processing:" + long_data[line_no]) + depth_profile = long_data[line_no].split() + if len(depth_profile) == 5: + profile = depth_profile[0] + value = depth_profile[1]+"x"+depth_profile[3] + format = depth_profile[4] + elif len(depth_profile) == 6: + profile = depth_profile[0]+depth_profile[1] + value = depth_profile[2]+"x"+depth_profile[4] + format = depth_profile[5] + else: + assert false, "Seems that the depth profile info format printed by rs-enumerate-devices changed" + value = value[:-2] + debug_print("depth profile added: " + profile, value, format) + cap.append([profile, value, format]) + debug_print(cap) + return cap + + +def get_color_profiles(long_data, start_index, end_index): + cap = [] + for line_no in range(start_index, end_index): + if len(long_data[line_no]) == 0: + break + debug_print("color profile processing:" + long_data[line_no]) + color_profile = long_data[line_no].split() + if len(color_profile) == 5: + profile = color_profile[0] + value = color_profile[1]+"x"+color_profile[3] + format = color_profile[4] + else: + assert false, "Seems that the color profile info format printed by rs-enumerate-devices changed" + value = value[:-2] + debug_print("color profile added: " + profile, value, format) + cap.append([profile, value, format]) + debug_print(cap) + return cap + +NAME_LINE_INDEX = 1 +NAME_LINE_NAME_OFFSET = 4 +SERIAL_NO_LINE_INDEX = 2 +SERIAL_NO_VALUE_OFFSET = 3 +def parse_device_info(long_data, start_index, end_index, device_type, serial_no): + #after device_info, the next line should have the name and device type + capability = {} + debug_print("Searching for data between lines ", str(start_index) + " and " + str(end_index)) + name_line = long_data[start_index+NAME_LINE_INDEX].split() + if name_line[0] != "Name": + assert False, "rs-enumerate-devices output format changed" + if name_line[4] != device_type: + debug_print("device not matching:" + name_line[NAME_LINE_NAME_OFFSET]) + return None + debug_print("device matched:" + name_line[NAME_LINE_NAME_OFFSET]) + if serial_no != None: + #next line after nameline should have the serial_no + serial_no_line = long_data[start_index+SERIAL_NO_LINE_INDEX].split() + if serial_no_line[0] != "Serial": + assert False, "rs-enumerate-devices output format changed" + if serial_no_line[SERIAL_NO_VALUE_OFFSET] != serial_no: + debug_print("serial_no not matching:" + serial_no_line[SERIAL_NO_VALUE_OFFSET]) + return None + debug_print("serial_no matched:" + serial_no_line[SERIAL_NO_VALUE_OFFSET]) + else: + serial_no = long_data[start_index+SERIAL_NO_LINE_INDEX].split()[SERIAL_NO_VALUE_OFFSET] + + capability["device_type"] = device_type + capability["serial_no"] = serial_no + depth_profile_index = get_stream_profile_location(long_data, start_index, end_index, "Stereo Module") + if depth_profile_index != None: + capability["depth_profile"] = get_depth_profiles(long_data, depth_profile_index+3, end_index) + rgb_profile_index = get_stream_profile_location(long_data, start_index, end_index, "RGB Camera") + if rgb_profile_index != None: + capability["color_profile"] = get_color_profiles(long_data, rgb_profile_index+3, end_index) + return capability + +def get_camera_capabilities(device_type, serial_no=None): + long_data = os.popen("rs-enumerate-devices").read().splitlines() + debug_print(serial_no) + index = 0 + while index < len(long_data): + index = get_device_info_location(long_data, index) + if index == len(long_data): + return + else: + debug_print("DeviceInfo found at: " + str(index)) + start_index = index + index += 1 + index = get_device_info_location(long_data, index) + capability = parse_device_info(long_data, start_index, index, device_type, serial_no) + if capability != None: + return capability + return None + +if __name__ == '__main__': + device_type = 'D455' + serial_no = None + if len(sys.argv) > 1: + device_type = sys.argv[1] + if len(sys.argv) > 2: + serial_no = sys.argv[2] + + cap = get_camera_capabilities(device_type, serial_no) + print("Capabilities:") + print(cap) \ No newline at end of file diff --git a/realsense2_camera/test/utils/pytest_rs_utils.py b/realsense2_camera/test/utils/pytest_rs_utils.py index 63ede24d57..d78eeb9ef8 100644 --- a/realsense2_camera/test/utils/pytest_rs_utils.py +++ b/realsense2_camera/test/utils/pytest_rs_utils.py @@ -15,6 +15,7 @@ import sys import time from collections import deque +import functools import pytest import numpy as np @@ -28,10 +29,31 @@ from rclpy import qos from rclpy.node import Node from rclpy.utilities import ok +from ros2topic.verb.bw import ROSTopicBandwidth +from ros2topic.verb.hz import ROSTopicHz +from ros2topic.api import get_msg_class import ctypes import struct import requests +import math + +#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 +''' +humble doesn't have the SetParametersResult and SetParameters_Response imported using +__init__.py. The below two lines can be used for iron and hopefully succeeding ROS2 versions +''' +#from rcl_interfaces.msg import SetParametersResult +#from rcl_interfaces.srv import SetParameters_Response +from rcl_interfaces.msg._set_parameters_result import SetParametersResult +from rcl_interfaces.srv._set_parameters import SetParameters_Response + +from rcl_interfaces.msg import ParameterType +from rcl_interfaces.msg import ParameterValue + from sensor_msgs.msg import Image as msg_Image @@ -54,6 +76,11 @@ import tempfile import os import requests + +def debug_print(*args): + if(True): + print(*args) + class RosbagManager(object): def __new__(cls): if not hasattr(cls, 'instance'): @@ -82,12 +109,13 @@ def init(self): def get_rosbag_path(self, filename): if filename in self.rosbag_files: return self.rosbag_location + "/" + filename -rosbagMgr = RosbagManager() + def get_rosbag_file_path(filename): + rosbagMgr = RosbagManager() path = rosbagMgr.get_rosbag_path(filename) assert path, "No rosbag file found :"+filename return path - +get_rosbag_file_path.rosbagMgr = None def CameraInfoGetData(rec_filename, topic): data = importRosbag(rec_filename, importTopics=[topic], log='ERROR', disable_bar=True)[topic] @@ -552,6 +580,16 @@ def delayed_launch_descr_with_parameters(request): actions = [ first_node,], period=period) ]),params +class HzWrapper(ROSTopicHz): + def _callback_hz(self, m, topic=None): + if self.get_last_printed_tn(topic=topic) == 0: + self.set_last_printed_tn(self.get_msg_tn(topic=topic), topic=topic) + return self.callback_hz(m, topic) + def restart_topic(self, topic): + self._last_printed_tn[topic] = 0 + self._times[topic].clear() + self._msg_tn[topic] = 0 + self._msg_t0[topic] = -1 ''' This is that holds the test node that listens to a subscription created by a test. @@ -564,6 +602,7 @@ def __init__(self, name='test_node'): self.data = {} self.tfBuffer = None self.frame_counter = {} + self._ros_topic_hz = HzWrapper(super(), 10000, use_wtime=False) def wait_for_node(self, node_name, timeout=8.0): start = time.time() @@ -576,10 +615,21 @@ def wait_for_node(self, node_name, timeout=8.0): return True, "" time.sleep(timeout/5) return False, "Timed out waiting for "+ str(timeout)+ "seconds" - def create_subscription(self, msg_type, topic , data_type, store_raw_data): - super().create_subscription(msg_type, topic , self.rsCallback(topic,msg_type, store_raw_data), data_type) + def reset_data(self, topic): self.data[topic] = deque() self.frame_counter[topic] = 0 + self._ros_topic_hz.restart_topic(topic) + + + def create_subscription(self, msg_type, topic , data_type, store_raw_data, measure_hz): + self.reset_data(topic) + super().create_subscription(msg_type, topic , self.rsCallback(topic,msg_type, store_raw_data), data_type) + #hz measurements are not working + if measure_hz == True: + msg_class = get_msg_class(super(), topic, blocking=True, include_hidden_topics=True) + super().create_subscription(msg_class,topic,functools.partial(self._ros_topic_hz._callback_hz, topic=topic),data_type) + self._ros_topic_hz.set_last_printed_tn(0, topic=topic) + if (os.getenv('ROS_DISTRO') != "dashing") and (self.tfBuffer == None): self.tfBuffer = tf2_ros.Buffer() self.tf_listener = tf2_ros.TransformListener(self.tfBuffer, super()) @@ -592,14 +642,14 @@ def pop_first_chunk(self, topic): def image_msg_to_numpy(self, data): fmtString = data.encoding - if fmtString in ['mono8', '8UC1', 'bgr8', 'rgb8', 'bgra8', 'rgba8']: + if fmtString in ['mono8', '8UC1', 'bgr8', 'rgb8', 'bgra8', 'rgba8', 'yuv422_yuy2', 'yuv422']: img = np.frombuffer(data.data, np.uint8) elif fmtString in ['mono16', '16UC1', '16SC1']: img = np.frombuffer(data.data, np.uint16) elif fmtString == '32FC1': img = np.frombuffer(data.data, np.float32) else: - print('image format not supported:' + fmtString) + print('py_rs_utils.image_msg_to_numpy:image format not supported:' + fmtString) return None depth = data.step / (data.width * img.dtype.itemsize) @@ -613,9 +663,9 @@ def image_msg_to_numpy(self, data): The processing of data is taken from the existing testcase in scripts rs2_test ''' def rsCallback(self, topic, msg_type, store_raw_data): - print("RSCallback") + debug_print("RSCallback") def _rsCallback(data): - print('Got the callback for ' + topic) + debug_print('Got the callback for ' + topic) #print(data.header) self.flag = True if store_raw_data == True: @@ -707,23 +757,71 @@ def init_test(self,name='RsTestNode'): self.flag = False self.node = RsTestNode(name) self.subscribed_topics = [] - def create_subscription(self, msg_type, topic, data_type, store_raw_data=False): + + def wait_for_node(self, node_name, timeout=8.0): + self.node.wait_for_node(node_name, timeout) + def create_subscription(self, msg_type, topic, data_type, store_raw_data=False, measure_hz=False): if not topic in self.subscribed_topics: - self.node.create_subscription(msg_type, topic, data_type, store_raw_data) + self.node.create_subscription(msg_type, topic, data_type, store_raw_data, measure_hz) self.subscribed_topics.append(topic) + else: + self.node.reset_data(topic) + + - def spin_for_data(self,themes): + 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 send_param(self, req): + future = self.set_param_if.call_async(req) + while rclpy.ok(): + rclpy.spin_once(self.node) + if future.done(): + try: + response = future.result() + if response.results[0].successful: + return True + except Exception as e: + print("exception raised:") + print(e) + pass + return False + + 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)] + return self.send_param(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)] + return self.send_param(req) + + def set_integer_param(self, param_name, param_value): + req = SetParameters.Request() + new_param_value = ParameterValue(type=ParameterType.PARAMETER_INTEGER, integer_value=param_value) + req.parameters = [Parameter(name=param_name, value=new_param_value)] + return self.send_param(req) + + def spin_for_data(self,themes, timeout=5.0): start = time.time() ''' timeout value varies depending upon the system, it needs to be more if the access is over the network ''' - timeout = 25.0 print('Waiting for topic... ' ) flag = False while (time.time() - start) < timeout: rclpy.spin_once(self.node, timeout_sec=1) - print('Spun once... ' ) + debug_print('Spun once... ' ) all_found = True for theme in themes: if theme['expected_data_chunks'] > int(self.node.get_num_chunks(theme['topic'])): @@ -739,21 +837,28 @@ def spin_for_data(self,themes): def spin_for_time(self,wait_time): start = time.time() - print('Waiting for topic... ' ) + print('Waiting for time... ' ) flag = False - while time.time() - start < wait_time: - rclpy.spin_once(self.node) - print('Spun once... ' ) + while (time.time() - start) < wait_time: + print('Spun for time once... ' ) + rclpy.spin_once(self.node, timeout_sec=wait_time) - def run_test(self, themes): + def run_test(self, themes, initial_wait_time=0.0, timeout=5.0): try: for theme in themes: store_raw_data = False if 'store_raw_data' in theme: store_raw_data = theme['store_raw_data'] - self.create_subscription(theme['msg_type'], theme['topic'] , qos.qos_profile_sensor_data,store_raw_data) + if 'expected_fps_in_hz' in theme: + measure_hz = True + else: + measure_hz = False + + self.create_subscription(theme['msg_type'], theme['topic'] , qos.qos_profile_sensor_data,store_raw_data, measure_hz) print('subscription created for ' + theme['topic']) - self.flag = self.spin_for_data(themes) + if initial_wait_time != 0.0: + self.spin_for_time(initial_wait_time) + self.flag = self.spin_for_data(themes, timeout) except Exception as e: exc_type, exc_obj, exc_tb = sys.exc_info() fname = os.path.split(exc_tb.tb_frame.f_code.co_filename)[1] @@ -773,11 +878,25 @@ def run_test(self, themes): ''' def process_data(self, themes): for theme in themes: + if theme['expected_data_chunks'] == 0: + assert self.node.get_num_chunks(theme['topic']) == 0, "Received data, when not expected for topic:" + theme['topic'] + continue #no more checks needed if data is not available + + if 'expected_fps_in_hz' in theme: + hz = self.node._ros_topic_hz.get_hz(theme['topic']) + if hz == None: + print("Couldn't measure fps, no of data frames expected are enough for the measurement?") + else: + speed= 1e9*hz[0] + msg = "FPS in Hz of topic " + theme['topic'] + " is " + str(speed) + ". Expected is " + str(theme['expected_fps_in_hz']) + print(msg) + if (abs(theme['expected_fps_in_hz']-speed) > theme['expected_fps_in_hz']/10): + assert False,msg 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) From 6922d4db5b56d4d8b6039d8a670aa734091a7904 Mon Sep 17 00:00:00 2001 From: PrasRsRos Date: Thu, 31 Aug 2023 17:47:07 +0530 Subject: [PATCH 02/18] Updated readme and imu tests --- realsense2_camera/test/README.md | 20 +++++++-- .../test/live_camera/test_camera_imu_tests.py | 41 ++++++++----------- .../test/utils/pytest_rs_utils.py | 8 +++- 3 files changed, 39 insertions(+), 30 deletions(-) diff --git a/realsense2_camera/test/README.md b/realsense2_camera/test/README.md index 0be7600c24..a026a36b2e 100644 --- a/realsense2_camera/test/README.md +++ b/realsense2_camera/test/README.md @@ -146,7 +146,21 @@ The below command finds the test with the name test_static_tf_1 in realsense2_ca pytest-3 -s -k test_static_tf_1 realsense2_camera/test/ -### Points to be noted while writing pytests -The tests that are in one file are nromally run in parallel. So if there are multiple tests in one file, the system capacity can influence the test execution. It's recomended to have 3-4 tests in file, more than that can affect the test results due to delays. - +## Points to be noted while writing pytests +The tests that are in one file are normally run in parallel. So if there are multiple tests in one file, the system capacity can influence the test execution. It's recomended to have 3-4 tests in file, more than that can affect the test results due to delays. +### Passing/changing parameters +The parameters passed while creating the node can be initialized individually for each test, please see the test_parameterized_template example for reference. The default values are taken from rs_launch.py and the passed parameters are used for overriding the default values. The parameters that can be dynamically modified can be changed using the param interface provided. However, the function create_param_ifs has to be called to create this interface. Please see the test_d455_basic_tests.py for reference. There are specific functions to change the string, integer and bool parameters, the utils can be extended if any more types are needed. +### Difference in setting the bool parameters +There is a difference between setting the default values of bool parameters and setting them dynamically. +The bool test params are assinged withn quotes as below. + test_params_all_profiles_d455 = { + 'camera_name': 'D455', + 'device_type': 'D455', + 'enable_accel':"True", + 'enable_gyro':"True", + 'unite_imu_method':1, + } + +However the function that implements the setting of bool parameter dynamically takes the python bool datatype. For example: + self.set_bool_param('enable_accel', False) diff --git a/realsense2_camera/test/live_camera/test_camera_imu_tests.py b/realsense2_camera/test/live_camera/test_camera_imu_tests.py index fa173e0a8e..c712cd1ba7 100644 --- a/realsense2_camera/test/live_camera/test_camera_imu_tests.py +++ b/realsense2_camera/test/live_camera/test_camera_imu_tests.py @@ -43,8 +43,8 @@ test_params_all_profiles_d455 = { 'camera_name': 'D455', 'device_type': 'D455', - 'enable_accel':True, - 'enable_gyro':True, + 'enable_accel':"True", + 'enable_gyro':"True", 'unite_imu_method':1, } @@ -64,62 +64,53 @@ def test_LiveCamera_check_motion_sensor(self,launch_descr_with_parameters): if params['unite_imu_method'] == '0': themes[IMU_TOPIC]['expected_data_chunks'] = 0 try: - ''' - initialize, run and check the data - ''' - msg = "Test with the default params " + #initialize self.init_test("RsTest"+params['camera_name']) self.create_param_ifs(get_node_heirarchy(params)) + + + #run with default params and check the data + msg = "Test with the default params " print(msg) - ret = self.run_test(themes) + ret = self.run_test(themes, timeout=50) assert ret[0], msg + str(ret[1]) assert self.process_data(themes), msg + " failed" - msg = "Test with the accel false " self.set_bool_param('enable_accel', False) self.set_bool_param('enable_gyro', True) - ''' - This step fails if the next line is commented - ''' - self.set_integer_param('unite_imu_method', 0) #this shouldn't matter because the unite_imu_method cannot be changed themes[ACCEL_TOPIC]['expected_data_chunks'] = 0 + #uncomment once RSDEV-550 is closed + #themes[IMU_TOPIC]['expected_data_chunks'] = 0 print(msg) ret = self.run_test(themes) assert ret[0], msg + str(ret[1]) assert self.process_data(themes), msg + " failed" - ''' - Test fails - ''' msg = "Test with the gyro false " self.set_bool_param('enable_accel', True) self.set_bool_param('enable_gyro', False) - self.set_integer_param('unite_imu_method', 0) #this shouldn't matter because the unite_imu_method cannot be changed - themes[IMU_TOPIC]['expected_data_chunks'] = 1 + themes[IMU_TOPIC]['expected_data_chunks'] = 0 themes[ACCEL_TOPIC]['expected_data_chunks'] = 1 themes[GYRO_TOPIC]['expected_data_chunks'] = 0 print(msg) + self.spin_for_time(wait_time=1.0) ret = self.run_test(themes) assert ret[0], msg + str(ret[1]) assert self.process_data(themes), msg + " failed" - ''' - Test fails, both gyro and accel data is available, not imu - ''' msg = "Test with both gyro and accel false " self.set_bool_param('enable_accel', False) self.set_bool_param('enable_gyro', False) - self.set_integer_param('unite_imu_method', 1) #this shouldn't matter because the unite_imu_method cannot be changed - + self.set_integer_param('unite_imu_method', 1) + self.spin_for_time(wait_time=1.0) themes[ACCEL_TOPIC]['expected_data_chunks'] = 0 themes[GYRO_TOPIC]['expected_data_chunks'] = 0 - themes[IMU_TOPIC]['expected_data_chunks'] = 1 #Seems that imu data is available even if gyro and accel is disabled + themes[IMU_TOPIC]['expected_data_chunks'] = 0 print(msg) - ret = self.run_test(themes, initial_wait_time=1.0) #wait_time added as test doesn't have to wait for any data + ret = self.run_test(themes, initial_wait_time=1.0) assert ret[0], msg + " failed" assert self.process_data(themes), msg +" failed" - finally: #this step is important because the test will fail next time pytest_rs_utils.kill_realsense2_camera_node() diff --git a/realsense2_camera/test/utils/pytest_rs_utils.py b/realsense2_camera/test/utils/pytest_rs_utils.py index d78eeb9ef8..e693eefc64 100644 --- a/realsense2_camera/test/utils/pytest_rs_utils.py +++ b/realsense2_camera/test/utils/pytest_rs_utils.py @@ -819,9 +819,12 @@ def spin_for_data(self,themes, timeout=5.0): ''' print('Waiting for topic... ' ) flag = False + data_not_expected = [i for i in themes if (i["expected_data_chunks"]) == 0] while (time.time() - start) < timeout: rclpy.spin_once(self.node, timeout_sec=1) debug_print('Spun once... ' ) + if data_not_expected == True: + continue all_found = True for theme in themes: if theme['expected_data_chunks'] > int(self.node.get_num_chunks(theme['topic'])): @@ -831,8 +834,9 @@ def spin_for_data(self,themes, timeout=5.0): flag =True break else: - print("Timed out waiting for", timeout, "seconds" ) - return False, "run_test timedout" + if data_not_expected == False: + print("Timed out waiting for", timeout, "seconds" ) + return False, "run_test timedout" return flag,"" def spin_for_time(self,wait_time): From a20eb25394a4ff705a0e976749400fb9efadd9da Mon Sep 17 00:00:00 2001 From: PrasRsRos Date: Fri, 1 Sep 2023 18:46:04 +0530 Subject: [PATCH 03/18] static_tf fix --- .../test/rosbag/test_rosbag_depth_tests.py | 43 ++++++++----------- .../test/utils/pytest_rs_utils.py | 34 +++++++++++---- 2 files changed, 44 insertions(+), 33 deletions(-) diff --git a/realsense2_camera/test/rosbag/test_rosbag_depth_tests.py b/realsense2_camera/test/rosbag/test_rosbag_depth_tests.py index abbfe28cab..1b628e07f6 100644 --- a/realsense2_camera/test/rosbag/test_rosbag_depth_tests.py +++ b/realsense2_camera/test/rosbag/test_rosbag_depth_tests.py @@ -100,12 +100,10 @@ def process_data(self, themes): test_params_static_tf_1 = {"rosbag_filename":get_rosbag_file_path("outdoors_1color.bag"), - 'camera_name': 'Static_tf_1', + 'camera_name': 'Static_tf1', 'color_width': '0', 'color_height': '0', - 'depth_width': '0', - 'depth_height': '0', - 'infra_width': '0', + "static_tf":True, 'infra_height': '0', 'enable_infra1':'true', 'enable_infra2':'true' @@ -119,43 +117,38 @@ def process_data(self, themes): @pytest.mark.launch(fixture=delayed_launch_descr_with_parameters) class TestStaticTf1(pytest_rs_utils.RsTestBaseClass): def test_static_tf_1(self,delayed_launch_descr_with_parameters): - params = delayed_launch_descr_with_parameters[1] - self.rosbag = params["rosbag_filename"] - data = {('camera_link', 'camera_color_frame'): ([-0.00010158783697988838, 0.014841210097074509, -0.00022671300393994898], [-0.0008337442995980382, 0.0010442184284329414, -0.0009920650627464056, 0.9999986290931702]), - ('camera_link', 'camera_depth_frame'): ([0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0]), - ('camera_link', 'camera_infra1_frame'): ([0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0]), - ('camera_depth_frame', 'camera_infra1_frame'): ([0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0]), - ('camera_depth_frame', 'camera_color_frame'): ([-0.00010158783697988838, 0.014841210097074509, -0.00022671300393994898], [-0.0008337442995980382, 0.0010442184284329414, -0.0009920650627464056, 0.9999986290931702]), - ('camera_infra1_frame', 'camera_color_frame'): ([-0.00010158783697988838, 0.014841210097074509, -0.00022671300393994898], [-0.0008337442995980382, 0.0010442184284329414, -0.0009920650627464056, 0.9999986290931702])} + self.params = delayed_launch_descr_with_parameters[1] + self.rosbag = self.params["rosbag_filename"] themes = [ - {'topic':get_node_heirarchy(params)+'/color/image_raw', + {'topic':get_node_heirarchy(self.params)+'/color/image_raw', 'msg_type':msg_Image, 'expected_data_chunks':1, - 'data':data, + 'static_tf':True, } ] try: ''' initialize, run and check the data ''' - self.init_test("RsTest"+params['camera_name']) + self.init_test("RsTest"+self.params['camera_name']) ret = self.run_test(themes) assert ret[0], ret[1] assert self.process_data(themes) finally: self.shutdown() def process_data(self, themes): - #print ('Gathering static transforms') - frame_ids = ['camera_link', 'camera_depth_frame', 'camera_infra1_frame', 'camera_infra2_frame', 'camera_color_frame', 'camera_fisheye_frame', 'camera_pose'] + expected_data = {(self.params['camera_name']+'_link', self.params['camera_name']+'_color_frame'): ([-0.00010158783697988838, 0.014841210097074509, -0.00022671300393994898], [-0.0008337442995980382, 0.0010442184284329414, -0.0009920650627464056, 0.9999986290931702]), + (self.params['camera_name']+'_link', self.params['camera_name']+'_depth_frame'): ([0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0]), + (self.params['camera_name']+'_link', self.params['camera_name']+'_infra1_frame'): ([0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0]), + (self.params['camera_name']+'_depth_frame', self.params['camera_name']+'_infra1_frame'): ([0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0]), + (self.params['camera_name']+'_depth_frame', self.params['camera_name']+'_color_frame'): ([-0.00010158783697988838, 0.014841210097074509, -0.00022671300393994898], [-0.0008337442995980382, 0.0010442184284329414, -0.0009920650627464056, 0.9999986290931702]), + (self.params['camera_name']+'_infra1_frame', self.params['camera_name']+'_color_frame'): ([-0.00010158783697988838, 0.014841210097074509, -0.00022671300393994898], [-0.0008337442995980382, 0.0010442184284329414, -0.0009920650627464056, 0.9999986290931702])} + frame_ids = [self.params['camera_name']+'_link', self.params['camera_name']+'_depth_frame', self.params['camera_name']+'_infra1_frame', self.params['camera_name']+'_infra2_frame', self.params['camera_name']+'_color_frame', self.params['camera_name']+'_fisheye_frame', self.params['camera_name']+'_pose'] coupled_frame_ids = [xx for xx in itertools.combinations(frame_ids, 2)] - res = {} - for couple in coupled_frame_ids: - from_id, to_id = couple - if (self.node.tfBuffer.can_transform(from_id, to_id, rclpy.time.Time(), rclpy.time.Duration(nanoseconds=3e6))): - res[couple] = self.node.tfBuffer.lookup_transform(from_id, to_id, rclpy.time.Time(), rclpy.time.Duration(nanoseconds=1e6)).transform - else: - res[couple] = None - return pytest_rs_utils.staticTFTest(res, themes[0]["data"]) + tfs_data = self.get_tfs(coupled_frame_ids) + ret = pytest_rs_utils.staticTFTest(tfs_data, expected_data) + assert ret[0], ret[1] + return ret[0] test_params_non_existing_rosbag = {"rosbag_filename":"non_existent.bag", diff --git a/realsense2_camera/test/utils/pytest_rs_utils.py b/realsense2_camera/test/utils/pytest_rs_utils.py index e693eefc64..a30f2fb7a2 100644 --- a/realsense2_camera/test/utils/pytest_rs_utils.py +++ b/realsense2_camera/test/utils/pytest_rs_utils.py @@ -16,8 +16,11 @@ import time from collections import deque import functools +import itertools + import pytest + import numpy as np from launch import LaunchDescription @@ -620,8 +623,7 @@ def reset_data(self, topic): self.frame_counter[topic] = 0 self._ros_topic_hz.restart_topic(topic) - - def create_subscription(self, msg_type, topic , data_type, store_raw_data, measure_hz): + def create_subscription(self, msg_type, topic , data_type, store_raw_data, measure_hz, static_tf): self.reset_data(topic) super().create_subscription(msg_type, topic , self.rsCallback(topic,msg_type, store_raw_data), data_type) #hz measurements are not working @@ -630,9 +632,18 @@ def create_subscription(self, msg_type, topic , data_type, store_raw_data, measu super().create_subscription(msg_class,topic,functools.partial(self._ros_topic_hz._callback_hz, topic=topic),data_type) self._ros_topic_hz.set_last_printed_tn(0, topic=topic) - if (os.getenv('ROS_DISTRO') != "dashing") and (self.tfBuffer == None): + if (static_tf == True) and (self.tfBuffer == None): self.tfBuffer = tf2_ros.Buffer() self.tf_listener = tf2_ros.TransformListener(self.tfBuffer, super()) + def get_tfs(self, coupled_frame_ids): + res = dict() + for couple in coupled_frame_ids: + from_id, to_id = couple + if (self.tfBuffer.can_transform(from_id, to_id, rclpy.time.Time(), rclpy.time.Duration(nanoseconds=3e6))): + res[couple] = self.tfBuffer.lookup_transform(from_id, to_id, rclpy.time.Time(), rclpy.time.Duration(nanoseconds=1e6)).transform + else: + res[couple] = None + return res def get_num_chunks(self,topic): return len(self.data[topic]) @@ -760,9 +771,9 @@ def init_test(self,name='RsTestNode'): def wait_for_node(self, node_name, timeout=8.0): self.node.wait_for_node(node_name, timeout) - def create_subscription(self, msg_type, topic, data_type, store_raw_data=False, measure_hz=False): + def create_subscription(self, msg_type, topic, data_type, store_raw_data=False, measure_hz=False, static_tf=False): if not topic in self.subscribed_topics: - self.node.create_subscription(msg_type, topic, data_type, store_raw_data, measure_hz) + self.node.create_subscription(msg_type, topic, data_type, store_raw_data, measure_hz, static_tf) self.subscribed_topics.append(topic) else: self.node.reset_data(topic) @@ -770,7 +781,6 @@ def create_subscription(self, msg_type, topic, data_type, store_raw_data=False, 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): @@ -849,6 +859,7 @@ def spin_for_time(self,wait_time): def run_test(self, themes, initial_wait_time=0.0, timeout=5.0): try: + static_tf_found = False for theme in themes: store_raw_data = False if 'store_raw_data' in theme: @@ -857,12 +868,17 @@ def run_test(self, themes, initial_wait_time=0.0, timeout=5.0): measure_hz = True else: measure_hz = False + if 'static_tf' in theme: + static_tf = True + static_tf_found = True + else: + static_tf = False - self.create_subscription(theme['msg_type'], theme['topic'] , qos.qos_profile_sensor_data,store_raw_data, measure_hz) + self.create_subscription(theme['msg_type'], theme['topic'] , qos.qos_profile_sensor_data,store_raw_data, measure_hz, static_tf) print('subscription created for ' + theme['topic']) if initial_wait_time != 0.0: self.spin_for_time(initial_wait_time) - self.flag = self.spin_for_data(themes, timeout) + self.flag = self.spin_for_data(themes, timeout) except Exception as e: exc_type, exc_obj, exc_tb = sys.exc_info() fname = os.path.split(exc_tb.tb_frame.f_code.co_filename)[1] @@ -875,6 +891,8 @@ def run_test(self, themes, initial_wait_time=0.0, timeout=5.0): self.flag =False,e return self.flag + def get_tfs(self, coupled_frame_ids): + return self.node.get_tfs(coupled_frame_ids) ''' Please override and use your own process_data if the default check is not suitable. Please also store_raw_data parameter in the themes as True, if you want the From c80e47117cccf88d4e7043eb075468163b8d0952 Mon Sep 17 00:00:00 2001 From: PrasRsRos Date: Tue, 5 Sep 2023 19:30:17 +0530 Subject: [PATCH 04/18] Added tf and tf_static tests --- .../test/live_camera/test_camera_fps_tests.py | 4 +- .../test/live_camera/test_camera_tf_tests.py | 219 ++++++++++++++++++ .../test/utils/pytest_live_camera_utils.py | 17 ++ .../test/utils/pytest_rs_utils.py | 44 ++-- 4 files changed, 269 insertions(+), 15 deletions(-) create mode 100644 realsense2_camera/test/live_camera/test_camera_tf_tests.py diff --git a/realsense2_camera/test/live_camera/test_camera_fps_tests.py b/realsense2_camera/test/live_camera/test_camera_fps_tests.py index cff5cadbb4..291e4eaf8c 100644 --- a/realsense2_camera/test/live_camera/test_camera_fps_tests.py +++ b/realsense2_camera/test/live_camera/test_camera_fps_tests.py @@ -38,7 +38,7 @@ from rcl_interfaces.msg import ParameterValue from rcl_interfaces.srv import SetParameters, GetParameters, ListParameters -test_params_depth_avg_1 = { +test_params_test_fps = { 'camera_name': 'D455', 'device_type': 'D455', } @@ -47,7 +47,7 @@ modified to make it work, see py_rs_utils for more details. To check the fps, a value 'expected_fps_in_hz' has to be added to the corresponding theme ''' -@pytest.mark.parametrize("launch_descr_with_parameters", [test_params_depth_avg_1],indirect=True) +@pytest.mark.parametrize("launch_descr_with_parameters", [test_params_test_fps],indirect=True) @pytest.mark.launch(fixture=launch_descr_with_parameters) class TestCamera_TestFPS(pytest_rs_utils.RsTestBaseClass): def test_camera_test_fps(self,launch_descr_with_parameters): diff --git a/realsense2_camera/test/live_camera/test_camera_tf_tests.py b/realsense2_camera/test/live_camera/test_camera_tf_tests.py new file mode 100644 index 0000000000..10c72c8b09 --- /dev/null +++ b/realsense2_camera/test/live_camera/test_camera_tf_tests.py @@ -0,0 +1,219 @@ +# 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 rclpy.qos import DurabilityPolicy +from rclpy.qos import HistoryPolicy +from rclpy.qos import QoSProfile +import tf2_ros + + +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 +from tf2_msgs.msg import TFMessage as msg_TFMessage + +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 + +import pytest_live_camera_utils +from pytest_rs_utils import get_rosbag_file_path +from pytest_rs_utils import get_node_heirarchy + +from rclpy.parameter import Parameter +from rcl_interfaces.msg import ParameterValue +from rcl_interfaces.srv import SetParameters, GetParameters, ListParameters + +''' +The test was implemented to check the tf_static before and after infra1 was enabled. There shouldn't be any +related data before enabling. +''' +test_params_tf_static_change_d455 = { + 'camera_name': 'D455', + 'device_type': 'D455', + 'enable_infra1': 'false', + 'enable_infra2': 'true', + 'enable_accel': 'true', + 'enable_gyro': 'true', + } +test_params_tf_static_change_d435 = { + 'camera_name': 'D435', + 'device_type': 'D435', + 'enable_infra1': 'false', + 'enable_infra2': 'true', + 'enable_accel': 'true', + 'enable_gyro': 'true', + } + +test_params_tf_static_change_d415 = { + 'camera_name': 'D415', + 'device_type': 'D415', + 'enable_infra1': 'false', + 'enable_infra2': 'true', + 'enable_accel': 'true', + 'enable_gyro': 'true', + } +@pytest.mark.parametrize("launch_descr_with_parameters", [ + pytest.param(test_params_tf_static_change_d455, marks=pytest.mark.d455), + pytest.param(test_params_tf_static_change_d435, marks=pytest.mark.d435), + pytest.param(test_params_tf_static_change_d415, marks=pytest.mark.d415), + ],indirect=True) +@pytest.mark.launch(fixture=launch_descr_with_parameters) +class TestCamera_TestTF_Static_change(pytest_rs_utils.RsTestBaseClass): + def test_camera_test_tf_static_change(self,launch_descr_with_parameters): + self.params = launch_descr_with_parameters[1] + if pytest_live_camera_utils.check_if_camera_connected(self.params['device_type']) == False: + print("Device not found? : " + self.params['device_type']) + return + themes = [ + {'topic':'/tf_static', + 'msg_type':msg_TFMessage, + 'expected_data_chunks':1, + 'qos' : QoSProfile(depth=100,durability=DurabilityPolicy.TRANSIENT_LOCAL,history=HistoryPolicy.KEEP_LAST,)#static + } + ] + try: + ''' + initialize, run and check the data + ''' + self.init_test("RsTest"+self.params['camera_name']) + self.wait_for_node(self.params['camera_name']) + self.create_param_ifs(get_node_heirarchy(self.params)) + ret = self.run_test(themes, timeout=10) + assert ret[0], ret[1] + frame_ids = [self.params['camera_name']+'_link', + self.params['camera_name']+'_depth_frame', + self.params['camera_name']+'_infra1_frame', + self.params['camera_name']+'_infra2_frame', + self.params['camera_name']+'_color_frame'] + if self.params['device_type'] == 'D455': + frame_ids.append(self.params['camera_name']+'_gyro_frame') + frame_ids.append(self.params['camera_name']+'_accel_frame') + + ret = self.process_data(themes, False) + assert ret[0], ret[1] + assert self.set_bool_param('enable_infra1', True) + + ret = self.run_test(themes, timeout=10) + assert ret[0], ret[1] + ret = self.process_data(themes, True) + assert ret[0], ret[1] + finally: + self.shutdown() + def process_data(self, themes, enable_infra1): + frame_ids = [self.params['camera_name']+'_link', + self.params['camera_name']+'_depth_frame', + self.params['camera_name']+'_infra2_frame', + self.params['camera_name']+'_color_frame'] + if self.params['device_type'] == 'D455': + frame_ids.append(self.params['camera_name']+'_gyro_frame') + frame_ids.append(self.params['camera_name']+'_accel_frame') + if enable_infra1: + frame_ids.append(self.params['camera_name']+'_infra1_frame') + + data = self.node.pop_first_chunk('/tf_static') + ret = self.check_transform_data(data, frame_ids, True) + return ret + + +test_params_tf_d455 = { + 'camera_name': 'D455', + 'device_type': 'D455', + 'publish_tf': 'true', + 'tf_publish_rate': '1.1', + } + +test_params_tf_d435 = { + 'camera_name': 'D435', + 'device_type': 'D435', + 'publish_tf': 'true', + 'tf_publish_rate': '1.1', + } + +test_params_tf_d415 = { + 'camera_name': 'D415', + 'device_type': 'D415', + 'publish_tf': 'true', + 'tf_publish_rate': '1.1', + } +@pytest.mark.parametrize("launch_descr_with_parameters", [ + pytest.param(test_params_tf_d455, marks=pytest.mark.d455), + pytest.param(test_params_tf_d435, marks=pytest.mark.d415), + pytest.param(test_params_tf_d415, marks=pytest.mark.d415), + ],indirect=True) +@pytest.mark.launch(fixture=launch_descr_with_parameters) +class TestCamera_TestTF_DYN(pytest_rs_utils.RsTestBaseClass): + def test_camera_test_tf_dyn(self,launch_descr_with_parameters): + self.params = launch_descr_with_parameters[1] + if pytest_live_camera_utils.check_if_camera_connected(self.params['device_type']) == False: + print("Device not found? : " + self.params['device_type']) + return + themes = [ + {'topic':'/tf', + 'msg_type':msg_TFMessage, + 'expected_data_chunks':3, + 'qos' : QoSProfile(depth=100,durability=DurabilityPolicy.VOLATILE,history=HistoryPolicy.KEEP_LAST,) + #'qos' : QoSProfile(depth=100,durability=DurabilityPolicy.TRANSIENT_LOCAL,history=HistoryPolicy.KEEP_LAST,)#static + }, + {'topic':'/tf_static', + 'msg_type':msg_TFMessage, + 'expected_data_chunks':1, + #'qos' : QoSProfile(depth=100,durability=DurabilityPolicy.VOLATILE,history=HistoryPolicy.KEEP_LAST,) + 'qos' : QoSProfile(depth=100,durability=DurabilityPolicy.TRANSIENT_LOCAL,history=HistoryPolicy.KEEP_LAST,)#static + } + ] + try: + ''' + initialize, run and check the data + ''' + self.init_test("RsTest"+self.params['camera_name']) + self.wait_for_node(self.params['camera_name']) + self.create_param_ifs(get_node_heirarchy(self.params)) + ret = self.run_test(themes, timeout=10) + assert ret[0], ret[1] + ret = self.process_data(themes, False) + assert ret[0], ret[1] + finally: + self.shutdown() + + def process_data(self, themes, enable_infra1): + frame_ids = [self.params['camera_name']+'_link', + self.params['camera_name']+'_depth_frame', + self.params['camera_name']+'_color_frame'] + data = self.node.pop_first_chunk('/tf_static') + ret = self.check_transform_data(data, frame_ids, True) + assert ret[0], ret[1] + data = self.node.pop_first_chunk('/tf') + ret = self.check_transform_data(data, frame_ids) + assert ret[0], ret[1] + data = self.node.pop_first_chunk('/tf') + ret = self.check_transform_data(data, frame_ids) + assert ret[0], ret[1] + data = self.node.pop_first_chunk('/tf') + ret = self.check_transform_data(data, frame_ids) + assert ret[0], ret[1] + #return True, "" + + return True, "" diff --git a/realsense2_camera/test/utils/pytest_live_camera_utils.py b/realsense2_camera/test/utils/pytest_live_camera_utils.py index 3a5ab6f600..36ade782d5 100644 --- a/realsense2_camera/test/utils/pytest_live_camera_utils.py +++ b/realsense2_camera/test/utils/pytest_live_camera_utils.py @@ -177,6 +177,23 @@ def get_camera_capabilities(device_type, serial_no=None): return capability return None +def check_if_camera_connected(device_type, serial_no=None): + long_data = os.popen("rs-enumerate-devices -s").read().splitlines() + debug_print(serial_no) + index = 0 + for index in range(len(long_data)): + name_line = long_data[index].split() + if name_line[0] != "Intel": + continue + if name_line[2] != device_type: + continue + if serial_no == None: + return True + if serial_no == name_line[3]: + return True + + return False + if __name__ == '__main__': device_type = 'D455' serial_no = None diff --git a/realsense2_camera/test/utils/pytest_rs_utils.py b/realsense2_camera/test/utils/pytest_rs_utils.py index a30f2fb7a2..05e6847e13 100644 --- a/realsense2_camera/test/utils/pytest_rs_utils.py +++ b/realsense2_camera/test/utils/pytest_rs_utils.py @@ -81,7 +81,7 @@ import requests def debug_print(*args): - if(True): + if(False): print(*args) class RosbagManager(object): @@ -623,7 +623,7 @@ def reset_data(self, topic): self.frame_counter[topic] = 0 self._ros_topic_hz.restart_topic(topic) - def create_subscription(self, msg_type, topic , data_type, store_raw_data, measure_hz, static_tf): + def create_subscription(self, msg_type, topic , data_type, store_raw_data, measure_hz): self.reset_data(topic) super().create_subscription(msg_type, topic , self.rsCallback(topic,msg_type, store_raw_data), data_type) #hz measurements are not working @@ -632,7 +632,7 @@ def create_subscription(self, msg_type, topic , data_type, store_raw_data, measu super().create_subscription(msg_class,topic,functools.partial(self._ros_topic_hz._callback_hz, topic=topic),data_type) self._ros_topic_hz.set_last_printed_tn(0, topic=topic) - if (static_tf == True) and (self.tfBuffer == None): + if self.tfBuffer == None: self.tfBuffer = tf2_ros.Buffer() self.tf_listener = tf2_ros.TransformListener(self.tfBuffer, super()) def get_tfs(self, coupled_frame_ids): @@ -771,9 +771,9 @@ def init_test(self,name='RsTestNode'): def wait_for_node(self, node_name, timeout=8.0): self.node.wait_for_node(node_name, timeout) - def create_subscription(self, msg_type, topic, data_type, store_raw_data=False, measure_hz=False, static_tf=False): + def create_subscription(self, msg_type, topic, data_type, store_raw_data=False, measure_hz=False): if not topic in self.subscribed_topics: - self.node.create_subscription(msg_type, topic, data_type, store_raw_data, measure_hz, static_tf) + self.node.create_subscription(msg_type, topic, data_type, store_raw_data, measure_hz) self.subscribed_topics.append(topic) else: self.node.reset_data(topic) @@ -859,7 +859,6 @@ def spin_for_time(self,wait_time): def run_test(self, themes, initial_wait_time=0.0, timeout=5.0): try: - static_tf_found = False for theme in themes: store_raw_data = False if 'store_raw_data' in theme: @@ -868,13 +867,10 @@ def run_test(self, themes, initial_wait_time=0.0, timeout=5.0): measure_hz = True else: measure_hz = False - if 'static_tf' in theme: - static_tf = True - static_tf_found = True - else: - static_tf = False - - self.create_subscription(theme['msg_type'], theme['topic'] , qos.qos_profile_sensor_data,store_raw_data, measure_hz, static_tf) + qos_type = qos.qos_profile_sensor_data + if 'qos' in theme: + qos_type = theme['qos'] + self.create_subscription(theme['msg_type'], theme['topic'] , qos_type,store_raw_data, measure_hz) print('subscription created for ' + theme['topic']) if initial_wait_time != 0.0: self.spin_for_time(initial_wait_time) @@ -893,6 +889,28 @@ def run_test(self, themes, initial_wait_time=0.0, timeout=5.0): return self.flag def get_tfs(self, coupled_frame_ids): return self.node.get_tfs(coupled_frame_ids) + + + def check_transform_data(self, data, frame_ids, is_static=False): + coupled_frame_ids = [xx for xx in itertools.combinations(frame_ids, 2)] + tfBuffer = tf2_ros.Buffer() + for transform in data.transforms: + if is_static: + tfBuffer.set_transform_static(transform, "default_authority") + else: + tfBuffer.set_transform(transform, "default_authority") + res = dict() + for couple in coupled_frame_ids: + from_id, to_id = couple + if (tfBuffer.can_transform(from_id, to_id, rclpy.time.Time(), rclpy.time.Duration(nanoseconds=3e6))): + res[couple] = tfBuffer.lookup_transform(from_id, to_id, rclpy.time.Time(), rclpy.time.Duration(nanoseconds=1e6)).transform + else: + res[couple] = None + for couple in coupled_frame_ids: + if res[couple] == None: + return False, str(couple) + ": didn't get any tf data" + return True,"" + ''' Please override and use your own process_data if the default check is not suitable. Please also store_raw_data parameter in the themes as True, if you want the From adc75d74c3cf9ed878708e3e058ee8b5b09e0d8c Mon Sep 17 00:00:00 2001 From: PrasRsRos Date: Tue, 5 Sep 2023 21:29:05 +0530 Subject: [PATCH 05/18] Added pointcloud tests for live camera --- .../test_camera_point_cloud_tests.py | 114 ++++++++++++++++++ .../test/live_camera/test_camera_tf_tests.py | 28 ++--- .../test/utils/pytest_rs_utils.py | 7 +- 3 files changed, 133 insertions(+), 16 deletions(-) create mode 100644 realsense2_camera/test/live_camera/test_camera_point_cloud_tests.py diff --git a/realsense2_camera/test/live_camera/test_camera_point_cloud_tests.py b/realsense2_camera/test/live_camera/test_camera_point_cloud_tests.py new file mode 100644 index 0000000000..14def86ca9 --- /dev/null +++ b/realsense2_camera/test/live_camera/test_camera_point_cloud_tests.py @@ -0,0 +1,114 @@ +# 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 rclpy.qos import DurabilityPolicy +from rclpy.qos import HistoryPolicy +from rclpy.qos import QoSProfile +import tf2_ros + + +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 +from tf2_msgs.msg import TFMessage as msg_TFMessage + +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 + +import pytest_live_camera_utils +from pytest_rs_utils import get_rosbag_file_path +from pytest_rs_utils import get_node_heirarchy + +from rclpy.parameter import Parameter +from rcl_interfaces.msg import ParameterValue +from rcl_interfaces.srv import SetParameters, GetParameters, ListParameters + +''' +The test was implemented to check the tf_static before and after infra1 was enabled. There shouldn't be any +related data before enabling. +''' +test_params_points_cloud_d455 = { + 'camera_name': 'D455', + 'device_type': 'D455', + 'pointcloud.enable': 'true' + } +test_params_points_cloud_d435 = { + 'camera_name': 'D435', + 'device_type': 'D435', + 'pointcloud.enable': 'true' + } + +test_params_points_cloud_d415 = { + 'camera_name': 'D415', + 'device_type': 'D415', + 'enable_color':'true', + 'enable_depth':'true', + 'depth_module.profile':'848x480x30', + 'pointcloud.enable': 'true' + } + +''' +This test was ported from rs2_test.py +the command used to run is "python3 realsense2_camera/scripts/rs2_test.py points_cloud_1" +''' +@pytest.mark.parametrize("launch_descr_with_parameters", [ + pytest.param(test_params_points_cloud_d455, marks=pytest.mark.d455), + pytest.param(test_params_points_cloud_d435, marks=pytest.mark.d435), + pytest.param(test_params_points_cloud_d415, marks=pytest.mark.d415), + ],indirect=True) +@pytest.mark.launch(fixture=launch_descr_with_parameters) +class TestCamera_TestPointCloud(pytest_rs_utils.RsTestBaseClass): + def test_camera_test_point_cloud(self,launch_descr_with_parameters): + self.params = launch_descr_with_parameters[1] + if pytest_live_camera_utils.check_if_camera_connected(self.params['device_type']) == False: + print("Device not found? : " + self.params['device_type']) + return + themes = [ + { + 'topic':get_node_heirarchy(self.params)+'/depth/color/points', + 'msg_type':msg_PointCloud2, + 'expected_data_chunks':5, + }, + ] + try: + ''' + initialize, run and check the data + ''' + self.init_test("RsTest"+self.params['camera_name']) + self.wait_for_node(self.params['camera_name']) + self.create_param_ifs(get_node_heirarchy(self.params)) + ret = self.run_test(themes, timeout=10) + assert ret[0], ret[1] + ret = self.process_data(themes, False) + assert ret[0], ret[1] + finally: + self.shutdown() + def process_data(self, themes, enable_infra1): + for count in range(self.node.get_num_chunks(get_node_heirarchy(self.params)+'/depth/color/points')): + data = self.node.pop_first_chunk(get_node_heirarchy(self.params)+'/depth/color/points') + print(data)#the frame counter starts with zero, jumps to 2 and continues. To be checked + return True,"" + diff --git a/realsense2_camera/test/live_camera/test_camera_tf_tests.py b/realsense2_camera/test/live_camera/test_camera_tf_tests.py index 10c72c8b09..4e8250f063 100644 --- a/realsense2_camera/test/live_camera/test_camera_tf_tests.py +++ b/realsense2_camera/test/live_camera/test_camera_tf_tests.py @@ -103,15 +103,7 @@ def test_camera_test_tf_static_change(self,launch_descr_with_parameters): self.create_param_ifs(get_node_heirarchy(self.params)) ret = self.run_test(themes, timeout=10) assert ret[0], ret[1] - frame_ids = [self.params['camera_name']+'_link', - self.params['camera_name']+'_depth_frame', - self.params['camera_name']+'_infra1_frame', - self.params['camera_name']+'_infra2_frame', - self.params['camera_name']+'_color_frame'] - if self.params['device_type'] == 'D455': - frame_ids.append(self.params['camera_name']+'_gyro_frame') - frame_ids.append(self.params['camera_name']+'_accel_frame') - + ret = self.process_data(themes, False) assert ret[0], ret[1] assert self.set_bool_param('enable_infra1', True) @@ -130,12 +122,20 @@ def process_data(self, themes, enable_infra1): if self.params['device_type'] == 'D455': frame_ids.append(self.params['camera_name']+'_gyro_frame') frame_ids.append(self.params['camera_name']+'_accel_frame') - if enable_infra1: - frame_ids.append(self.params['camera_name']+'_infra1_frame') - + frame_ids.append(self.params['camera_name']+'_infra1_frame') data = self.node.pop_first_chunk('/tf_static') - ret = self.check_transform_data(data, frame_ids, True) - return ret + coupled_frame_ids = [xx for xx in itertools.combinations(frame_ids, 2)] + res = self.get_transform_data(data, coupled_frame_ids, is_static=True) + for couple in coupled_frame_ids: + if self.params['camera_name']+'_infra1_frame' in couple: + if enable_infra1 == True and res[couple] != None: + continue + if enable_infra1 == False and res[couple] == None: + continue + return False, str(couple) + ": tf_data not as expected" + if res[couple] == None: + return False, str(couple) + ": didn't get any tf data" + return True,"" test_params_tf_d455 = { diff --git a/realsense2_camera/test/utils/pytest_rs_utils.py b/realsense2_camera/test/utils/pytest_rs_utils.py index 05e6847e13..c07b135065 100644 --- a/realsense2_camera/test/utils/pytest_rs_utils.py +++ b/realsense2_camera/test/utils/pytest_rs_utils.py @@ -891,8 +891,7 @@ def get_tfs(self, coupled_frame_ids): return self.node.get_tfs(coupled_frame_ids) - def check_transform_data(self, data, frame_ids, is_static=False): - coupled_frame_ids = [xx for xx in itertools.combinations(frame_ids, 2)] + def get_transform_data(self, data, coupled_frame_ids, is_static=False): tfBuffer = tf2_ros.Buffer() for transform in data.transforms: if is_static: @@ -906,6 +905,10 @@ def check_transform_data(self, data, frame_ids, is_static=False): res[couple] = tfBuffer.lookup_transform(from_id, to_id, rclpy.time.Time(), rclpy.time.Duration(nanoseconds=1e6)).transform else: res[couple] = None + return res + def check_transform_data(self, data, frame_ids, is_static=False): + coupled_frame_ids = [xx for xx in itertools.combinations(frame_ids, 2)] + res = self.get_transform_data(data, coupled_frame_ids, is_static) for couple in coupled_frame_ids: if res[couple] == None: return False, str(couple) + ": didn't get any tf data" From 163444451afb2c7aadff78885ff3bc3d00809a46 Mon Sep 17 00:00:00 2001 From: PrasRsRos Date: Fri, 8 Sep 2023 16:58:47 +0530 Subject: [PATCH 06/18] Add aligned tests --- realsense2_camera/test/README.md | 11 + .../live_camera/test_camera_aligned_tests.py | 274 ++++++++++++++++++ .../test_camera_all_profile_tests.py | 1 + .../rosbag/test_rosbag_all_topics_test.py | 1 - .../test/utils/pytest_live_camera_utils.py | 28 +- 5 files changed, 300 insertions(+), 15 deletions(-) create mode 100644 realsense2_camera/test/live_camera/test_camera_aligned_tests.py diff --git a/realsense2_camera/test/README.md b/realsense2_camera/test/README.md index a026a36b2e..9624848955 100644 --- a/realsense2_camera/test/README.md +++ b/realsense2_camera/test/README.md @@ -146,6 +146,17 @@ The below command finds the test with the name test_static_tf_1 in realsense2_ca pytest-3 -s -k test_static_tf_1 realsense2_camera/test/ +### Marking tests as regression tests +Some of the tests, especially the live tests with multiple runs, for e.g., all profile tests (test_camera_all_profile_tests.py) take a long time. Such tests are marked are skipped with condition so that "colcon test" skips it. +If a user wants to add a test to this conditional skip, user can add by adding a marker as below. + +@pytest.mark.skipif (os.getenv('RS_ROS_REGRESSION', "not found") == "not found",reason="Regression is not enabled, define RS_ROS_REGRESSION") + +### Running skipped regression tests +1. Set the environment variable RS_ROS_REGRESSION as 1 and run the "colcon test" +2. pytest example: + RS_ROS_REGRESSION=1 PYTHONPATH=$PYTHONPATH:$PWD/realsense2_camera/test/utils:$PWD/realsense2_camera//launch:$PWD/realsense2_camera//scripts pytest-3 -s realsense2_camera/test/live_camera/test_camera_aligned_tests.py -k test_camera_align_depth_color_all -m d415 + ## Points to be noted while writing pytests The tests that are in one file are normally run in parallel. So if there are multiple tests in one file, the system capacity can influence the test execution. It's recomended to have 3-4 tests in file, more than that can affect the test results due to delays. ### Passing/changing parameters diff --git a/realsense2_camera/test/live_camera/test_camera_aligned_tests.py b/realsense2_camera/test/live_camera/test_camera_aligned_tests.py new file mode 100644 index 0000000000..d489b5d546 --- /dev/null +++ b/realsense2_camera/test/live_camera/test_camera_aligned_tests.py @@ -0,0 +1,274 @@ +# 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 pytest_rs_utils import get_node_heirarchy +import pytest_live_camera_utils +from rclpy.parameter import Parameter +from rcl_interfaces.msg import ParameterValue +from rcl_interfaces.srv import SetParameters, GetParameters, ListParameters +from pytest_live_camera_utils import debug_print + + + +test_params_align_depth_color_d455 = { + 'camera_name': 'D455', + 'device_type': 'D455', + 'enable_color':'true', + 'enable_depth':'true', + 'depth_module.profile':'848x480x30', + 'rgb_camera.profile':'640x480x30', + 'align_depth.enable':'true' + } +test_params_align_depth_color_d415 = { + 'camera_name': 'D415', + 'device_type': 'D415', + 'enable_color':'true', + 'enable_depth':'true', + 'depth_module.profile':'848x480x30', + 'rgb_camera.profile':'640x480x30', + 'align_depth.enable':'true' + } +''' +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",[ + pytest.param(test_params_align_depth_color_d455, marks=pytest.mark.d455), + pytest.param(test_params_align_depth_color_d415, marks=pytest.mark.d415), + #pytest.param(test_params_align_depth_color_d435, marks=pytest.mark.d435), + ] + ,indirect=True) +@pytest.mark.launch(fixture=launch_descr_with_parameters) +class TestCamera_AlignDepthColor(pytest_rs_utils.RsTestBaseClass): + def test_camera_align_depth_color(self,launch_descr_with_parameters): + params = launch_descr_with_parameters[1] + themes = [ + {'topic':get_node_heirarchy(params)+'/color/image_raw', + 'msg_type':msg_Image, + 'expected_data_chunks':1, + 'width':640, + 'height':480, + }, + {'topic':get_node_heirarchy(params)+'/depth/image_rect_raw', + 'msg_type':msg_Image, + 'expected_data_chunks':1, + 'width':848, + 'height':480, + }, + {'topic':get_node_heirarchy(params)+'/aligned_depth_to_color/image_raw', + 'msg_type':msg_Image, + 'expected_data_chunks':1, + 'width':640, + 'height':480, + }, + ] + 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)) + ret = self.run_test(themes) + assert ret[0], ret[1] + assert self.process_data(themes) + self.set_string_param('rgb_camera.profile', '1280x720x30') + self.set_bool_param('enable_color', True) + themes[0]['width'] = 1280 + themes[0]['height'] = 720 + themes[2]['width'] = 1280 + themes[2]['height'] = 720 + + ret = self.run_test(themes) + assert ret[0], ret[1] + 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() + + + +test_params_all_profiles_d455 = { + 'camera_name': 'D455', + 'device_type': 'D455', + 'enable_color':'true', + 'enable_depth':'true', + 'depth_module.profile':'848x480x30', + 'rgb_camera.profile':'640x480x30', + 'align_depth.enable':'true' + } +test_params_all_profiles_d415 = { + 'camera_name': 'D415', + 'device_type': 'D415', + 'enable_color':'true', + 'enable_depth':'true', + 'depth_module.profile':'848x480x30', + 'rgb_camera.profile':'640x480x30', + 'align_depth.enable':'true' + } +test_params_all_profiles_d435 = { + 'camera_name': 'D435', + 'device_type': 'D435', + 'enable_color':'true', + 'enable_depth':'true', + 'depth_module.profile':'848x480x30', + 'rgb_camera.profile':'640x480x30', + 'align_depth.enable':'true' + } + + +''' +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.skipif (os.getenv('RS_ROS_REGRESSION', "not found") == "not found",reason="Regression is not enabled, define RS_ROS_REGRESSION") +@pytest.mark.parametrize("launch_descr_with_parameters", [ + pytest.param(test_params_all_profiles_d455, marks=pytest.mark.d455), + pytest.param(test_params_all_profiles_d415, marks=pytest.mark.d415), + pytest.param(test_params_all_profiles_d435, marks=pytest.mark.d435),] + ,indirect=True) +@pytest.mark.launch(fixture=launch_descr_with_parameters) +class TestCamera_AlignDepthColor(pytest_rs_utils.RsTestBaseClass): + def test_camera_align_depth_color_all(self,launch_descr_with_parameters): + skipped_tests = [] + failed_tests = [] + num_passed = 0 + num_failed = 0 + params = launch_descr_with_parameters[1] + themes = [ + {'topic':get_node_heirarchy(params)+'/color/image_raw', + 'msg_type':msg_Image, + 'expected_data_chunks':1, + 'width':640, + 'height':480, + }, + {'topic':get_node_heirarchy(params)+'/depth/image_rect_raw', + 'msg_type':msg_Image, + 'expected_data_chunks':1, + 'width':848, + 'height':480, + }, + {'topic':get_node_heirarchy(params)+'/aligned_depth_to_color/image_raw', + 'msg_type':msg_Image, + 'expected_data_chunks':1, + 'width':640, + 'height':480, + }, + ] + try: + ''' + initialize, run and check the data + ''' + self.init_test("RsTest"+params['camera_name']) + self.spin_for_time(wait_time=1.0) + cap = pytest_live_camera_utils.get_camera_capabilities(params['device_type']) + if cap == None: + debug_print("Device not found? : " + params['device_type']) + return + self.create_param_ifs(get_node_heirarchy(params)) + color_profiles = set([i[1] for i in cap["color_profile"] if i[2] == "RGB8"]) + depth_profiles = set([i[1] for i in cap["depth_profile"] if i[0] == "Depth"]) + for color_profile in color_profiles: + for depth_profile in depth_profiles: + if depth_profile == color_profile: + continue + print("Testing the alignment of Depth:", depth_profile, " and Color:", color_profile) + self.set_bool_param('enable_color', False) + self.set_bool_param('enable_color', False) + self.set_bool_param('align_depth.enable', False) + + themes[0]['width'] = themes[2]['width'] = int(color_profile.split('x')[0]) + themes[0]['height'] = themes[2]['height'] = int(color_profile.split('x')[1]) + themes[1]['width'] = int(depth_profile.split('x')[0]) + themes[1]['height'] = int(depth_profile.split('x')[1]) + dfps = int(depth_profile.split('x')[2]) + cfps = int(color_profile.split('x')[2]) + if dfps > cfps: + fps = cfps + else: + fps = dfps + timeout=100.0/fps + #for the changes to take effect + self.spin_for_time(wait_time=timeout/20) + self.set_string_param('rgb_camera.profile', color_profile) + self.set_string_param('depth_module.profile', depth_profile) + self.set_bool_param('enable_color', True) + self.set_bool_param('enable_color', True) + self.set_bool_param('align_depth.enable', True) + #for the changes to take effect + self.spin_for_time(wait_time=timeout/20) + try: + ret = self.run_test(themes, timeout=timeout) + assert ret[0], ret[1] + assert self.process_data(themes), "".join(str(depth_profile) + " " + str(color_profile)) + " failed" + num_passed += 1 + except Exception as e: + exc_type, exc_obj, exc_tb = sys.exc_info() + fname = os.path.split(exc_tb.tb_frame.f_code.co_filename)[1] + print("Test failed") + print("Tested the alignment of Depth:", depth_profile, " and Color:", color_profile, " with timeout ", timeout) + print(e) + print(exc_type, fname, exc_tb.tb_lineno) + num_failed += 1 + failed_tests.append("".join(str(depth_profile) + " " + str(color_profile))) + + finally: + #this step is important because the test will fail next time + pytest_rs_utils.kill_realsense2_camera_node() + self.shutdown() + print("Tests passed " + str(num_passed)) + print("Tests skipped " + str(len(skipped_tests))) + if len(skipped_tests): + debug_print("\nSkipped tests:" + params['device_type']) + debug_print("\n".join(skipped_tests)) + print("Tests failed " + str(num_failed)) + if len(failed_tests): + print("\nFailed tests:" + params['device_type']) + print("\n".join(failed_tests)) + + def disable_all_params(self): + self.set_bool_param('enable_color', False) + self.set_bool_param('enable_depth', False) + self.set_bool_param('enable_infra', False) + self.set_bool_param('enable_infra1', False) + self.set_bool_param('enable_infra2', False) + diff --git a/realsense2_camera/test/live_camera/test_camera_all_profile_tests.py b/realsense2_camera/test/live_camera/test_camera_all_profile_tests.py index 274ee67625..b337e6f8d5 100644 --- a/realsense2_camera/test/live_camera/test_camera_all_profile_tests.py +++ b/realsense2_camera/test/live_camera/test_camera_all_profile_tests.py @@ -140,6 +140,7 @@ def check_if_skip_test(profile, format): 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.skipif (os.getenv('RS_ROS_REGRESSION', "not found") == "not found",reason="Regression is not enabled, define RS_ROS_REGRESSION") @pytest.mark.parametrize("launch_descr_with_parameters", [ pytest.param(test_params_all_profiles_d455, marks=pytest.mark.d455), pytest.param(test_params_all_profiles_d415, marks=pytest.mark.d415), diff --git a/realsense2_camera/test/rosbag/test_rosbag_all_topics_test.py b/realsense2_camera/test/rosbag/test_rosbag_all_topics_test.py index 34abbc3039..22c4e1caee 100644 --- a/realsense2_camera/test/rosbag/test_rosbag_all_topics_test.py +++ b/realsense2_camera/test/rosbag/test_rosbag_all_topics_test.py @@ -57,7 +57,6 @@ ''' To test all topics published ''' -@pytest.mark.skip @pytest.mark.rosbag @pytest.mark.parametrize("delayed_launch_descr_with_parameters", [test_params_all_topics],indirect=True) @pytest.mark.launch(fixture=delayed_launch_descr_with_parameters) diff --git a/realsense2_camera/test/utils/pytest_live_camera_utils.py b/realsense2_camera/test/utils/pytest_live_camera_utils.py index 36ade782d5..9b5bfeac70 100644 --- a/realsense2_camera/test/utils/pytest_live_camera_utils.py +++ b/realsense2_camera/test/utils/pytest_live_camera_utils.py @@ -48,19 +48,6 @@ def get_default_profiles(cap, profile): debug_print(profile + " default profile2:" + profile2) return profile1,profile2 -def get_camera_capabilities_short(device_type, serial_no=None): - short_data = os.popen("rs-enumerate-devices -s").read().splitlines() - print(serial_no) - for line in short_data: - print(line) - if device_type in line: - if serial_no is None or serial_no == "" : - print(device_type+ " found in " + line) - return - if serial_no in line: - print(device_type + " with serial_no " + serial_no +" found in " + line) - return - print(device_type + " not found") device_info_string = "Device info:" @@ -177,6 +164,20 @@ def get_camera_capabilities(device_type, serial_no=None): return capability return None +def get_camera_capabilities_short(device_type, serial_no=None): + short_data = os.popen("rs-enumerate-devices -s").read().splitlines() + print(serial_no) + for line in short_data: + print(line) + if device_type in line: + if serial_no is None or serial_no == "" : + print(device_type+ " found in " + line) + return + if serial_no in line: + print(device_type + " with serial_no " + serial_no +" found in " + line) + return + print(device_type + " not found") + def check_if_camera_connected(device_type, serial_no=None): long_data = os.popen("rs-enumerate-devices -s").read().splitlines() debug_print(serial_no) @@ -201,7 +202,6 @@ def check_if_camera_connected(device_type, serial_no=None): device_type = sys.argv[1] if len(sys.argv) > 2: serial_no = sys.argv[2] - cap = get_camera_capabilities(device_type, serial_no) print("Capabilities:") print(cap) \ No newline at end of file From af646fb609de96219351fb37b55c3323c0720e9d Mon Sep 17 00:00:00 2001 From: PrasRsRos Date: Fri, 8 Sep 2023 17:36:35 +0530 Subject: [PATCH 07/18] Modified the imu test to remove workaround for RS550 --- realsense2_camera/test/live_camera/test_camera_imu_tests.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/realsense2_camera/test/live_camera/test_camera_imu_tests.py b/realsense2_camera/test/live_camera/test_camera_imu_tests.py index c712cd1ba7..e2defb6b52 100644 --- a/realsense2_camera/test/live_camera/test_camera_imu_tests.py +++ b/realsense2_camera/test/live_camera/test_camera_imu_tests.py @@ -80,8 +80,7 @@ def test_LiveCamera_check_motion_sensor(self,launch_descr_with_parameters): self.set_bool_param('enable_accel', False) self.set_bool_param('enable_gyro', True) themes[ACCEL_TOPIC]['expected_data_chunks'] = 0 - #uncomment once RSDEV-550 is closed - #themes[IMU_TOPIC]['expected_data_chunks'] = 0 + themes[IMU_TOPIC]['expected_data_chunks'] = 0 print(msg) ret = self.run_test(themes) assert ret[0], msg + str(ret[1]) From 139acdea099dac495ccc1f86677e9cef0ea982a5 Mon Sep 17 00:00:00 2001 From: PrasRsRos Date: Fri, 8 Sep 2023 19:28:55 +0530 Subject: [PATCH 08/18] removed failing_test file, was covered in all_profile_tests anyway --- realsense2_camera/CMakeLists.txt | 7 +- .../live_camera/test_camera_failing_tests.py | 268 ------------------ 2 files changed, 1 insertion(+), 274 deletions(-) delete mode 100644 realsense2_camera/test/live_camera/test_camera_failing_tests.py diff --git a/realsense2_camera/CMakeLists.txt b/realsense2_camera/CMakeLists.txt index d8eaf08348..75572d377f 100644 --- a/realsense2_camera/CMakeLists.txt +++ b/realsense2_camera/CMakeLists.txt @@ -330,16 +330,11 @@ if(BUILD_TESTING) OUTPUT_VARIABLE RS_DEVICE_INFO) message(STATUS "rs_device_info:") message(STATUS "${RS_DEVICE_INFO}") - if(RS_DEVICE_INFO MATCHES "D455") + if((RS_DEVICE_INFO MATCHES "D455") OR (RS_DEVICE_INFO MATCHES "D415") OR (RS_DEVICE_INFO MATCHES "D435")) message(STATUS "D455 device found") set(_pytest_live_folders test/live_camera ) - elseif(RS_DEVICE_INFO MATCHES "D415") - message(STATUS "D415 device found") - set(_pytest_live_folders - test/live_camera - ) endif() foreach(test_folder ${_pytest_live_folders}) diff --git a/realsense2_camera/test/live_camera/test_camera_failing_tests.py b/realsense2_camera/test/live_camera/test_camera_failing_tests.py deleted file mode 100644 index 3bdc87b19b..0000000000 --- a/realsense2_camera/test/live_camera/test_camera_failing_tests.py +++ /dev/null @@ -1,268 +0,0 @@ -# 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 pytest_rs_utils import get_node_heirarchy -import pytest_live_camera_utils -from rclpy.parameter import Parameter -from rcl_interfaces.msg import ParameterValue -from rcl_interfaces.srv import SetParameters, GetParameters, ListParameters -from pytest_live_camera_utils import debug_print - -test_params_all_profiles_d455 = { - 'camera_name': 'D455', - 'device_type': 'D455', - } -test_params_all_profiles_d415 = { - 'camera_name': 'D415', - 'device_type': 'D415', - } -''' -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_all_profiles_d415],indirect=True) -@pytest.mark.launch(fixture=launch_descr_with_parameters) -class TestLiveCamera_Change_Resolution(pytest_rs_utils.RsTestBaseClass): - def test_LiveCamera_Change_Resolution(self,launch_descr_with_parameters): - passed_tests = [] - failed_tests = [] - num_passed = 0 - num_failed = 0 - wait_time_s = 1.2 - cap = {} - ''' - need two configurations with different profiles(height & width) for each profile, - this is to ensure the test sets a different profile first, before testing the - actual profile to be tested. - ''' - cap['color_profile'] = [ - ['Color', '1920x1080x30','RGB8'], - ['Color', '1280x720x30','RGB8'], - ] - cap['depth_profile'] = [ - ['Infrared1', '1920x1080x25', 'Y8'], - ['Infrared1', '1920x1080x25', 'Y16'], - ['Infrared1', '1920x1080x15', 'Y16'], - ['Infrared', '848x100x100', 'BGRA8'], - ['Infrared', '640x480x60', 'BGRA8'], - ['Infrared', '640x480x60', 'RGBA8'], - ['Infrared', '640x480x60', 'RGB8'], - ['Infrared', '640x360x60', 'RGBA8'], - ['Infrared', '640x360x60', 'RGB8'], - ['Infrared', '640x360x30', 'UYVY'], - ['Infrared', '480x270x15', 'RGB8'], - ['Infrared', '424x240x60', 'BGRA8'], - ['Infrared', '424x240x30', 'UYVY'], - ['Infrared1', '1920x1080x15', 'Y8'], - ['Infrared1', '1280x720x30', 'Y8'], - ['Infrared1', '1280x720x15', 'Y8'], - ['Infrared1', '1280x720x6', 'Y8'], - ['Infrared1', '960x540x25', 'Y16'], - ['Infrared1', '960x540x15', 'Y16'], - ['Infrared1', '848x480x90', 'Y8'], - ['Infrared1', '848x480x60', 'Y8'], - ['Infrared1', '848x480x30', 'Y8'], - ['Infrared1', '848x480x15', 'Y8'], - ['Infrared1', '848x480x6', 'Y8'], - ['Infrared1', '848x100x100', 'Y8'], - ['Infrared1', '640x480x90', 'Y8'], - ['Infrared1', '640x480x60', 'Y8'], - ['Infrared1', '640x480x30', 'Y8'], - ['Infrared1', '640x480x15', 'Y8'], - ['Infrared1', '640x480x6', 'Y8'], - ['Infrared1', '640x360x90', 'Y8'], - ['Infrared1', '640x360x60', 'Y8'], - ['Infrared1', '640x360x30', 'Y8'], - ['Infrared1', '640x360x15', 'Y8'], - ['Infrared1', '640x360x6', 'Y8'], - ['Infrared1', '480x270x90', 'Y8'], - ['Infrared1', '480x270x60', 'Y8'], - ['Infrared1', '480x270x30', 'Y8'], - ['Infrared1', '480x270x15', 'Y8'], - ['Infrared1', '480x270x6', 'Y8'], - ['Infrared1', '424x240x90', 'Y8'], - ['Infrared1', '424x240x60', 'Y8'], - ['Infrared1', '424x240x30', 'Y8'], - ['Infrared1', '424x240x15', 'Y8'], - ['Infrared1', '424x240x6', 'Y8'], - ['Infrared2', '1920x1080x25', 'Y16'], - ['Infrared2', '1920x1080x25', 'Y8'], - ['Infrared2', '1920x1080x15', 'Y16'], - ['Infrared2', '1920x1080x15', 'Y8'], - ['Infrared2', '1280x720x30', 'Y8'], - ['Infrared2', '1280x720x15', 'Y8'], - ['Infrared2', '1280x720x6', 'Y8'], - ['Infrared2', '960x540x25', 'Y16'], - ['Infrared2', '960x540x15', 'Y16'], - ['Infrared2', '848x480x90', 'Y8'], - ['Infrared2', '848x480x60', 'Y8'], - ['Infrared2', '848x480x30', 'Y8'], - ['Infrared2', '848x480x15', 'Y8'], - ['Infrared2', '848x480x6', 'Y8'], - ['Infrared2', '848x100x100', 'Y8'], - ['Infrared2', '640x480x90', 'Y8'], - ['Infrared2', '640x480x60', 'Y8'], - ['Infrared2', '640x480x30', 'Y8'], - ['Infrared2', '640x480x15', 'Y8'], - ['Infrared2', '640x480x6', 'Y8'], - ['Infrared2', '640x360x90', 'Y8'], - ['Infrared2', '640x360x60', 'Y8'], - ['Infrared2', '640x360x30', 'Y8'], - ['Infrared2', '640x360x15', 'Y8'], - ['Infrared2', '640x360x6', 'Y8'], - ['Infrared2', '480x270x90', 'Y8'], - ['Infrared2', '480x270x60', 'Y8'], - ['Infrared2', '480x270x30', 'Y8'], - ['Infrared2', '480x270x15', 'Y8'], - ['Infrared2', '480x270x6', 'Y8'], - ['Infrared2', '424x240x90', 'Y8'], - ['Infrared2', '424x240x60', 'Y8'], - ['Infrared2', '424x240x30', 'Y8'], - ['Infrared2', '424x240x15', 'Y8'], - ['Infrared2', '424x240x6', 'Y8'], - ['Depth', '640x360x30', 'Z16'], - ['Depth', '480x270x30', 'Z16'], - ['Depth', '424x240x30', 'Z16'], - ] - params = launch_descr_with_parameters[1] - themes = [{'topic':get_node_heirarchy(params)+'/color/image_raw', 'msg_type':msg_Image,'expected_data_chunks':1,'initial_reset':True}] - config = pytest_live_camera_utils.get_profile_config(get_node_heirarchy(params)) - try: - ''' - initialize, run and check the data - ''' - serial_no = None - if 'serial_no' in params: - serial_no = params['serial_no'] - self.init_test("RsTest"+params['camera_name']) - self.spin_for_time(wait_time=1.0) - if cap == None: - debug_print("Device not found? : " + params['device_type']) - return - self.create_param_ifs(get_node_heirarchy(params)) - config["Color"]["default_profile1"],config["Color"]["default_profile2"] = pytest_live_camera_utils.get_default_profiles(cap["color_profile"], "Color") - config["Depth"]["default_profile1"],config["Depth"]["default_profile2"] = pytest_live_camera_utils.get_default_profiles(cap["depth_profile"], "Depth") - config["Infrared"]["default_profile1"],config["Infrared"]["default_profile2"] = pytest_live_camera_utils.get_default_profiles(cap["depth_profile"], "Infrared") - config["Infrared1"]["default_profile1"],config["Infrared1"]["default_profile2"] = pytest_live_camera_utils.get_default_profiles(cap["depth_profile"], "Infrared1") - config["Infrared2"]["default_profile1"],config["Infrared2"]["default_profile2"] = pytest_live_camera_utils.get_default_profiles(cap["depth_profile"], "Infrared2") - for key in cap["color_profile"]: - profile_type = key[0] - profile = key[1] - format = key[2] - print("Testing " + " ".join(key)) - themes[0]['topic'] = config[profile_type]['topic'] - themes[0]['width'] = int(profile.split('x')[0]) - themes[0]['height'] = int(profile.split('x')[1]) - if themes[0]['width'] == int(config[profile_type]["default_profile2"].split('x')[0]): - self.set_string_param(config[profile_type]["profile"], config[profile_type]["default_profile1"]) - else: - self.set_string_param(config[profile_type]["profile"], config[profile_type]["default_profile2"]) - self.set_bool_param(config[profile_type]["param"], True) - self.disable_all_params() - self.spin_for_time(wait_time=wait_time_s) - self.set_string_param(config[profile_type]["profile"], profile) - self.set_string_param(config[profile_type]["format"], format) - self.set_bool_param(config[profile_type]["param"], True) - self.spin_for_time(wait_time=wait_time_s) - try: - ret = self.run_test(themes) - assert ret[0], ret[1] - assert self.process_data(themes), " ".join(key) + " failed" - num_passed += 1 - passed_tests.append(" ".join(key)) - except Exception as e: - exc_type, exc_obj, exc_tb = sys.exc_info() - fname = os.path.split(exc_tb.tb_frame.f_code.co_filename)[1] - print("Test failed") - print(e) - print(exc_type, fname, exc_tb.tb_lineno) - num_failed += 1 - failed_tests.append(" ".join(key)) - debug_print("Color tests completed") - for key in cap["depth_profile"]: - profile_type = key[0] - profile = key[1] - format = key[2] - print("Testing " + " ".join(key)) - - themes[0]['topic'] = config[profile_type]['topic'] - themes[0]['width'] = int(profile.split('x')[0]) - themes[0]['height'] = int(profile.split('x')[1]) - if themes[0]['width'] == int(config[profile_type]["default_profile2"].split('x')[0]): - self.set_string_param(config[profile_type]["profile"], config[profile_type]["default_profile1"]) - else: - self.set_string_param(config[profile_type]["profile"], config[profile_type]["default_profile2"]) - self.set_bool_param(config[profile_type]["param"], True) - - - self.disable_all_params() - self.spin_for_time(wait_time=wait_time_s) - self.set_string_param(config[profile_type]["profile"], profile) - self.set_string_param(config[profile_type]["format"], format) - self.set_bool_param(config[profile_type]["param"], True) - self.spin_for_time(wait_time=wait_time_s) - try: - ret = self.run_test(themes) - assert ret[0], ret[1] - assert self.process_data(themes), " ".join(key) + " failed" - num_passed += 1 - passed_tests.append(" ".join(key)) - except Exception as e: - print("Test failed") - print(e) - num_failed += 1 - failed_tests.append(" ".join(key)) - debug_print("Depth tests completed") - finally: - #this step is important because the test will fail next time - pytest_rs_utils.kill_realsense2_camera_node() - self.shutdown() - print("Tests passed " + str(num_passed)) - if len(passed_tests): - debug_print("\nPassed tests:" + params['device_type']) - debug_print("\n".join(passed_tests)) - print("Tests failed " + str(num_failed)) - if len(failed_tests): - print("\nFailed tests:" + params['device_type']) - print("\n".join(failed_tests)) - - def disable_all_params(self): - self.set_bool_param('enable_color', False) - self.set_bool_param('enable_depth', False) - self.set_bool_param('enable_infra', False) - self.set_bool_param('enable_infra1', False) - self.set_bool_param('enable_infra2', False) \ No newline at end of file From 348dec3bfd2acc195aa188545b4b0eb188a183c7 Mon Sep 17 00:00:00 2001 From: PrasRsRos Date: Mon, 11 Sep 2023 17:28:12 +0530 Subject: [PATCH 09/18] All topics may need more time in CI --- realsense2_camera/test/rosbag/test_rosbag_all_topics_test.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/realsense2_camera/test/rosbag/test_rosbag_all_topics_test.py b/realsense2_camera/test/rosbag/test_rosbag_all_topics_test.py index 22c4e1caee..0e77f552ee 100644 --- a/realsense2_camera/test/rosbag/test_rosbag_all_topics_test.py +++ b/realsense2_camera/test/rosbag/test_rosbag_all_topics_test.py @@ -52,7 +52,7 @@ 'infra_height': '0', 'enable_infra1':'true', 'enable_infra2':'true', - #'align_depth.enable':'true', + 'align_depth.enable':'true', } ''' To test all topics published @@ -99,7 +99,7 @@ def test_all_topics(self,delayed_launch_descr_with_parameters): initialize, run and check the data ''' self.init_test("RsTest"+params['camera_name']) - ret = self.run_test(themes) + ret = self.run_test(themes, timeout=25.0) assert ret[0], ret[1] time.sleep(0.5) assert self.process_data(themes), "Data check failed, probably the rosbag file changed?" From 0446d7c7c0886d9d3a2905c232e5ceb466dbc19b Mon Sep 17 00:00:00 2001 From: PrasRsRos Date: Mon, 11 Sep 2023 17:51:57 +0530 Subject: [PATCH 10/18] All topics testing --- realsense2_camera/test/utils/pytest_rs_utils.py | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/realsense2_camera/test/utils/pytest_rs_utils.py b/realsense2_camera/test/utils/pytest_rs_utils.py index c07b135065..291ea61b16 100644 --- a/realsense2_camera/test/utils/pytest_rs_utils.py +++ b/realsense2_camera/test/utils/pytest_rs_utils.py @@ -822,7 +822,6 @@ def set_integer_param(self, param_name, param_value): return self.send_param(req) def spin_for_data(self,themes, timeout=5.0): - start = time.time() ''' timeout value varies depending upon the system, it needs to be more if the access is over the network @@ -830,6 +829,7 @@ def spin_for_data(self,themes, timeout=5.0): print('Waiting for topic... ' ) flag = False data_not_expected = [i for i in themes if (i["expected_data_chunks"]) == 0] + start = time.time() while (time.time() - start) < timeout: rclpy.spin_once(self.node, timeout_sec=1) debug_print('Spun once... ' ) @@ -847,7 +847,9 @@ def spin_for_data(self,themes, timeout=5.0): if data_not_expected == False: print("Timed out waiting for", timeout, "seconds" ) return False, "run_test timedout" - return flag,"" + if flag: + return flag,"" + return flag,"Unexpected error in spin_for_data" def spin_for_time(self,wait_time): start = time.time() @@ -885,8 +887,8 @@ def run_test(self, themes, initial_wait_time=0.0, timeout=5.0): else: print(e) self.flag =False,e - return self.flag + def get_tfs(self, coupled_frame_ids): return self.node.get_tfs(coupled_frame_ids) From 5f7ec208e111358a70b6703d40b93866ae7f508f Mon Sep 17 00:00:00 2001 From: PrasRsRos Date: Mon, 11 Sep 2023 18:20:44 +0530 Subject: [PATCH 11/18] All topics testing1 --- realsense2_camera/test/utils/pytest_rs_utils.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/realsense2_camera/test/utils/pytest_rs_utils.py b/realsense2_camera/test/utils/pytest_rs_utils.py index 291ea61b16..2d12903ed9 100644 --- a/realsense2_camera/test/utils/pytest_rs_utils.py +++ b/realsense2_camera/test/utils/pytest_rs_utils.py @@ -830,6 +830,7 @@ def spin_for_data(self,themes, timeout=5.0): flag = False data_not_expected = [i for i in themes if (i["expected_data_chunks"]) == 0] start = time.time() + msg = "Unexpected error in spin_for_data" while (time.time() - start) < timeout: rclpy.spin_once(self.node, timeout_sec=1) debug_print('Spun once... ' ) @@ -847,9 +848,10 @@ def spin_for_data(self,themes, timeout=5.0): if data_not_expected == False: print("Timed out waiting for", timeout, "seconds" ) return False, "run_test timedout" + flag = True if flag: return flag,"" - return flag,"Unexpected error in spin_for_data" + return flag,msg def spin_for_time(self,wait_time): start = time.time() From 426776b8d1525620997e3730fc04e040d5b45ce8 Mon Sep 17 00:00:00 2001 From: PrasRsRos Date: Mon, 11 Sep 2023 18:41:32 +0530 Subject: [PATCH 12/18] All topics testing2 --- realsense2_camera/test/utils/pytest_rs_utils.py | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/realsense2_camera/test/utils/pytest_rs_utils.py b/realsense2_camera/test/utils/pytest_rs_utils.py index 2d12903ed9..8326ce5592 100644 --- a/realsense2_camera/test/utils/pytest_rs_utils.py +++ b/realsense2_camera/test/utils/pytest_rs_utils.py @@ -830,7 +830,7 @@ def spin_for_data(self,themes, timeout=5.0): flag = False data_not_expected = [i for i in themes if (i["expected_data_chunks"]) == 0] start = time.time() - msg = "Unexpected error in spin_for_data" + msg = "" while (time.time() - start) < timeout: rclpy.spin_once(self.node, timeout_sec=1) debug_print('Spun once... ' ) @@ -848,9 +848,11 @@ def spin_for_data(self,themes, timeout=5.0): if data_not_expected == False: print("Timed out waiting for", timeout, "seconds" ) return False, "run_test timedout" - flag = True - if flag: - return flag,"" + if flag ==False: + for theme in themes: + if theme['expected_data_chunks'] > int(self.node.get_num_chunks(theme['topic'])): + msg = "Data expected, but not received for: " + theme['topic'] + break return flag,msg def spin_for_time(self,wait_time): From 015bd4a181f05c81c2c32e8ab837c8f6a70e7e3a Mon Sep 17 00:00:00 2001 From: PrasRsRos Date: Mon, 11 Sep 2023 20:28:31 +0530 Subject: [PATCH 13/18] All topics testing3 --- .../rosbag/test_rosbag_all_topics_test.py | 8 +------- .../test/utils/pytest_rs_utils.py | 20 ++++++++++--------- 2 files changed, 12 insertions(+), 16 deletions(-) diff --git a/realsense2_camera/test/rosbag/test_rosbag_all_topics_test.py b/realsense2_camera/test/rosbag/test_rosbag_all_topics_test.py index 0e77f552ee..e462e91b57 100644 --- a/realsense2_camera/test/rosbag/test_rosbag_all_topics_test.py +++ b/realsense2_camera/test/rosbag/test_rosbag_all_topics_test.py @@ -44,15 +44,9 @@ test_params_all_topics = {"rosbag_filename":get_rosbag_file_path("outdoors_1color.bag"), 'camera_name': 'AllTopics', - 'color_width': '0', - 'color_height': '0', - 'depth_width': '0', - 'depth_height': '0', - 'infra_width': '0', - 'infra_height': '0', 'enable_infra1':'true', 'enable_infra2':'true', - 'align_depth.enable':'true', + #'align_depth.enable':'true', } ''' To test all topics published diff --git a/realsense2_camera/test/utils/pytest_rs_utils.py b/realsense2_camera/test/utils/pytest_rs_utils.py index 8326ce5592..810007f4ba 100644 --- a/realsense2_camera/test/utils/pytest_rs_utils.py +++ b/realsense2_camera/test/utils/pytest_rs_utils.py @@ -676,7 +676,7 @@ def image_msg_to_numpy(self, data): def rsCallback(self, topic, msg_type, store_raw_data): debug_print("RSCallback") def _rsCallback(data): - debug_print('Got the callback for ' + topic) + print('Got the callback for ' + topic) #print(data.header) self.flag = True if store_raw_data == True: @@ -828,7 +828,11 @@ def spin_for_data(self,themes, timeout=5.0): ''' print('Waiting for topic... ' ) flag = False - data_not_expected = [i for i in themes if (i["expected_data_chunks"]) == 0] + data_not_expected1 = [i for i in themes if (i["expected_data_chunks"]) == 0] + if data_not_expected1 == []: + data_not_expected = False + else: + data_not_expected = True start = time.time() msg = "" while (time.time() - start) < timeout: @@ -846,13 +850,11 @@ def spin_for_data(self,themes, timeout=5.0): break else: if data_not_expected == False: - print("Timed out waiting for", timeout, "seconds" ) - return False, "run_test timedout" - if flag ==False: - for theme in themes: - if theme['expected_data_chunks'] > int(self.node.get_num_chunks(theme['topic'])): - msg = "Data expected, but not received for: " + theme['topic'] - break + msg = "Timedout: Data expected, but not received for: " + for theme in themes: + if theme['expected_data_chunks'] > int(self.node.get_num_chunks(theme['topic'])): + msg += " " + theme['topic'] + return False, msg return flag,msg def spin_for_time(self,wait_time): From 7235d999b999c2844f01c88b696ed0a802dae3b1 Mon Sep 17 00:00:00 2001 From: PrasRsRos Date: Mon, 11 Sep 2023 20:48:13 +0530 Subject: [PATCH 14/18] All topics testing4 --- realsense2_camera/test/utils/pytest_rs_utils.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/realsense2_camera/test/utils/pytest_rs_utils.py b/realsense2_camera/test/utils/pytest_rs_utils.py index 810007f4ba..578d27b469 100644 --- a/realsense2_camera/test/utils/pytest_rs_utils.py +++ b/realsense2_camera/test/utils/pytest_rs_utils.py @@ -515,7 +515,7 @@ def get_rs_node_description(params): executable='realsense2_camera_node', parameters=[tmp_yaml.name], output='screen', - arguments=['--ros-args', '--log-level', "info"], + arguments=['--ros-args', '--log-level', "debug"], emulate_tty=True, ) From e2cd2410d5cf4538e23eb3f5c9045d74766b9abe Mon Sep 17 00:00:00 2001 From: PrasRsRos Date: Mon, 11 Sep 2023 22:28:30 +0530 Subject: [PATCH 15/18] All topics added to regression only, excluded from CI --- .../test/rosbag/test_rosbag_all_topics_test.py | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/realsense2_camera/test/rosbag/test_rosbag_all_topics_test.py b/realsense2_camera/test/rosbag/test_rosbag_all_topics_test.py index e462e91b57..3c5bdb83ac 100644 --- a/realsense2_camera/test/rosbag/test_rosbag_all_topics_test.py +++ b/realsense2_camera/test/rosbag/test_rosbag_all_topics_test.py @@ -51,7 +51,15 @@ ''' To test all topics published ''' +''' +To test all topics published + +The test doesn't work in CI due to sync. The unlike the live camera, rosbag node publishes the extrinsics only once, +not every time the subscription is created. The CI due to limited resource can start the ros node much earlier than +the testcase, hence missing the published data by the test +''' @pytest.mark.rosbag +@pytest.mark.skipif (os.getenv('RS_ROS_REGRESSION', "not found") == "not found",reason="The test doesn't work in CI") @pytest.mark.parametrize("delayed_launch_descr_with_parameters", [test_params_all_topics],indirect=True) @pytest.mark.launch(fixture=delayed_launch_descr_with_parameters) class TestAllTopics(pytest_rs_utils.RsTestBaseClass): From aef295123f95737564da626c4e0b4e9bdb5f441e Mon Sep 17 00:00:00 2001 From: PrasRsRos Date: Thu, 14 Sep 2023 17:17:03 +0530 Subject: [PATCH 16/18] updated tests for the failures in CI --- .../live_camera/test_camera_aligned_tests.py | 13 +++-- .../test/live_camera/test_camera_fps_tests.py | 30 ++++++++++-- .../test/live_camera/test_camera_imu_tests.py | 1 + .../rosbag/test_rosbag_all_topics_test.py | 4 +- .../test/rosbag/test_rosbag_depth_tests.py | 19 +++++--- .../test/utils/pytest_rs_utils.py | 48 ++++++++++++------- 6 files changed, 80 insertions(+), 35 deletions(-) diff --git a/realsense2_camera/test/live_camera/test_camera_aligned_tests.py b/realsense2_camera/test/live_camera/test_camera_aligned_tests.py index d489b5d546..0c17e76e47 100644 --- a/realsense2_camera/test/live_camera/test_camera_aligned_tests.py +++ b/realsense2_camera/test/live_camera/test_camera_aligned_tests.py @@ -66,6 +66,7 @@ 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",[ pytest.param(test_params_align_depth_color_d455, marks=pytest.mark.d455), pytest.param(test_params_align_depth_color_d415, marks=pytest.mark.d415), @@ -76,6 +77,9 @@ class TestCamera_AlignDepthColor(pytest_rs_utils.RsTestBaseClass): def test_camera_align_depth_color(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 themes = [ {'topic':get_node_heirarchy(params)+'/color/image_raw', 'msg_type':msg_Image, @@ -122,8 +126,6 @@ def test_camera_align_depth_color(self,launch_descr_with_parameters): pytest_rs_utils.kill_realsense2_camera_node() self.shutdown() - - test_params_all_profiles_d455 = { 'camera_name': 'D455', 'device_type': 'D455', @@ -167,13 +169,16 @@ def test_camera_align_depth_color(self,launch_descr_with_parameters): pytest.param(test_params_all_profiles_d435, marks=pytest.mark.d435),] ,indirect=True) @pytest.mark.launch(fixture=launch_descr_with_parameters) -class TestCamera_AlignDepthColor(pytest_rs_utils.RsTestBaseClass): - def test_camera_align_depth_color_all(self,launch_descr_with_parameters): +class TestCamera_AllAlignDepthColor(pytest_rs_utils.RsTestBaseClass): + def test_camera_all_align_depth_color(self,launch_descr_with_parameters): skipped_tests = [] failed_tests = [] num_passed = 0 num_failed = 0 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 themes = [ {'topic':get_node_heirarchy(params)+'/color/image_raw', 'msg_type':msg_Image, diff --git a/realsense2_camera/test/live_camera/test_camera_fps_tests.py b/realsense2_camera/test/live_camera/test_camera_fps_tests.py index 291e4eaf8c..31b2d31187 100644 --- a/realsense2_camera/test/live_camera/test_camera_fps_tests.py +++ b/realsense2_camera/test/live_camera/test_camera_fps_tests.py @@ -38,16 +38,35 @@ from rcl_interfaces.msg import ParameterValue from rcl_interfaces.srv import SetParameters, GetParameters, ListParameters -test_params_test_fps = { +test_params_test_fps_d455 = { 'camera_name': 'D455', 'device_type': 'D455', + + } +test_params_test_fps_d415 = { + 'camera_name': 'D415', + 'device_type': 'D415', + } +test_profiles ={ + 'D455':{ + 'depth_test_profiles':['640x480x5','640x480x15', '640x480x30', '640x480x90'], + 'color_test_profiles':['640x480x5','640x480x15', '640x480x30', '1280x720x30'] + }, + 'D415':{ + 'depth_test_profiles':['640x480x6','640x480x15', '640x480x30', '640x480x90'], + 'color_test_profiles':['640x480x6','640x480x15', '640x480x30', '1280x720x30'] } +} ''' The test was implemented to check the fps of Depth and Color frames. The RosTopicHz had to be modified to make it work, see py_rs_utils for more details. To check the fps, a value 'expected_fps_in_hz' has to be added to the corresponding theme ''' -@pytest.mark.parametrize("launch_descr_with_parameters", [test_params_test_fps],indirect=True) +@pytest.mark.parametrize("launch_descr_with_parameters", [ + pytest.param(test_params_test_fps_d455, marks=pytest.mark.d455), + pytest.param(test_params_test_fps_d415, marks=pytest.mark.d415), + #pytest.param(test_params_test_fps_d435, marks=pytest.mark.d435), + ],indirect=True) @pytest.mark.launch(fixture=launch_descr_with_parameters) class TestCamera_TestFPS(pytest_rs_utils.RsTestBaseClass): def test_camera_test_fps(self,launch_descr_with_parameters): @@ -68,12 +87,14 @@ def test_camera_test_fps(self,launch_descr_with_parameters): 'expected_data_chunks':100, } ] - profiles = ['640x480x5','640x480x15', '640x480x30', '640x480x90'] + profiles = test_profiles[params['camera_name']]['depth_test_profiles'] + assert self.set_bool_param('enable_color', False) for profile in profiles: print("Testing profile: ", profile) themes[0]['expected_fps_in_hz'] = float(profile.split('x')[2]) assert self.set_string_param('depth_module.profile', profile) assert self.set_bool_param('enable_depth', True) + self.spin_for_time(0.5) ret = self.run_test(themes, timeout=25.0) assert ret[0], ret[1] assert self.process_data(themes) @@ -84,7 +105,8 @@ def test_camera_test_fps(self,launch_descr_with_parameters): 'expected_data_chunks':100, } ] - profiles = ['640x480x5','640x480x15', '640x480x30', '1280x720x30'] + assert self.set_bool_param('enable_depth', False) + profiles = test_profiles[params['camera_name']]['color_test_profiles'] for profile in profiles: print("Testing profile: ", profile) themes[0]['expected_fps_in_hz'] = float(profile.split('x')[2]) diff --git a/realsense2_camera/test/live_camera/test_camera_imu_tests.py b/realsense2_camera/test/live_camera/test_camera_imu_tests.py index e2defb6b52..317833e331 100644 --- a/realsense2_camera/test/live_camera/test_camera_imu_tests.py +++ b/realsense2_camera/test/live_camera/test_camera_imu_tests.py @@ -77,6 +77,7 @@ def test_LiveCamera_check_motion_sensor(self,launch_descr_with_parameters): assert self.process_data(themes), msg + " failed" msg = "Test with the accel false " + self.set_integer_param('unite_imu_method', 0) self.set_bool_param('enable_accel', False) self.set_bool_param('enable_gyro', True) themes[ACCEL_TOPIC]['expected_data_chunks'] = 0 diff --git a/realsense2_camera/test/rosbag/test_rosbag_all_topics_test.py b/realsense2_camera/test/rosbag/test_rosbag_all_topics_test.py index 3c5bdb83ac..9cda836f3a 100644 --- a/realsense2_camera/test/rosbag/test_rosbag_all_topics_test.py +++ b/realsense2_camera/test/rosbag/test_rosbag_all_topics_test.py @@ -46,7 +46,7 @@ 'camera_name': 'AllTopics', 'enable_infra1':'true', 'enable_infra2':'true', - #'align_depth.enable':'true', + 'align_depth.enable':'true', } ''' To test all topics published @@ -101,7 +101,7 @@ def test_all_topics(self,delayed_launch_descr_with_parameters): initialize, run and check the data ''' self.init_test("RsTest"+params['camera_name']) - ret = self.run_test(themes, timeout=25.0) + ret = self.run_test(themes) assert ret[0], ret[1] time.sleep(0.5) assert self.process_data(themes), "Data check failed, probably the rosbag file changed?" diff --git a/realsense2_camera/test/rosbag/test_rosbag_depth_tests.py b/realsense2_camera/test/rosbag/test_rosbag_depth_tests.py index 1b628e07f6..51d574da85 100644 --- a/realsense2_camera/test/rosbag/test_rosbag_depth_tests.py +++ b/realsense2_camera/test/rosbag/test_rosbag_depth_tests.py @@ -20,10 +20,14 @@ import pytest import rclpy +from rclpy.qos import DurabilityPolicy +from rclpy.qos import HistoryPolicy +from rclpy.qos import QoSProfile 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 +from tf2_msgs.msg import TFMessage as msg_TFMessage import numpy as np @@ -78,12 +82,6 @@ def test_depth_points_cloud_1(self,delayed_launch_descr_with_parameters): 'expected_data_chunks':1, 'data':data1 }, - {'topic':get_node_heirarchy(params)+'/depth/image_rect_raw', - 'msg_type':msg_Image, - 'expected_data_chunks':1, - 'data':data2 - } - ] try: ''' @@ -124,6 +122,11 @@ def test_static_tf_1(self,delayed_launch_descr_with_parameters): 'msg_type':msg_Image, 'expected_data_chunks':1, 'static_tf':True, + }, + {'topic':'/tf_static', + 'msg_type':msg_TFMessage, + 'expected_data_chunks':1, + 'qos' : QoSProfile(depth=100,durability=DurabilityPolicy.TRANSIENT_LOCAL,history=HistoryPolicy.KEEP_LAST,)#static } ] try: @@ -145,7 +148,9 @@ def process_data(self, themes): (self.params['camera_name']+'_infra1_frame', self.params['camera_name']+'_color_frame'): ([-0.00010158783697988838, 0.014841210097074509, -0.00022671300393994898], [-0.0008337442995980382, 0.0010442184284329414, -0.0009920650627464056, 0.9999986290931702])} frame_ids = [self.params['camera_name']+'_link', self.params['camera_name']+'_depth_frame', self.params['camera_name']+'_infra1_frame', self.params['camera_name']+'_infra2_frame', self.params['camera_name']+'_color_frame', self.params['camera_name']+'_fisheye_frame', self.params['camera_name']+'_pose'] coupled_frame_ids = [xx for xx in itertools.combinations(frame_ids, 2)] - tfs_data = self.get_tfs(coupled_frame_ids) + data = self.node.pop_first_chunk('/tf_static') + coupled_frame_ids = [xx for xx in itertools.combinations(frame_ids, 2)] + tfs_data = self.get_transform_data(data, coupled_frame_ids, is_static=True) ret = pytest_rs_utils.staticTFTest(tfs_data, expected_data) assert ret[0], ret[1] return ret[0] diff --git a/realsense2_camera/test/utils/pytest_rs_utils.py b/realsense2_camera/test/utils/pytest_rs_utils.py index 578d27b469..db9c09c3c0 100644 --- a/realsense2_camera/test/utils/pytest_rs_utils.py +++ b/realsense2_camera/test/utils/pytest_rs_utils.py @@ -17,6 +17,7 @@ from collections import deque import functools import itertools +import subprocess import pytest @@ -443,6 +444,17 @@ def kill_realsense2_camera_node(): os.system(cmd) pass +''' +function gets all the topics for a camera node +''' + +def get_all_topics(camera_name=None): + cmd = 'ros2 topic list' + if camera_name!=None: + cmd += '| grep ' + camera_name + direct_output = os.popen(cmd).read() + return direct_output + ''' get the default parameters from the launch script so that the test doesn't have to get updated for each change to the parameter or default values @@ -511,11 +523,11 @@ def get_rs_node_description(params): #name=LaunchConfiguration("camera_name"), namespace=params["camera_namespace"], name=params["camera_name"], - #prefix=['xterm -e gdb --args'], + #prefix=['xterm -e gdb -ex=run --args'], executable='realsense2_camera_node', parameters=[tmp_yaml.name], output='screen', - arguments=['--ros-args', '--log-level', "debug"], + arguments=['--ros-args', '--log-level', "info"], emulate_tty=True, ) @@ -635,16 +647,6 @@ def create_subscription(self, msg_type, topic , data_type, store_raw_data, measu if self.tfBuffer == None: self.tfBuffer = tf2_ros.Buffer() self.tf_listener = tf2_ros.TransformListener(self.tfBuffer, super()) - def get_tfs(self, coupled_frame_ids): - res = dict() - for couple in coupled_frame_ids: - from_id, to_id = couple - if (self.tfBuffer.can_transform(from_id, to_id, rclpy.time.Time(), rclpy.time.Duration(nanoseconds=3e6))): - res[couple] = self.tfBuffer.lookup_transform(from_id, to_id, rclpy.time.Time(), rclpy.time.Duration(nanoseconds=1e6)).transform - else: - res[couple] = None - return res - def get_num_chunks(self,topic): return len(self.data[topic]) @@ -676,7 +678,10 @@ def image_msg_to_numpy(self, data): def rsCallback(self, topic, msg_type, store_raw_data): debug_print("RSCallback") def _rsCallback(data): - print('Got the callback for ' + topic) + ''' + enabling prints in callback reduces the fps in some cases + ''' + debug_print('Got the callback for ' + topic) #print(data.header) self.flag = True if store_raw_data == True: @@ -854,7 +859,9 @@ def spin_for_data(self,themes, timeout=5.0): for theme in themes: if theme['expected_data_chunks'] > int(self.node.get_num_chunks(theme['topic'])): msg += " " + theme['topic'] + msg += " Nodes available: " + str(self.node.get_node_names()) return False, msg + flag = True return flag,msg def spin_for_time(self,wait_time): @@ -865,7 +872,7 @@ def spin_for_time(self,wait_time): print('Spun for time once... ' ) rclpy.spin_once(self.node, timeout_sec=wait_time) - def run_test(self, themes, initial_wait_time=0.0, timeout=5.0): + def run_test(self, themes, initial_wait_time=0.0, timeout=0): try: for theme in themes: store_raw_data = False @@ -880,6 +887,15 @@ def run_test(self, themes, initial_wait_time=0.0, timeout=5.0): qos_type = theme['qos'] self.create_subscription(theme['msg_type'], theme['topic'] , qos_type,store_raw_data, measure_hz) print('subscription created for ' + theme['topic']) + ''' + change the default based on whether data is expected or not + ''' + if timeout == 0: + timeout = 5.0 + data_not_expected1 = [i for i in themes if (i["expected_data_chunks"]) == 0] + if data_not_expected1 == []: + timeout = 50.0 #high value due to resource constraints in CI + if initial_wait_time != 0.0: self.spin_for_time(initial_wait_time) self.flag = self.spin_for_data(themes, timeout) @@ -895,10 +911,6 @@ def run_test(self, themes, initial_wait_time=0.0, timeout=5.0): self.flag =False,e return self.flag - def get_tfs(self, coupled_frame_ids): - return self.node.get_tfs(coupled_frame_ids) - - def get_transform_data(self, data, coupled_frame_ids, is_static=False): tfBuffer = tf2_ros.Buffer() for transform in data.transforms: From e861b350930f313eaae657ef73d1711921a82738 Mon Sep 17 00:00:00 2001 From: PrasRsRos Date: Thu, 14 Sep 2023 17:40:12 +0530 Subject: [PATCH 17/18] correct the d435 marker --- realsense2_camera/test/live_camera/test_camera_tf_tests.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/realsense2_camera/test/live_camera/test_camera_tf_tests.py b/realsense2_camera/test/live_camera/test_camera_tf_tests.py index 4e8250f063..a31f74e77c 100644 --- a/realsense2_camera/test/live_camera/test_camera_tf_tests.py +++ b/realsense2_camera/test/live_camera/test_camera_tf_tests.py @@ -160,7 +160,7 @@ def process_data(self, themes, enable_infra1): } @pytest.mark.parametrize("launch_descr_with_parameters", [ pytest.param(test_params_tf_d455, marks=pytest.mark.d455), - pytest.param(test_params_tf_d435, marks=pytest.mark.d415), + pytest.param(test_params_tf_d435, marks=pytest.mark.d435), pytest.param(test_params_tf_d415, marks=pytest.mark.d415), ],indirect=True) @pytest.mark.launch(fixture=launch_descr_with_parameters) From de0100539e7e8cf5fc9bf4899e7fdfb47c002dc9 Mon Sep 17 00:00:00 2001 From: PrasRsRos Date: Fri, 15 Sep 2023 16:23:33 +0530 Subject: [PATCH 18/18] added markers for d415 d455 specific tests --- realsense2_camera/test/README.md | 2 +- realsense2_camera/test/live_camera/test_d415_basic_tests.py | 1 + realsense2_camera/test/live_camera/test_d455_basic_tests.py | 1 + 3 files changed, 3 insertions(+), 1 deletion(-) diff --git a/realsense2_camera/test/README.md b/realsense2_camera/test/README.md index 9624848955..ea3a043a92 100644 --- a/realsense2_camera/test/README.md +++ b/realsense2_camera/test/README.md @@ -158,7 +158,7 @@ If a user wants to add a test to this conditional skip, user can add by adding a RS_ROS_REGRESSION=1 PYTHONPATH=$PYTHONPATH:$PWD/realsense2_camera/test/utils:$PWD/realsense2_camera//launch:$PWD/realsense2_camera//scripts pytest-3 -s realsense2_camera/test/live_camera/test_camera_aligned_tests.py -k test_camera_align_depth_color_all -m d415 ## Points to be noted while writing pytests -The tests that are in one file are normally run in parallel. So if there are multiple tests in one file, the system capacity can influence the test execution. It's recomended to have 3-4 tests in file, more than that can affect the test results due to delays. +The tests that are in one file are normally run in parallel, there could also be changes in the pytest plugin. So if there are multiple tests in one file, the system capacity can influence the test execution. It's recomended to have 3-4 tests in file, more than that can affect the test results due to delays. ### Passing/changing parameters The parameters passed while creating the node can be initialized individually for each test, please see the test_parameterized_template example for reference. The default values are taken from rs_launch.py and the passed parameters are used for overriding the default values. The parameters that can be dynamically modified can be changed using the param interface provided. However, the function create_param_ifs has to be called to create this interface. Please see the test_d455_basic_tests.py for reference. There are specific functions to change the string, integer and bool parameters, the utils can be extended if any more types are needed. ### Difference in setting the bool parameters diff --git a/realsense2_camera/test/live_camera/test_d415_basic_tests.py b/realsense2_camera/test/live_camera/test_d415_basic_tests.py index 8b88a30e67..d93bbe5b57 100644 --- a/realsense2_camera/test/live_camera/test_d415_basic_tests.py +++ b/realsense2_camera/test/live_camera/test_d415_basic_tests.py @@ -49,6 +49,7 @@ 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.d415 @pytest.mark.parametrize("launch_descr_with_parameters", [test_params_depth_avg_1],indirect=True) @pytest.mark.launch(fixture=launch_descr_with_parameters) class TestD415_Change_Resolution(pytest_rs_utils.RsTestBaseClass): 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 fcf4561714..0d2163a3e3 100644 --- a/realsense2_camera/test/live_camera/test_d455_basic_tests.py +++ b/realsense2_camera/test/live_camera/test_d455_basic_tests.py @@ -49,6 +49,7 @@ 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.d455 @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):