# Path: intera_interface/src/intera_motion_interface/utility_functions.py
# def ensure_path_to_file_exists(raw_file_path):
#     """
#     This function does two checks:
#     1) Expands the ~ in the file path string
#     2) Creates any intermediate directories that do not exist
#     @param raw_file_path
#     @return: file_name = valid path to file
#     """
#     file_name = os.path.expanduser(raw_file_path)
#     file_dir = os.path.dirname(file_name)
#     if file_dir and not os.path.exists(file_dir):
#         os.makedirs(file_dir)
#     return file_name
# 
# def clamp_float_warn(low, val, upp, name):
#     """
#     Clamps: low <= val <= upp
#     Prints: warning if clamped, error if input is not a float
#     @param low: lower bound for the input  (float)
#     @param val: input (float ?)
#     @param upp: upper bound for the input  (float)
#     @param name: input name (str)
#     @return: clamp(low,val,upp) if input is a float, None otherwise.
#     """
#     if not isinstance(val, float):
#         rospy.logerr('Invalid input type: ' + name + ' must be a float!')
#         return None
#     if val < low:
#         rospy.logwarn(''.join(['Clamping ' + name, ' (' + str(val) + ')',
#                                ' to the lower bound: ',  str(low)]))
#         return low
#     if val > upp:
#         rospy.logwarn(''.join(['Clamping ' + name, ' (' + str(val) + ')',
#                                ' to the upper bound: ',  str(upp)]))
#         return upp
#     return val


import rospy
import yaml
from intera_motion_msgs.msg import WaypointOptions
from copy import deepcopy
from .utility_functions import (
    ensure_path_to_file_exists,
    clamp_float_warn
)
from rospy_message_converter import message_converter
        """
        if tol is None:
            self.set_joint_tolerances(self.default_joint_tolerance)
        elif not tol:
            self._data.tol = []
        elif isinstance(tol, float):
            tol = clamp_float_warn(1e-6, tol, float('inf'), 'joint tolerance')
            self._data.joint_tolerances = [tol] * self._n_dim
        elif len(tol) == self._n_dim:
            self._data.joint_tolerances = deepcopy(joint_tolerances)
        else:
            rospy.logerr('Invalid input to set_joint_tolerances!')

    def set_max_linear_speed(self, speed = None):
        """
        For Cartesian paths only
        @param speed: max linear tip speed (m/s)
        """
        if speed is None:
            speed = self.default_max_linear_speed
        tol = clamp_float_warn(0.001, speed, float('inf'), 'max linear speed')
        self._data.max_linear_speed = speed

    def set_max_linear_accel(self, accel = None):
        """
        For Cartesian paths only
        @param accel: max linear tip acceleration (m/s/s)
        """
        if accel is None:
            accel = self.default_max_linear_accel

# Path: intera_interface/src/intera_motion_interface/utility_functions.py
# def ensure_path_to_file_exists(raw_file_path):
#     """
#     This function does two checks:
#     1) Expands the ~ in the file path string
#     2) Creates any intermediate directories that do not exist
#     @param raw_file_path
#     @return: file_name = valid path to file
#     """
#     file_name = os.path.expanduser(raw_file_path)
#     file_dir = os.path.dirname(file_name)
#     if file_dir and not os.path.exists(file_dir):
#         os.makedirs(file_dir)
#     return file_name
# 
# def clamp_float_warn(low, val, upp, name):
#     """
#     Clamps: low <= val <= upp
#     Prints: warning if clamped, error if input is not a float
#     @param low: lower bound for the input  (float)
#     @param val: input (float ?)
#     @param upp: upper bound for the input  (float)
#     @param name: input name (str)
#     @return: clamp(low,val,upp) if input is a float, None otherwise.
#     """
#     if not isinstance(val, float):
#         rospy.logerr('Invalid input type: ' + name + ' must be a float!')
#         return None
#     if val < low:
#         rospy.logwarn(''.join(['Clamping ' + name, ' (' + str(val) + ')',
#                                ' to the lower bound: ',  str(low)]))
#         return low
#     if val > upp:
#         rospy.logwarn(''.join(['Clamping ' + name, ' (' + str(val) + ')',
#                                ' to the upper bound: ',  str(upp)]))
#         return upp
#     return val


# Path: intera_interface/src/intera_motion_interface/motion_waypoint_options.py
import rospy
import yaml
from intera_motion_msgs.msg import WaypointOptions
from copy import deepcopy
from .utility_functions import (
    ensure_path_to_file_exists,
    clamp_float_warn
)
from rospy_message_converter import message_converter

        """
        if tol is None:
            self.set_joint_tolerances(self.default_joint_tolerance)
        elif not tol:
            self._data.tol = []
        elif isinstance(tol, float):
            tol = clamp_float_warn(1e-6, tol, float('inf'), 'joint tolerance')
            self._data.joint_tolerances = [tol] * self._n_dim
        elif len(tol) == self._n_dim:
            self._data.joint_tolerances = deepcopy(joint_tolerances)
        else:
            rospy.logerr('Invalid input to set_joint_tolerances!')

    def set_max_linear_speed(self, speed = None):
        """
        For Cartesian paths only
        @param speed: max linear tip speed (m/s)
        """
        if speed is None:
            speed = self.default_max_linear_speed
        tol = clamp_float_warn(0.001, speed, float('inf'), 'max linear speed')
        self._data.max_linear_speed = speed

    def set_max_linear_accel(self, accel = None):
        """
        For Cartesian paths only
        @param accel: max linear tip acceleration (m/s/s)
        """
        if accel is None:
            accel = self.default_max_linear_accel

        tol = clamp_float_warn(0.001, accel, float('inf'), 'max linear accel')