84 lines
3.2 KiB
Python
84 lines
3.2 KiB
Python
#!/usr/bin/env python3
|
|
|
|
# Copyright (c) 2021 PickNik, Inc.
|
|
#
|
|
# Redistribution and use in source and binary forms, with or without
|
|
# modification, are permitted provided that the following conditions are met:
|
|
#
|
|
# * Redistributions of source code must retain the above copyright
|
|
# notice, this list of conditions and the following disclaimer.
|
|
#
|
|
# * Redistributions in binary form must reproduce the above copyright
|
|
# notice, this list of conditions and the following disclaimer in the
|
|
# documentation and/or other materials provided with the distribution.
|
|
#
|
|
# * Neither the name of the {copyright_holder} nor the names of its
|
|
# contributors may be used to endorse or promote products derived from
|
|
# this software without specific prior written permission.
|
|
#
|
|
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
|
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
|
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
|
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
|
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
|
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
|
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
|
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
|
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
|
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
|
# POSSIBILITY OF SUCH DAMAGE.
|
|
|
|
#
|
|
# Author: Lovro Ivanov
|
|
|
|
import math
|
|
import os
|
|
import yaml
|
|
|
|
from ament_index_python.packages import get_package_share_directory
|
|
|
|
|
|
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("invalid expression: %s" % 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 load_yaml(package_name, file_path):
|
|
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:
|
|
raise Exception("yaml support not available; install python-yaml")
|
|
|
|
try:
|
|
with open(absolute_file_path) as file:
|
|
return yaml.safe_load(file)
|
|
except OSError: # parent of IOError, OSError *and* WindowsError where available
|
|
return None
|
|
|
|
|
|
def load_yaml_abs(absolute_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(absolute_file_path) as file:
|
|
return yaml.safe_load(file)
|
|
except OSError: # parent of IOError, OSError *and* WindowsError where available
|
|
return None
|