58 lines
2.1 KiB
Python
58 lines
2.1 KiB
Python
#!/usr/bin/env python3
|
|
|
|
import math
|
|
import os
|
|
import sys
|
|
import yaml
|
|
from typing import Dict
|
|
|
|
from ament_index_python.packages import get_package_share_directory
|
|
|
|
def print_error(message: str) -> None:
|
|
"""Print an error message in red color."""
|
|
print("\033[91m{}\033[0m".format(message), file=sys.stderr)
|
|
|
|
def construct_angle_radians(loader, node) -> float:
|
|
"""Utility function to construct radian values from yaml."""
|
|
value = loader.construct_scalar(node)
|
|
try:
|
|
return float(value)
|
|
except SyntaxError:
|
|
raise Exception("Invalid expression: %s" % value)
|
|
|
|
def construct_angle_degrees(loader, node) -> float:
|
|
"""Utility function for converting degrees into radians from yaml."""
|
|
return math.radians(construct_angle_radians(loader, node))
|
|
|
|
def load_yaml(package_name: str, file_path: str) -> Dict:
|
|
package_path = get_package_share_directory(package_name)
|
|
absolute_file_path = os.path.join(package_path, file_path)
|
|
|
|
try:
|
|
yaml.SafeLoader.add_constructor("!radians", construct_angle_radians)
|
|
yaml.SafeLoader.add_constructor("!degrees", construct_angle_degrees)
|
|
except Exception:
|
|
print_error("Error: YAML support not available; install python-yaml")
|
|
sys.exit(1)
|
|
|
|
try:
|
|
with open(absolute_file_path) as file:
|
|
return yaml.safe_load(file)
|
|
except OSError: # parent of IOError, OSError *and* WindowsError where available
|
|
print_error("Error: Unable to open the file {}".format(absolute_file_path))
|
|
sys.exit(1)
|
|
|
|
def load_yaml_abs(absolute_file_path: str) -> Dict:
|
|
try:
|
|
yaml.SafeLoader.add_constructor("!radians", construct_angle_radians)
|
|
yaml.SafeLoader.add_constructor("!degrees", construct_angle_degrees)
|
|
except Exception:
|
|
print_error("Error: YAML support not available; install python-yaml")
|
|
sys.exit(1)
|
|
|
|
try:
|
|
with open(absolute_file_path) as file:
|
|
return yaml.safe_load(file)
|
|
except OSError: # parent of IOError, OSError *and* WindowsError where available
|
|
print_error("Error: Unable to open the file {}".format(absolute_file_path))
|
|
sys.exit(1)
|