From 028610f2220bde4a225de7f3ded3154787569546 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Fri, 10 May 2024 13:50:21 +0200 Subject: [PATCH] Made new tests Signed-off-by: Jakub Delicat --- CMakeLists.txt | 10 +- test/test_components_xacro.py | 139 ++++++++++++ test/test_components_yaml_parse.py | 347 ----------------------------- urdf/components.urdf.xacro | 1 - urdf/orbbec_astra.urdf.xacro | 2 +- urdf/slamtec_rplidar_a2.urdf.xacro | 2 +- urdf/slamtec_rplidar_a3.urdf.xacro | 2 +- urdf/slamtec_rplidar_s1.urdf.xacro | 2 +- urdf/slamtec_rplidar_s2.urdf.xacro | 2 +- urdf/slamtec_rplidar_s3.urdf.xacro | 2 +- 10 files changed, 150 insertions(+), 359 deletions(-) create mode 100644 test/test_components_xacro.py delete mode 100644 test/test_components_yaml_parse.py diff --git a/CMakeLists.txt b/CMakeLists.txt index 36a8d13..c07fe79 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -10,12 +10,12 @@ install( if(BUILD_TESTING) find_package(ament_cmake_pytest REQUIRED) - set(_pytest_tests - test/test_components_yaml_parse.py + set(pytest_tests + test/test_components_xacro.py ) - foreach(_test_path ${_pytest_tests}) - get_filename_component(_test_name ${_test_path} NAME_WE) - ament_add_pytest_test(${_test_name} ${_test_path} + foreach(test_path ${pytest_tests}) + get_filename_component(test_name ${test_path} NAME_WE) + ament_add_pytest_test(${test_name} ${test_path} APPEND_ENV PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR} TIMEOUT 60 WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} diff --git a/test/test_components_xacro.py b/test/test_components_xacro.py new file mode 100644 index 0000000..75e1577 --- /dev/null +++ b/test/test_components_xacro.py @@ -0,0 +1,139 @@ +# Copyright 2024 Husarion sp. z o.o. +# +# 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 xml.dom +import xml.dom.minidom +import os +import xacro +import xml +import yaml +from ament_index_python.packages import get_package_share_directory + +ros_components_description = get_package_share_directory("ros_components_description") +xacro_path = os.path.join(ros_components_description, "test/component.urdf.xacro") + +# Type: [model_link, sensor_link, sensor_name] +components_types_with_names = { + "LDR01": ["slamtec_rplidar_s1", "laser", "slamtec_rplidar_s1_sensor"], + "LDR06": ["slamtec_rplidar_s3", "laser", "slamtec_rplidar_s3_sensor"], + "LDR13": ["ouster_os1_32", "os_lidar", "ouster_os1_32_sensor"], + "LDR20": ["velodyne_puck", "velodyne", "velodyne_puck_sensor"], + "CAM01": ["orbbec_astra", "orbbec_astra", "orbbec_astra_color"], +} + + +class ComponentsYamlParseUtils: + __test__ = False + + def __init__(self, components_config_path: str) -> None: + self.components_config_path = components_config_path + self._urdf = xml.dom.minidom.Document() + + def save_yaml(self, node: yaml.Node) -> None: + with open(self.components_config_path, mode="w", encoding="utf-8") as file: + yaml.dump(node, file, default_flow_style=False) + + def create_component( + self, + type: str, + device_namespace: str, + parent_link="cover_link", + xyz="0.0 0.0 0.0", + rpy="0.0 0.0 0.0", + ) -> dict: + return { + "type": type, + "parent_link": parent_link, + "xyz": xyz, + "rpy": rpy, + "device_namespace": device_namespace, + } + + def does_urdf_parse(self) -> bool: + try: + self._urdf = xacro.process_file( + xacro_path, mappings={"components_config_path": self.components_config_path} + ) + except xacro.XacroException as e: + return False + return True + + def does_link_exist(self, doc: xml.dom.minidom.Document, link_name: str) -> bool: + links = doc.getElementsByTagName('link') + for link in links: + if link.getAttribute('name') == link_name: + return True + return False + + def does_sensor_name_exist( + self, doc: xml.dom.minidom.Document, link_name: str, sensor_name: str + ) -> bool: + gazebos_tags = doc.getElementsByTagName('gazebo') + for tag in gazebos_tags: + if tag.getAttribute('reference') == link_name: + sensors = doc.getElementsByTagName('sensor') + for sensor in sensors: + if sensor.getAttribute('name') == sensor_name: + return True + + return False + + def test_component(self, component: dict, expected_result: list, components_config_path: str): + names = components_types_with_names[component["type"]] + component_name = names[0] + sensor_reference = names[1] + sensor_name = names[2] + + device_namespace = component["device_namespace"] + link_name = device_namespace + "_" + component_name + "_link" + sensor_name = device_namespace + "_" + sensor_name + sensor_link_name = device_namespace + "_" + sensor_reference + + if self.does_urdf_parse() != expected_result[0]: + assert ( + False + ), f"Expected prase result {expected_result[0]} with file {components_config_path} and component {component_name}." + + if self.does_link_exist(self._urdf, link_name) != expected_result[1]: + assert ( + False + ), f"Link name: {link_name}. Expected result {expected_result[1]} with file {components_config_path} and component {component_name}." + + if ( + self.does_sensor_name_exist(self._urdf, sensor_link_name, sensor_name) + != expected_result[2] + ): + assert ( + False + ), f"Sensor name: {sensor_name}, sensor link name: {sensor_link_name}. Expected result {expected_result[2]} with file {components_config_path} and component {component_name}." + + +def test_all_good_single_components(tmpdir_factory): + for type_name, value in components_types_with_names.items(): + name = value[0] + dir = tmpdir_factory.mktemp(name) + components_config_path = dir.join(name + "_test_components.yaml") + + utils = ComponentsYamlParseUtils(str(components_config_path)) + components = { + "components": [utils.create_component(type_name, name)], + } + + utils.save_yaml(components) + + for component in components["components"]: + utils.test_component(component, [True, True, True], str(components_config_path)) + + diff --git a/test/test_components_yaml_parse.py b/test/test_components_yaml_parse.py deleted file mode 100644 index ac11d1b..0000000 --- a/test/test_components_yaml_parse.py +++ /dev/null @@ -1,347 +0,0 @@ -# Copyright 2024 Husarion sp. z o.o. -# -# 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 xml.dom -import xml.dom.minidom -import os -import xacro -import xml -import yaml -from ament_index_python.packages import get_package_share_directory - -ros_components_description = get_package_share_directory("ros_components_description") -xacro_path = os.path.join(ros_components_description, "test/component.urdf.xacro") - - -class ComponentsYamlParseUtils: - __test__ = False - - def __init__(self, components_config_path: str) -> None: - self.components_config_path = components_config_path - - def save_yaml(self, node: yaml.Node) -> None: - with open(self.components_config_path, mode="w", encoding="utf-8") as file: - yaml.dump(node, file, default_flow_style=False) - - def create_component( - self, - type: str, - tf_prefix: str, - namespace: str, - name="None", - parent_link="cover_link", - xyz="0.0 0.0 0.0", - rpy="0.0 0.0 0.0", - ) -> dict: - return { - "type": type, - "parent_link": parent_link, - "xyz": xyz, - "rpy": rpy, - "tf_prefix": tf_prefix, - "namespace": namespace, - "name": name, - } - - def try_to_parse_wrong_and_test(self, log: str) -> None: - try: - xacro.process_file( - xacro_path, mappings={"components_config_path": self.components_config_path} - ) - assert False, log - except xacro.XacroException as e: - f"{log} Error:\n{e}" - assert True - - def try_to_parse_well_and_test(self, log: str) -> xml.dom.minidom.Document: - doc = xml.dom.minidom.Document() - try: - doc = xacro.process_file( - xacro_path, mappings={"components_config_path": self.components_config_path} - ) - assert True - except xacro.XacroException as e: - assert False, f"{log}\nError:\n{e}" - - return doc - - def look_for_link_well_and_test( - self, doc: xml.dom.minidom.Document, link_name: str, log: str - ) -> None: - links = doc.getElementsByTagName('link') - for link in links: - if link.getAttribute('name') == link_name: - assert True - return - assert False, f"{log}" - - def look_for_link_wrong_and_test( - self, doc: xml.dom.minidom.Document, link_name: str, log: str - ) -> None: - links = doc.getElementsByTagName('link') - for link in links: - if link.getAttribute('name') == link_name: - assert False, f"{log}" - assert True - - def look_for_sensor_name_well( - self, doc: xml.dom.minidom.Document, link_name: str, sensor_name: str, log: str - ) -> None: - gazebos_tags = doc.getElementsByTagName('gazebo') - for tag in gazebos_tags: - if tag.getAttribute('reference') == link_name: - sensors = doc.getElementsByTagName('sensor') - for sensor in sensors: - if sensor.getAttribute('name') == sensor_name: - assert True - return - - assert False, f"{log}" - - def test_lidar(self, doc: xml.dom.minidom.Document, tf_prefix: str, namespace: str) -> None: - link_name = "laser" - if tf_prefix != "None": - link_name = tf_prefix + "_" + link_name - - sensor_name = "rplidar_s1_sensor" - if tf_prefix != "None": - sensor_name = tf_prefix + "_" + sensor_name - if namespace != "None": - sensor_name = namespace + "/" + sensor_name - - self.look_for_link_well_and_test(doc, link_name, f"Could not find link: {link_name}") - self.look_for_sensor_name_well( - doc, - link_name, - sensor_name, - f"Could not find sensor: {sensor_name}, referencing to link: {link_name} with config {self.components_config_path}", - ) - - def test_camera( - self, doc: xml.dom.minidom.Document, tf_prefix: str, namespace: str, name: str - ) -> None: - link_name = name + "_orbbec_astra_link" - if tf_prefix != "None": - link_name = tf_prefix + "_" + link_name - - sensor_name = name + "_orbbec_astra_color" - if tf_prefix != "None": - sensor_name = tf_prefix + "_" + sensor_name - if namespace != "None": - sensor_name = namespace + "/" + sensor_name - - self.look_for_link_well_and_test( - doc, - link_name, - f"Could not find link: {link_name} with config {self.components_config_path}", - ) - - self.look_for_sensor_name_well( - doc, - link_name, - sensor_name, - f"Could not find sensor: {sensor_name}, referencing to link: {link_name} with config {self.components_config_path}", - ) - - -def test_wrong_config(tmpdir_factory): - dir = tmpdir_factory.mktemp("panther_bringup_test") - components_config_path = dir.join("test_wrong_config.yaml") - - node = yaml.safe_load("a: 5") - utils = ComponentsYamlParseUtils(components_config_path) - - utils.save_yaml(node) - utils.try_to_parse_wrong_and_test(f"Xacro should not parse well for this yaml file: {node}") - - -def test_good_lidars_with_tf_prefix_and_namespace(tmpdir_factory): - dir = tmpdir_factory.mktemp("panther_bringup_test") - components_config_path = dir.join("test_good_lidars_with_tf_prefix_and_namespace.yaml") - - tf_prefixes = ["main_lidar", "additional_lidar"] - namespaces = ["/main_lidar", "/additional_lidar"] - - utils = ComponentsYamlParseUtils(str(components_config_path)) - - node = { - "components": [], - } - for i in range(len(tf_prefixes)): - node["components"].append(utils.create_component("LDR01", tf_prefixes[i], namespaces[i])) - - utils.save_yaml(node) - - doc = utils.try_to_parse_well_and_test( - f"Could not parse without namespace:\n\t{node}\nIn file {components_config_path} with config {components_config_path}" - ) - - for i in range(len(tf_prefixes)): - utils.test_lidar(doc, tf_prefixes[i], namespaces[i]) - - -def test_good_cameras_with_tf_prefix_and_namespace(tmpdir_factory): - dir = tmpdir_factory.mktemp("panther_bringup_test") - components_config_path = dir.join("test_good_cameras_with_tf_prefix_and_namespace.yaml") - - tf_prefixes = ["main", "additional"] - namespaces = ["/main", "/additional"] - names = ["camera", "camera"] - - utils = ComponentsYamlParseUtils(str(components_config_path)) - - node = { - "components": [], - } - for i in range(len(tf_prefixes)): - node["components"].append( - utils.create_component( - "CAM01", - tf_prefixes[i], - namespaces[i], - names[i], - ) - ) - - utils.save_yaml(node) - - doc = utils.try_to_parse_well_and_test( - f"Could not parse without namespace:\n\t{node}\nIn file {components_config_path}" - ) - - for i in range(len(tf_prefixes)): - utils.test_camera(doc, tf_prefixes[i], namespaces[i], names[i]) - - -def test_good_cameras_and_lidars_without_namespace_and_tf_prefix(tmpdir_factory): - dir = tmpdir_factory.mktemp("panther_bringup_test") - components_config_path = dir.join( - "test_good_cameras_and_lidars_without_namespace_and_tf_prefix.yaml" - ) - - utils = ComponentsYamlParseUtils(str(components_config_path)) - - names = ["camera_front", "camera_back"] - node = { - "components": [], - } - for i in range(len(names)): - node["components"].append(utils.create_component("CAM01", "None", "None", names[i])) - - node["components"].append(utils.create_component("LDR01", "None", "None")) - - utils.save_yaml(node) - - doc = utils.try_to_parse_well_and_test( - f"Could not parse without namespace:\n\t{node}\nIn file {components_config_path}" - ) - - for i in range(len(names)): - utils.test_camera(doc, "None", "None", names[i]) - - utils.test_lidar(doc, "None", "None") - - -def test_good_cameras_and_lidars_without_tf_prefix(tmpdir_factory): - dir = tmpdir_factory.mktemp("panther_bringup_test") - components_config_path = dir.join("test_good_cameras_and_lidars_without_tf_prefix.yaml") - - utils = ComponentsYamlParseUtils(str(components_config_path)) - - names = ["camera_front", "camera_back"] - node = { - "components": [], - } - namespaces = ["/front", "/left"] - lidar_namespace = "/main_lidar" - - for i in range(len(names)): - node["components"].append(utils.create_component("CAM01", "None", namespaces[i], names[i])) - - node["components"].append(utils.create_component("LDR01", "None", lidar_namespace)) - - utils.save_yaml(node) - - doc = utils.try_to_parse_well_and_test( - f"Could not parse without namespace:\n\t{node}\nIn file {components_config_path}" - ) - - for i in range(len(names)): - utils.test_camera(doc, "None", namespaces[i], names[i]) - - utils.test_lidar(doc, "None", lidar_namespace) - - -def test_good_cameras_and_lidars_without_namespace(tmpdir_factory): - dir = tmpdir_factory.mktemp("panther_bringup_test") - components_config_path = dir.join("test_good_cameras_and_lidars_without_namespace.yaml") - - utils = ComponentsYamlParseUtils(str(components_config_path)) - - names = ["camera_front", "camera_back"] - node = { - "components": [], - } - tf_prefixes = ["front", "left"] - lidar_tf_prefix = "main_lidar" - - for i in range(len(names)): - node["components"].append( - utils.create_component( - "CAM01", - tf_prefixes[i], - "None", - names[i], - ) - ) - - node["components"].append(utils.create_component("LDR01", lidar_tf_prefix, "None")) - - utils.save_yaml(node) - - doc = utils.try_to_parse_well_and_test( - f"Could not parse without namespace:\n\t{node}\nIn file {components_config_path}" - ) - - for i in range(len(names)): - utils.test_camera(doc, tf_prefixes[i], "None", names[i]) - - utils.test_lidar(doc, lidar_tf_prefix, "None") - - -def test_good_without_any_component(tmpdir_factory): - dir = tmpdir_factory.mktemp("panther_bringup_test") - components_config_path = dir.join("test_wrong_unfilled_params.yaml") - - utils = ComponentsYamlParseUtils(str(components_config_path)) - node = { - "components": [], - } - - utils.save_yaml(node) - utils.try_to_parse_well_and_test( - f"This configuration should be parsed :\n\t{node}\nIn file {components_config_path}" - ) - -def test_wrong_empty_file(tmpdir_factory): - dir = tmpdir_factory.mktemp("panther_bringup_test") - components_config_path = dir.join("test_wrong_unfilled_params.yaml") - - utils = ComponentsYamlParseUtils(str(components_config_path)) - open(components_config_path, mode="w") - - utils.try_to_parse_wrong_and_test( - f"This configuration should not be parsed.\nIn file {components_config_path}" - ) diff --git a/urdf/components.urdf.xacro b/urdf/components.urdf.xacro index 6aa3035..ac3d727 100644 --- a/urdf/components.urdf.xacro +++ b/urdf/components.urdf.xacro @@ -28,7 +28,6 @@ - diff --git a/urdf/orbbec_astra.urdf.xacro b/urdf/orbbec_astra.urdf.xacro index 2031595..866f122 100644 --- a/urdf/orbbec_astra.urdf.xacro +++ b/urdf/orbbec_astra.urdf.xacro @@ -26,7 +26,7 @@ - + diff --git a/urdf/slamtec_rplidar_a2.urdf.xacro b/urdf/slamtec_rplidar_a2.urdf.xacro index df286d0..c99f7c0 100644 --- a/urdf/slamtec_rplidar_a2.urdf.xacro +++ b/urdf/slamtec_rplidar_a2.urdf.xacro @@ -94,7 +94,7 @@ - + ${ns}${device_ns}scan ${ns}${prefix}laser diff --git a/urdf/slamtec_rplidar_a3.urdf.xacro b/urdf/slamtec_rplidar_a3.urdf.xacro index 9371050..223f8e9 100644 --- a/urdf/slamtec_rplidar_a3.urdf.xacro +++ b/urdf/slamtec_rplidar_a3.urdf.xacro @@ -69,7 +69,7 @@ - + ${ns}${device_ns}scan ${ns}${prefix}laser diff --git a/urdf/slamtec_rplidar_s1.urdf.xacro b/urdf/slamtec_rplidar_s1.urdf.xacro index 9d5401d..baf3820 100644 --- a/urdf/slamtec_rplidar_s1.urdf.xacro +++ b/urdf/slamtec_rplidar_s1.urdf.xacro @@ -76,7 +76,7 @@ - + ${ns}${device_ns}scan ${ns}${prefix}laser diff --git a/urdf/slamtec_rplidar_s2.urdf.xacro b/urdf/slamtec_rplidar_s2.urdf.xacro index e3b084a..b739e75 100644 --- a/urdf/slamtec_rplidar_s2.urdf.xacro +++ b/urdf/slamtec_rplidar_s2.urdf.xacro @@ -68,7 +68,7 @@ - + ${ns}${device_ns}scan ${ns}${prefix}laser diff --git a/urdf/slamtec_rplidar_s3.urdf.xacro b/urdf/slamtec_rplidar_s3.urdf.xacro index 629e506..0c13a43 100644 --- a/urdf/slamtec_rplidar_s3.urdf.xacro +++ b/urdf/slamtec_rplidar_s3.urdf.xacro @@ -68,7 +68,7 @@ - + ${ns}${device_ns}scan ${ns}${prefix}laser