34 from __future__
import print_function
43 from base64
import standard_b64encode, standard_b64decode
48 if sys.version_info >= (3, 0):
51 "int": [
"int8",
"byte",
"uint8",
"char",
52 "int16",
"uint16",
"int32",
"uint32",
53 "int64",
"uint64",
"float32",
"float64"],
54 "float": [
"float32",
"float64"],
57 primitive_types = [bool, int, float]
62 "int": [
"int8",
"byte",
"uint8",
"char",
63 "int16",
"uint16",
"int32",
"uint32",
64 "int64",
"uint64",
"float32",
"float64"],
65 "float": [
"float32",
"float64"],
67 "unicode": [
"string"],
68 "long": [
"int64",
"uint64"]
70 primitive_types = [bool, int, long, float]
73 list_types = [list, tuple]
74 ros_time_types = [
"time",
"duration"]
75 ros_primitive_types = [
"bool",
"byte",
"char",
"int8",
"uint8",
"int16",
76 "uint16",
"int32",
"uint32",
"int64",
"uint64",
77 "float32",
"float64",
"string"]
78 ros_header_types = [
"Header",
"std_msgs/Header",
"roslib/Header"]
79 ros_binary_types = [
"uint8[]",
"char[]"]
80 list_braces = re.compile(
r'\[[^\]]*\]')
81 ros_binary_types_list_braces = [(
"uint8[]", re.compile(
r'uint8\[[^\]]*\]')),
82 (
"char[]", re.compile(
r'char\[[^\]]*\]'))]
88 global binary_encoder,bson_only_mode
89 if binary_encoder
is None:
90 binary_encoder_type = rospy.get_param(
'~binary_encoder',
'default')
91 bson_only_mode = rospy.get_param(
'~bson_only_mode',
False)
93 if binary_encoder_type ==
'bson' or bson_only_mode:
94 binary_encoder = bson.Binary
95 elif binary_encoder_type ==
'default' or binary_encoder_type ==
'b64':
96 binary_encoder = standard_b64encode
98 print(
"Unknown encoder type '%s'"%binary_encoder_type)
100 return binary_encoder
104 Exception.__init__(self,
"Unable to extract message values from %s instance" % type(inst).__name__)
109 Exception.__init__(self,
"Message type %s does not have a field %s" % (basetype,
'.'.join(fields)))
113 def __init__(self, roottype, fields, expected_type, found_type):
114 if roottype == expected_type:
115 Exception.__init__(self,
"Expected a JSON object for type %s but received a %s" % (roottype, found_type))
117 Exception.__init__(self,
"%s message requires a %s for field %s, but got a %s" % (roottype, expected_type,
'.'.join(fields), found_type))
121 rostype = getattr(inst,
"_type",
None)
128 """ Returns an instance of the provided class, with its fields populated
129 according to the values in msg """
130 return _to_inst(msg, inst._type, inst._type, inst)
134 global bson_only_mode
136 for binary_type, expression
in ros_binary_types_list_braces:
137 if expression.sub(binary_type, rostype)
in ros_binary_types:
139 return encoded
if python2
else encoded.decode(
'ascii')
142 if rostype
in ros_time_types:
143 return {
"secs": inst.secs,
"nsecs": inst.nsecs}
145 if(bson_only_mode
is None):bson_only_mode = rospy.get_param(
'~bson_only_mode',
False)
147 if rostype
in ros_primitive_types:
149 if (
not bson_only_mode)
and (rostype
in [
"float32",
"float64"]):
150 if math.isnan(inst)
or math.isinf(inst):
155 if type(inst)
in list_types:
168 rostype = list_braces.sub(
"", rostype)
171 if rostype
in ros_primitive_types
and not rostype
in [
"float32",
"float64"]:
181 for field_name, field_rostype
in zip(inst.__slots__, inst._slot_types):
182 field_inst = getattr(inst, field_name)
183 msg[field_name] =
_from_inst(field_inst, field_rostype)
187 def _to_inst(msg, rostype, roottype, inst=None, stack=[]):
189 for binary_type, expression
in ros_binary_types_list_braces:
190 if expression.sub(binary_type, rostype)
in ros_binary_types:
194 if rostype
in ros_time_types:
198 if rostype
in ros_primitive_types:
202 if inst
is not None and type(inst)
in list_types:
207 inst = ros_loader.get_message_instance(rostype)
213 if type(msg)
in string_types:
215 return standard_b64decode(msg)
220 return bytes(bytearray(msg))
227 if rostype ==
"time" and msg ==
"now":
228 return rospy.get_rostime()
231 if rostype ==
"time":
232 inst = rospy.rostime.Time()
233 elif rostype ==
"duration":
234 inst = rospy.rostime.Duration()
239 for field
in [
"secs",
"nsecs"]:
242 setattr(inst, field, msg[field])
252 if msgtype
in primitive_types
and rostype
in type_map[msgtype.__name__]:
254 elif msgtype
in string_types
and rostype
in type_map[msgtype.__name__]:
255 return msg.encode(
"utf-8",
"ignore")
if python2
else msg
261 if type(msg)
not in list_types:
269 rostype = list_braces.sub(
"", rostype)
272 return [
_to_inst(x, rostype, roottype,
None, stack)
for x
in msg]
277 if type(msg)
is not dict:
282 if rostype
in ros_header_types:
283 cur_time = rospy.get_rostime()
285 inst.stamp.secs = cur_time.secs
286 inst.stamp.nsecs = cur_time.nsecs
287 except rospy.exceptions.ROSInitException
as e:
288 rospy.logdebug(
"Not substituting the correct header time: %s" % e)
290 inst_fields = dict(zip(inst.__slots__, inst._slot_types))
292 for field_name
in msg:
294 field_stack = stack + [field_name]
297 if not field_name
in inst_fields:
300 field_rostype = inst_fields[field_name]
301 field_inst = getattr(inst, field_name)
303 field_value =
_to_inst(msg[field_name], field_rostype,
304 roottype, field_inst, field_stack)
306 setattr(inst, field_name, field_value)