# Copyright (c) 2012 CNRS
# Author: Florent Lamiraux
#
# This file is part of hpp-corbaserver.
# hpp-corbaserver is free software: you can redistribute it
# and/or modify it under the terms of the GNU Lesser General Public
# License as published by the Free Software Foundation, either version
# 3 of the License, or (at your option) any later version.
#
# hpp-corbaserver is distributed in the hope that it will be
# useful, but WITHOUT ANY WARRANTY; without even the implied warranty
# of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
# General Lesser Public License for more details.  You should have
# received a copy of the GNU Lesser General Public License along with
# hpp-corbaserver.  If not, see
# <http://www.gnu.org/licenses/>.

from .quaternion import Quaternion
from .transform import Transform

def retrieveRosResource(path):
    
import os
    ros_package_paths = os.environ["ROS_PACKAGE_PATH"].split(':')

    if path.startswith("package://"):
        relpath = path[len("package://"):]
        for dir in ros_package_paths:
            abspath = os.path.join(dir,relpath)
            if os.path.exists(abspath):
                return abspath
        return IOError ("Could not find resource " + path)
    else:
        return path