00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
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 def findros(pkg, resource):
00047 """
00048 Find ROS resource inside of a package.
00049
00050 @param pkg: ROS package name
00051 @type pkg: str
00052 @param resource: resource filename
00053 @type resource: str
00054 """
00055 val = roslib.packages.find_resource(pkg, resource)
00056 if val:
00057 return val[0]
00058 else:
00059 raise ROSHException("cannot find resource")
00060
00061 def YAMLBag(object):
00062
00063 def __init__(self, filename):
00064 self.filename = filename
00065 self._fp = open(filename, 'w')
00066
00067 def append(self, msg):
00068 self._fp.write(to_yaml(msg))
00069
00070 def close(self):
00071 if self._fp is not None:
00072 self._fp.close()
00073 self._fp = None
00074
00075 def to_yaml(obj):
00076 if isinstance(obj, roslib.message.Message):
00077 return _message_to_yaml(obj)
00078 pass
00079 else:
00080 return yaml.dump(obj)
00081
00082 def yaml_msg_str(type_, yaml_str, filename=None):
00083 """
00084 Load single message from YAML dictionary representation.
00085
00086 @param type_: Message class
00087 @type type_: class (Message subclass)
00088 @param filename: Name of YAML file
00089 @type filename: str
00090 """
00091 import yaml
00092 if yaml_str.strip() == '':
00093 msg_dict = {}
00094 else:
00095 msg_dict = yaml.load(yaml_str)
00096 if type(msg_dict) != dict:
00097 if filename:
00098 raise ValueError("yaml file [%s] does not contain a dictionary"%filename)
00099 else:
00100 raise ValueError("yaml string does not contain a dictionary")
00101 m = type_()
00102 roslib.message.fill_message_args(m, [msg_dict])
00103 return m
00104
00105 def yaml_msg(type_, filename):
00106 """
00107 Load single message from YAML dictionary representation.
00108
00109 @param type_: Message class
00110 @type type_: class (Message subclass)
00111 @param filename: Name of YAML file
00112 @type filename: str
00113 """
00114 with open(filename, 'r') as f:
00115 return yaml_msg_str(type_, f.read(), filename=filename)
00116
00117 def yaml_msgs_str(type_, yaml_str, filename=None):
00118 """
00119 Load messages from YAML list-of-dictionaries representation.
00120
00121 @param type_: Message class
00122 @type type_: class (Message subclass)
00123 @param filename: Name of YAML file
00124 @type filename: str
00125 """
00126 import yaml
00127 yaml_doc = yaml.load(yaml_str)
00128 msgs = []
00129 for msg_dict in yaml_doc:
00130 if not type(msg_dict) == dict:
00131 if filename:
00132 raise ValueError("yaml file [%s] does not contain a list of dictionaries"%filename)
00133 else:
00134 raise ValueError("yaml string does not contain a list of dictionaries")
00135 m = type_()
00136 roslib.message.fill_message_args(m, msg_dict)
00137 msgs.append(m)
00138 return msgs
00139
00140 def yaml_msgs(type_, filename):
00141 """
00142 Load messages from YAML list-of-dictionaries representation.
00143
00144 @param type_: Message class
00145 @type type_: class (Message subclass)
00146 @param filename: Name of YAML file
00147 @type filename: str
00148 """
00149 with open(filename, 'r') as f:
00150 return yaml_msgs_str(type_, f.read(), filename=filename)
00151
00152 def _message_to_yaml(msg, indent='', time_offset=None):
00153 """
00154 convert value to YAML representation
00155 @param val: to convert to string representation. Most likely a Message.
00156 @type val: Value
00157 @param indent: indentation
00158 @type indent: str
00159 @param time_offset: if not None, time fields will be displayed
00160 as deltas from time_offset
00161 @type time_offset: Time
00162 """
00163 if type(msg) in [int, long, float, str, bool]:
00164
00165 return msg
00166 elif isinstance(msg, rospy.Time) or isinstance(msg, rospy.Duration):
00167
00168 if time_offset is not None and isinstance(msg, rospy.Time):
00169 msg = msg-time_offset
00170
00171 return '\n%ssecs: %s\n%snsecs: %s'%(indent, msg.secs, indent, msg.nsecs)
00172
00173 elif type(msg) in [list, tuple]:
00174
00175 if len(msg) == 0:
00176 return str(list(msg))
00177 msg0 = msg[0]
00178 if type(msg0) in [int, float, str, bool] or \
00179 isinstance(msg0, Time) or isinstance(msg0, Duration) or \
00180 type(msg0) in [list, tuple]:
00181 return str(list(msg))
00182 else:
00183 indent = indent + ' '
00184 return "["+','.join([roslib.message.strify_message(v, indent, time_offset) for v in msg])+"]"
00185 elif isinstance(msg, rospy.Message):
00186 if indent:
00187 return '\n'+\
00188 '\n'.join(['%s%s: %s'%(indent, f,
00189 strify_message(getattr(msg, f), ' '+indent, time_offset)) for f in msg.__slots__])
00190 return '\n'.join(['%s%s: %s'%(indent, f, roslib.message.strify_message(getattr(msg, f), ' '+indent, time_offset)) for f in msg.__slots__])
00191 else:
00192 return str(msg)
