library.py
Go to the documentation of this file.
00001 # Software License Agreement (BSD License)
00002 #
00003 # Copyright (c) 2010, Willow Garage, Inc.
00004 # All rights reserved.
00005 #
00006 # Redistribution and use in source and binary forms, with or without
00007 # modification, are permitted provided that the following conditions
00008 # are met:
00009 #
00010 #  * Redistributions of source code must retain the above copyright
00011 #    notice, this list of conditions and the following disclaimer.
00012 #  * Redistributions in binary form must reproduce the above
00013 #    copyright notice, this list of conditions and the following
00014 #    disclaimer in the documentation and/or other materials provided
00015 #    with the distribution.
00016 #  * Neither the name of Willow Garage, Inc. nor the names of its
00017 #    contributors may be used to endorse or promote products derived
00018 #    from this software without specific prior written permission.
00019 #
00020 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00021 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00022 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00023 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00024 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00025 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00026 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00027 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00028 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00029 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00030 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00031 # POSSIBILITY OF SUCH DAMAGE.
00032 #
00033 # Revision $Id: library.py 9993 2010-06-09 02:35:02Z kwc $
00034 """
00035 Top-level library routines we expose to the end-user
00036 """
00037 
00038 from __future__ import with_statement
00039 import yaml
00040 
00041 import roslib.message
00042 import roslib.packages
00043 
00044 import rospy
00045 
00046 
00047 def findros(pkg, resource):
00048     """
00049     Find ROS resource inside of a package.
00050 
00051     @param pkg: ROS package name
00052     @type  pkg: str
00053     @param resource: resource filename
00054     @type  resource: str
00055     """
00056     val = roslib.packages.find_resource(pkg, resource)
00057     if val:
00058         return val[0]
00059     else:
00060         raise rospy.ROSException("cannot find resource")
00061 
00062 
00063 def YAMLBag(object):
00064     def __init__(self, filename):
00065         self.filename = filename
00066         self._fp = open(filename, 'w')
00067 
00068     def append(self, msg):
00069         self._fp.write(to_yaml(msg))
00070 
00071     def close(self):
00072         if self._fp is not None:
00073             self._fp.close()
00074             self._fp = None
00075 
00076 
00077 def to_yaml(obj):
00078     if isinstance(obj, roslib.message.Message):
00079         return _message_to_yaml(obj)
00080         pass
00081     else:
00082         return yaml.dump(obj)
00083 
00084 
00085 def yaml_msg_str(type_, yaml_str, filename=None):
00086     """
00087     Load single message from YAML dictionary representation.
00088 
00089     @param type_: Message class
00090     @type  type_: class (Message subclass)
00091     @param filename: Name of YAML file
00092     @type  filename: str
00093     """
00094     import yaml
00095     if yaml_str.strip() == '':
00096         msg_dict = {}
00097     else:
00098         msg_dict = yaml.load(yaml_str)
00099     if not isinstance(msg_dict, dict):
00100         if filename:
00101             raise ValueError("yaml file [%s] does not contain a dictionary" % filename)
00102         else:
00103             raise ValueError("yaml string does not contain a dictionary")
00104     m = type_()
00105     roslib.message.fill_message_args(m, [msg_dict])
00106     return m
00107 
00108 
00109 def yaml_msg(type_, filename):
00110     """
00111     Load single message from YAML dictionary representation.
00112 
00113     @param type_: Message class
00114     @type  type_: class (Message subclass)
00115     @param filename: Name of YAML file
00116     @type  filename: str
00117     """
00118     with open(filename, 'r') as f:
00119         return yaml_msg_str(type_, f.read(), filename=filename)
00120 
00121 
00122 def yaml_msgs_str(type_, yaml_str, filename=None):
00123     """
00124     Load messages from YAML list-of-dictionaries representation.
00125 
00126     @param type_: Message class
00127     @type  type_: class (Message subclass)
00128     @param filename: Name of YAML file
00129     @type  filename: str
00130     """
00131     import yaml
00132     yaml_doc = yaml.load(yaml_str)
00133     msgs = []
00134     for msg_dict in yaml_doc:
00135         if not isinstance(msg_dict, dict):
00136             if filename:
00137                 raise ValueError("yaml file [%s] does not contain a list of dictionaries" % filename)
00138             else:
00139                 raise ValueError("yaml string does not contain a list of dictionaries")
00140         m = type_()
00141         roslib.message.fill_message_args(m, msg_dict)
00142         msgs.append(m)
00143     return msgs
00144 
00145 
00146 def yaml_msgs(type_, filename):
00147     """
00148     Load messages from YAML list-of-dictionaries representation.
00149 
00150     @param type_: Message class
00151     @type  type_: class (Message subclass)
00152     @param filename: Name of YAML file
00153     @type  filename: str
00154     """
00155     with open(filename, 'r') as f:
00156         return yaml_msgs_str(type_, f.read(), filename=filename)
00157 
00158 
00159 def _message_to_yaml(msg, indent='', time_offset=None):
00160     """
00161     convert value to YAML representation
00162     @param val: to convert to string representation. Most likely a Message.
00163     @type  val: Value
00164     @param indent: indentation
00165     @type  indent: str
00166     @param time_offset: if not None, time fields will be displayed
00167     as deltas from  time_offset
00168     @type  time_offset: Time
00169     """
00170     if type(msg) in [int, long, float, str, bool]:
00171         # TODO: need to actually escape
00172         return msg
00173     elif isinstance(msg, rospy.Time) or isinstance(msg, rospy.Duration):
00174         if time_offset is not None and isinstance(msg, rospy.Time):
00175             msg = msg-time_offset
00176 
00177         return '\n%ssecs: %s\n%snsecs: %s' % (indent, msg.secs, indent, msg.nsecs)
00178 
00179     elif type(msg) in [list, tuple]:
00180         # have to convert tuple->list to be yaml-safe
00181         if len(msg) == 0:
00182             return str(list(msg))
00183         msg0 = msg[0]
00184         if type(msg0) in [int, float, str, bool] or \
00185                 isinstance(msg0, rospy.Time) or isinstance(msg0, rospy.Duration) or \
00186                 isinstance(msg0, list) or isinstance(msg0, tuple):
00187             # no array-of-arrays support yet
00188             return str(list(msg))
00189         else:
00190             indent = indent + '  '
00191             return "["+','.join([roslib.message.strify_message(v, indent, time_offset) for v in msg])+"]"
00192     elif isinstance(msg, rospy.Message):
00193         if indent:
00194             return '\n' + \
00195                 '\n'.join(['%s%s: %s' % (
00196                     indent, f, roslib.message.strify_message(getattr(msg, f), '  ' + indent, time_offset)) for f in msg.__slots__])
00197         return '\n'.join(['%s%s: %s' % (indent, f, roslib.message.strify_message(getattr(msg, f), '  ' + indent, time_offset)) for f in msg.__slots__])
00198     else:
00199         return str(msg)  # punt


actionlib
Author(s): Eitan Marder-Eppstein, Vijay Pradeep, Mikael Arguedas
autogenerated on Sat Feb 16 2019 03:21:28