diff --git a/launch_param_builder/utils.py b/launch_param_builder/utils.py index b2a517e..f6afc0b 100644 --- a/launch_param_builder/utils.py +++ b/launch_param_builder/utils.py @@ -32,6 +32,7 @@ from typing import List, Union import xacro from ament_index_python.packages import get_package_share_directory +import math class ParameterBuilderFileNotFoundError(KeyError): @@ -52,6 +53,20 @@ class ParameterBuilderFileNotFoundError(KeyError): ] +def construct_angle_radians(loader, node): + """Utility function to construct radian values from YAML.""" + value = loader.construct_scalar(node) + try: + return float(value) + except SyntaxError: + raise Exception(f"invalid expression: {value}") + + +def construct_angle_degrees(loader, node): + """Utility function for converting degrees into radians from YAML.""" + return math.radians(construct_angle_radians(loader, node)) + + def raise_if_file_not_found(file_path: Path): if not file_path.exists(): raise ParameterBuilderFileNotFoundError(f"File {file_path} doesn't exist") @@ -62,17 +77,23 @@ def load_file(file_path: Path): try: with open(file_path, "r") as file: return file.read() - except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + except OSError: # parent of IOError, OSError *and* WindowsError where available return None def load_yaml(file_path: Path): raise_if_file_not_found(file_path) + try: + yaml.SafeLoader.add_constructor("!radians", construct_angle_radians) + yaml.SafeLoader.add_constructor("!degrees", construct_angle_degrees) + except Exception: + raise Exception("yaml support not available; install python-yaml") + try: with open(file_path, "r") as file: - return yaml.load(file, Loader=yaml.FullLoader) - except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + return yaml.safe_load(file) + except OSError: # parent of IOError, OSError *and* WindowsError where available return None