diff --git a/src/ltcodecs/ros_message_codec.py b/src/ltcodecs/ros_message_codec.py
index 6e48f0ca9a6bf78e9401411f2abd0cbee090c2e2..87940c311b6746620b3446cbd76780c27fc8ea41 100644
--- a/src/ltcodecs/ros_message_codec.py
+++ b/src/ltcodecs/ros_message_codec.py
@@ -14,14 +14,11 @@ class RosMessageCodec(object):
self.checksum = checksum
if fields_dict:
- self.metadata_decoder_fields = {}
self.metadata_encoder_fields = {}
for field_name, field_params in fields_dict.items():
# We only support a few metadata fields for encoding
codec_name = field_params.get('codec', 'auto')
- if codec_name in (ltcodecs.metadata_decoders.keys()):
- self.metadata_decoder_fields[field_name] = codec_name
for field_name, field_params in fields_dict.items():
# We only support a few metadata fields for encoding
@@ -29,7 +26,6 @@ class RosMessageCodec(object):
if codec_name in (ltcodecs.metadata_encoders.keys()):
self.metadata_encoder_fields[field_name] = codec_name
else:
- self.metadata_decoder_fields = None
self.metadata_encoder_fields = None
self.root_field_codec = ltcodecs.RosMsgFieldCodec(ros_type=ros_type, fields=fields_dict)
@@ -60,7 +56,7 @@ class RosMessageCodec(object):
bits_copy.pos = bits_to_decode.pos
decoded_dict = self.root_field_codec.decode_as_dict(bits_copy)
- ros_msg = self.root_field_codec.decode(bits_to_decode)
+ ros_msg = self.root_field_codec.decode(bits_to_decode, metadata=received_packet)
if self.checksum:
msgpack_bytes = msgpack.packb(decoded_dict)
@@ -77,22 +73,9 @@ class RosMessageCodec(object):
if calculated_crc != received_crc:
raise ValueError("Message CRC Mismatch")
- # Append metadata
- if received_packet:
- self._decode_metadata(ros_msg, received_packet)
-
rospy.loginfo("ROS Message: {}".format(ros_msg))
return ros_msg
- def _decode_metadata(self, ros_msg, received_packet):
- # This will decode metadata fields into the ros msg.
- # it only works on fields of te base ROS message, not any nested ROS messages (for now)
- if self.metadata_decoder_fields:
- for field_name, codec in self.metadata_decoder_fields.items():
- metadata_attribute = ltcodecs.metadata_decoders[codec]
- value = operator.attrgetter(metadata_attribute)(received_packet)
- setattr(ros_msg, field_name, value)
-
def _encode_metadata(self, ros_msg):
# Look through the fields dict for one of the magic keywords specified in field_codecs.py
if self.metadata_encoder_fields:
diff --git a/src/ltcodecs/ros_msg_field_codec.py b/src/ltcodecs/ros_msg_field_codec.py
index fd399cf5022b87b622e5c9b200330f777dbe40e5..a4590c6384655e091f88ad3ceb120ad2611e1d0f 100644
--- a/src/ltcodecs/ros_msg_field_codec.py
+++ b/src/ltcodecs/ros_msg_field_codec.py
@@ -69,13 +69,14 @@ class RosMsgFieldCodec(FieldCodec):
if field_params['codec'] not in ltcodecs.metadata_decoders.keys():
self.field_codecs[field_name] = ltcodecs.field_codec_classes[field_params['codec']](**field_params)
else:
- self.field_codecs[field_name] = None
+ # The metadata codec just remains a string, which we use for a lookup later.
+ pass
except KeyError as e:
raise KeyError("Error parsing codec config for {}. Got params:\n{}\nError: {}".format(field_name,
field_params,
e))
- def encode(self, message: AnyMsg):
+ def encode(self, message: AnyMsg, metadata=None):
# ROS messages use __slots__, so we can't use __dict__ or vars() to get the attributes as a dict
message_dict = {}
for field in message.__slots__:
@@ -86,16 +87,18 @@ class RosMsgFieldCodec(FieldCodec):
for field_name, field_params in self.fields.items():
try:
field_codec = self.field_codecs[field_name]
- if not field_codec:
+ # Note that metadata encoding is done at the ros_msg_codec level, not here
+ if not field_codec or isinstance(field_codec, str):
continue
field_bits, encoded_dict[field_name] = field_codec.encode(message_dict[field_name])
encoded_bits.append(field_bits)
except Exception as e:
#print("Codec: {}, max len bits {}".format(field_codec, field_codec.max_length_bits))
- raise Exception('Error encoding field "{}" with codec {} (max len bits {})'.format(field_name, field_codec, field_codec.max_length_bits)).with_traceback(e.__traceback__)
+ raise Exception('Error encoding field "{}" with codec {} (max len bits {})'.format(field_name,
+ field_codec, field_codec.max_length_bits)).with_traceback(e.__traceback__)
return encoded_bits, encoded_dict
- def decode(self, bits_to_decode: ConstBitStream):
+ def decode(self, bits_to_decode: ConstBitStream, metadata=None):
# We go through the bits in sequence until we are decoded.
# The ConstBitStream has an internal read pointer.
decoded_message = {}
@@ -103,8 +106,25 @@ class RosMsgFieldCodec(FieldCodec):
field_codec = self.field_codecs[field_name]
if not field_codec:
continue
+ if isinstance(field_codec, str):
+ # try to decode it as a metadata codec
+ if not metadata:
+ continue
+ try:
+ if field_codec in ltcodecs.metadata_decoders:
+ metadata_attribute = ltcodecs.metadata_decoders[field_codec]
+ value = operator.attrgetter(metadata_attribute)(metadata)
+ setattr(decoded_message, field_name, value)
+ except KeyError:
+ # if we don't recognize the metadata codec, just keep going
+ continue
+
## print("Ros decode field {}".format(field_name))
- decoded_message[field_name] = field_codec.decode(bits_to_decode)
+ # pass metadata only to nested ros msg fields
+ if isinstance(field_codec, type(self)):
+ decoded_message[field_name] = field_codec.decode(bits_to_decode, metadata=metadata)
+ else:
+ decoded_message[field_name] = field_codec.decode(bits_to_decode)
## print("Ros decode got {}".format(decoded_message))
return self.ros_msg_class(**decoded_message)
@@ -129,4 +149,3 @@ class RosMsgFieldCodec(FieldCodec):
@property
def max_length_bits(self):
return sum([c.max_length_bits for c in self.field_codecs.values()])
-