Add serde
This commit is contained in:
Marko Durkovic
@@ -0,0 +1,19 @@
|
||||
# Copyright 2020-2021 Ternaris.
|
||||
# SPDX-License-Identifier: Apache-2.0
|
||||
"""Rosbags message serialization and deserialization.
|
||||
|
||||
Serializers and deserializers convert between python messages objects and
|
||||
the common rosbag serialization formats. Computationally cheap functions
|
||||
convert directly between different serialization formats.
|
||||
|
||||
"""
|
||||
|
||||
from .messages import SerdeError
|
||||
from .serdes import deserialize_cdr, ros1_to_cdr, serialize_cdr
|
||||
|
||||
__all__ = [
|
||||
'SerdeError',
|
||||
'deserialize_cdr',
|
||||
'ros1_to_cdr',
|
||||
'serialize_cdr',
|
||||
]
|
||||
@@ -0,0 +1,443 @@
|
||||
# Copyright 2020-2021 Ternaris.
|
||||
# SPDX-License-Identifier: Apache-2.0
|
||||
"""Code generators for CDR.
|
||||
|
||||
Common Data Representation `CDR`_ is the serialization format used by most ROS2
|
||||
middlewares.
|
||||
|
||||
.. _CDR: https://www.omg.org/cgi-bin/doc?formal/02-06-51
|
||||
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import sys
|
||||
from itertools import tee
|
||||
from typing import TYPE_CHECKING, Iterator, Optional, Tuple, cast
|
||||
|
||||
from .typing import Field
|
||||
from .utils import SIZEMAP, Valtype, align, align_after, compile_lines
|
||||
|
||||
if TYPE_CHECKING:
|
||||
from typing import Callable, List
|
||||
|
||||
|
||||
def generate_getsize_cdr(fields: List[Field]) -> Tuple[Callable, int]:
|
||||
"""Generate cdr size calculation function.
|
||||
|
||||
Args:
|
||||
fields: Fields of message.
|
||||
|
||||
Returns:
|
||||
Size calculation function and static size.
|
||||
|
||||
"""
|
||||
# pylint: disable=too-many-branches,too-many-locals,too-many-nested-blocks,too-many-statements
|
||||
size = 0
|
||||
is_stat = True
|
||||
|
||||
aligned = 8
|
||||
icurr, inext = cast(Tuple[Iterator[Field], Iterator[Optional[Field]]], tee([*fields, None]))
|
||||
next(inext)
|
||||
lines = [
|
||||
'import sys',
|
||||
'from rosbags.serde.messages import get_msgdef',
|
||||
'def getsize_cdr(pos, message):',
|
||||
]
|
||||
for fcurr, fnext in zip(icurr, inext):
|
||||
fieldname, desc = fcurr
|
||||
|
||||
if desc.valtype == Valtype.MESSAGE:
|
||||
if desc.args.size_cdr:
|
||||
lines.append(f' pos += {desc.args.size_cdr}')
|
||||
size += desc.args.size_cdr
|
||||
else:
|
||||
lines.append(f' func = get_msgdef("{desc.args.name}").getsize_cdr')
|
||||
lines.append(f' pos = func(pos, message.{fieldname})')
|
||||
is_stat = False
|
||||
aligned = align_after(desc)
|
||||
|
||||
elif desc.valtype == Valtype.BASE:
|
||||
if desc.args == 'string':
|
||||
lines.append(f' pos += 4 + len(message.{fieldname}.encode()) + 1')
|
||||
aligned = 1
|
||||
is_stat = False
|
||||
else:
|
||||
lines.append(f' pos += {SIZEMAP[desc.args]}')
|
||||
aligned = SIZEMAP[desc.args]
|
||||
size += SIZEMAP[desc.args]
|
||||
|
||||
elif desc.valtype == Valtype.ARRAY:
|
||||
subdesc = desc.args[1]
|
||||
|
||||
if subdesc.valtype == Valtype.BASE:
|
||||
if subdesc.args == 'string':
|
||||
lines.append(f' val = message.{fieldname}')
|
||||
for idx in range(desc.args[0]):
|
||||
lines.append(' pos = (pos + 4 - 1) & -4')
|
||||
lines.append(f' pos += 4 + len(val[{idx}].encode()) + 1')
|
||||
aligned = 1
|
||||
is_stat = False
|
||||
else:
|
||||
lines.append(f' pos += {desc.args[0] * SIZEMAP[subdesc.args]}')
|
||||
size += desc.args[0] * SIZEMAP[subdesc.args]
|
||||
|
||||
else:
|
||||
assert subdesc.valtype == Valtype.MESSAGE
|
||||
anext = align(subdesc)
|
||||
anext_after = align_after(subdesc)
|
||||
|
||||
if subdesc.args.size_cdr:
|
||||
for _ in range(desc.args[0]):
|
||||
if anext > anext_after:
|
||||
lines.append(f' pos = (pos + {anext} - 1) & -{anext}')
|
||||
size = (size + anext - 1) & -anext
|
||||
lines.append(f' pos += {subdesc.args.size_cdr}')
|
||||
size += subdesc.args.size_cdr
|
||||
else:
|
||||
lines.append(f' func = get_msgdef("{subdesc.args.name}").getsize_cdr')
|
||||
lines.append(f' val = message.{fieldname}')
|
||||
for idx in range(desc.args[0]):
|
||||
if anext > anext_after:
|
||||
lines.append(f' pos = (pos + {anext} - 1) & -{anext}')
|
||||
lines.append(f' pos = func(pos, val[{idx}])')
|
||||
is_stat = False
|
||||
aligned = align_after(subdesc)
|
||||
else:
|
||||
assert desc.valtype == Valtype.SEQUENCE
|
||||
lines.append(' pos += 4')
|
||||
aligned = 4
|
||||
subdesc = desc.args
|
||||
if subdesc.valtype == Valtype.BASE:
|
||||
if subdesc.args == 'string':
|
||||
lines.append(f' for val in message.{fieldname}:')
|
||||
lines.append(' pos = (pos + 4 - 1) & -4')
|
||||
lines.append(' pos += 4 + len(val.encode()) + 1')
|
||||
aligned = 1
|
||||
else:
|
||||
anext = align(subdesc)
|
||||
if aligned < anext:
|
||||
lines.append(f' pos = (pos + {anext} - 1) & -{anext}')
|
||||
aligned = anext
|
||||
lines.append(f' pos += len(message.{fieldname}) * {SIZEMAP[subdesc.args]}')
|
||||
|
||||
else:
|
||||
assert subdesc.valtype == Valtype.MESSAGE
|
||||
anext = align(subdesc)
|
||||
anext_after = align_after(subdesc)
|
||||
lines.append(f' val = message.{fieldname}')
|
||||
if subdesc.args.size_cdr:
|
||||
if aligned < anext <= anext_after:
|
||||
lines.append(f' pos = (pos + {anext} - 1) & -{anext}')
|
||||
lines.append(' for _ in val:')
|
||||
if anext > anext_after:
|
||||
lines.append(f' pos = (pos + {anext} - 1) & -{anext}')
|
||||
lines.append(f' pos += {subdesc.args.size_cdr}')
|
||||
|
||||
else:
|
||||
lines.append(f' func = get_msgdef("{subdesc.args.name}").getsize_cdr')
|
||||
if aligned < anext <= anext_after:
|
||||
lines.append(f' pos = (pos + {anext} - 1) & -{anext}')
|
||||
lines.append(' for item in val:')
|
||||
if anext > anext_after:
|
||||
lines.append(f' pos = (pos + {anext} - 1) & -{anext}')
|
||||
lines.append(' pos = func(pos, item)')
|
||||
aligned = align_after(subdesc)
|
||||
|
||||
aligned = min([aligned, 4])
|
||||
is_stat = False
|
||||
|
||||
if fnext and aligned < (anext := align(fnext.descriptor)):
|
||||
lines.append(f' pos = (pos + {anext} - 1) & -{anext}')
|
||||
aligned = anext
|
||||
is_stat = False
|
||||
lines.append(' return pos')
|
||||
return compile_lines(lines).getsize_cdr, is_stat * size # type: ignore
|
||||
|
||||
|
||||
def generate_serialize_cdr(fields: List[Field], endianess: str) -> Callable:
|
||||
"""Generate cdr serialization function.
|
||||
|
||||
Args:
|
||||
fields: Fields of message.
|
||||
endianess: Endianess of rawdata.
|
||||
|
||||
Returns:
|
||||
Serializer function.
|
||||
|
||||
"""
|
||||
# pylint: disable=too-many-branches,too-many-locals,too-many-statements
|
||||
aligned = 8
|
||||
icurr, inext = cast(Tuple[Iterator[Field], Iterator[Optional[Field]]], tee([*fields, None]))
|
||||
next(inext)
|
||||
lines = [
|
||||
'import sys',
|
||||
'import numpy',
|
||||
'from rosbags.serde.messages import SerdeError, get_msgdef',
|
||||
f'from rosbags.serde.primitives import pack_bool_{endianess}',
|
||||
f'from rosbags.serde.primitives import pack_int8_{endianess}',
|
||||
f'from rosbags.serde.primitives import pack_int16_{endianess}',
|
||||
f'from rosbags.serde.primitives import pack_int32_{endianess}',
|
||||
f'from rosbags.serde.primitives import pack_int64_{endianess}',
|
||||
f'from rosbags.serde.primitives import pack_uint8_{endianess}',
|
||||
f'from rosbags.serde.primitives import pack_uint16_{endianess}',
|
||||
f'from rosbags.serde.primitives import pack_uint32_{endianess}',
|
||||
f'from rosbags.serde.primitives import pack_uint64_{endianess}',
|
||||
f'from rosbags.serde.primitives import pack_float32_{endianess}',
|
||||
f'from rosbags.serde.primitives import pack_float64_{endianess}',
|
||||
'def serialize_cdr(rawdata, pos, message):',
|
||||
]
|
||||
for fcurr, fnext in zip(icurr, inext):
|
||||
fieldname, desc = fcurr
|
||||
|
||||
lines.append(f' val = message.{fieldname}')
|
||||
if desc.valtype == Valtype.MESSAGE:
|
||||
lines.append(f' func = get_msgdef("{desc.args.name}").serialize_cdr_{endianess}')
|
||||
lines.append(' pos = func(rawdata, pos, val)')
|
||||
aligned = align_after(desc)
|
||||
|
||||
elif desc.valtype == Valtype.BASE:
|
||||
if desc.args == 'string':
|
||||
lines.append(' bval = memoryview(val.encode())')
|
||||
lines.append(' length = len(bval) + 1')
|
||||
lines.append(f' pack_int32_{endianess}(rawdata, pos, length)')
|
||||
lines.append(' pos += 4')
|
||||
lines.append(' rawdata[pos:pos + length - 1] = bval')
|
||||
lines.append(' pos += length')
|
||||
aligned = 1
|
||||
else:
|
||||
lines.append(f' pack_{desc.args}_{endianess}(rawdata, pos, val)')
|
||||
lines.append(f' pos += {SIZEMAP[desc.args]}')
|
||||
aligned = SIZEMAP[desc.args]
|
||||
|
||||
elif desc.valtype == Valtype.ARRAY:
|
||||
subdesc = desc.args[1]
|
||||
lines.append(f' if len(val) != {desc.args[0]}:')
|
||||
lines.append(' raise SerdeError(\'Unexpected array length\')')
|
||||
|
||||
if subdesc.valtype == Valtype.BASE:
|
||||
if subdesc.args == 'string':
|
||||
for idx in range(desc.args[0]):
|
||||
lines.append(f' bval = memoryview(val[{idx}].encode())')
|
||||
lines.append(' length = len(bval) + 1')
|
||||
lines.append(' pos = (pos + 4 - 1) & -4')
|
||||
lines.append(f' pack_int32_{endianess}(rawdata, pos, length)')
|
||||
lines.append(' pos += 4')
|
||||
lines.append(' rawdata[pos:pos + length - 1] = bval')
|
||||
lines.append(' pos += length')
|
||||
aligned = 1
|
||||
else:
|
||||
if (endianess == 'le') != (sys.byteorder == 'little'):
|
||||
lines.append(' val = val.byteswap()')
|
||||
size = desc.args[0] * SIZEMAP[subdesc.args]
|
||||
lines.append(f' rawdata[pos:pos + {size}] = val.view(numpy.uint8)')
|
||||
lines.append(f' pos += {size}')
|
||||
|
||||
else:
|
||||
assert subdesc.valtype == Valtype.MESSAGE
|
||||
anext = align(subdesc)
|
||||
anext_after = align_after(subdesc)
|
||||
lines.append(
|
||||
f' func = get_msgdef("{subdesc.args.name}").serialize_cdr_{endianess}',
|
||||
)
|
||||
for idx in range(desc.args[0]):
|
||||
if anext > anext_after:
|
||||
lines.append(f' pos = (pos + {anext} - 1) & -{anext}')
|
||||
lines.append(f' pos = func(rawdata, pos, val[{idx}])')
|
||||
aligned = align_after(subdesc)
|
||||
else:
|
||||
assert desc.valtype == Valtype.SEQUENCE
|
||||
lines.append(f' pack_int32_{endianess}(rawdata, pos, len(val))')
|
||||
lines.append(' pos += 4')
|
||||
aligned = 4
|
||||
subdesc = desc.args
|
||||
|
||||
if subdesc.valtype == Valtype.BASE:
|
||||
if subdesc.args == 'string':
|
||||
lines.append(' for item in val:')
|
||||
lines.append(' bval = memoryview(item.encode())')
|
||||
lines.append(' length = len(bval) + 1')
|
||||
lines.append(' pos = (pos + 4 - 1) & -4')
|
||||
lines.append(f' pack_int32_{endianess}(rawdata, pos, length)')
|
||||
lines.append(' pos += 4')
|
||||
lines.append(' rawdata[pos:pos + length - 1] = bval')
|
||||
lines.append(' pos += length')
|
||||
aligned = 1
|
||||
else:
|
||||
lines.append(f' size = len(val) * {SIZEMAP[subdesc.args]}')
|
||||
if (endianess == 'le') != (sys.byteorder == 'little'):
|
||||
lines.append(' val = val.byteswap()')
|
||||
if aligned < (anext := align(subdesc)):
|
||||
lines.append(f' pos = (pos + {anext} - 1) & -{anext}')
|
||||
lines.append(' rawdata[pos:pos + size] = val.view(numpy.uint8)')
|
||||
lines.append(' pos += size')
|
||||
aligned = anext
|
||||
|
||||
if subdesc.valtype == Valtype.MESSAGE:
|
||||
anext = align(subdesc)
|
||||
lines.append(
|
||||
f' func = get_msgdef("{subdesc.args.name}").serialize_cdr_{endianess}',
|
||||
)
|
||||
lines.append(' for item in val:')
|
||||
lines.append(f' pos = (pos + {anext} - 1) & -{anext}')
|
||||
lines.append(' pos = func(rawdata, pos, item)')
|
||||
aligned = align_after(subdesc)
|
||||
|
||||
aligned = min([4, aligned])
|
||||
|
||||
if fnext and aligned < (anext := align(fnext.descriptor)):
|
||||
lines.append(f' pos = (pos + {anext} - 1) & -{anext}')
|
||||
aligned = anext
|
||||
lines.append(' return pos')
|
||||
return compile_lines(lines).serialize_cdr # type: ignore
|
||||
|
||||
|
||||
def generate_deserialize_cdr(fields: List[Field], endianess: str) -> Callable:
|
||||
"""Generate cdr deserialization function.
|
||||
|
||||
Args:
|
||||
fields: Fields of message.
|
||||
endianess: Endianess of rawdata.
|
||||
|
||||
Returns:
|
||||
Deserializer function.
|
||||
|
||||
"""
|
||||
# pylint: disable=too-many-branches,too-many-locals,too-many-nested-blocks,too-many-statements
|
||||
aligned = 8
|
||||
icurr, inext = cast(Tuple[Iterator[Field], Iterator[Optional[Field]]], tee([*fields, None]))
|
||||
next(inext)
|
||||
lines = [
|
||||
'import sys',
|
||||
'import numpy',
|
||||
'from rosbags.serde.messages import SerdeError, get_msgdef',
|
||||
f'from rosbags.serde.primitives import unpack_bool_{endianess}',
|
||||
f'from rosbags.serde.primitives import unpack_int8_{endianess}',
|
||||
f'from rosbags.serde.primitives import unpack_int16_{endianess}',
|
||||
f'from rosbags.serde.primitives import unpack_int32_{endianess}',
|
||||
f'from rosbags.serde.primitives import unpack_int64_{endianess}',
|
||||
f'from rosbags.serde.primitives import unpack_uint8_{endianess}',
|
||||
f'from rosbags.serde.primitives import unpack_uint16_{endianess}',
|
||||
f'from rosbags.serde.primitives import unpack_uint32_{endianess}',
|
||||
f'from rosbags.serde.primitives import unpack_uint64_{endianess}',
|
||||
f'from rosbags.serde.primitives import unpack_float32_{endianess}',
|
||||
f'from rosbags.serde.primitives import unpack_float64_{endianess}',
|
||||
'def deserialize_cdr(rawdata, pos, cls):',
|
||||
]
|
||||
|
||||
funcname = f'deserialize_cdr_{endianess}'
|
||||
lines.append(' values = []')
|
||||
for fcurr, fnext in zip(icurr, inext):
|
||||
desc = fcurr[1]
|
||||
|
||||
if desc.valtype == Valtype.MESSAGE:
|
||||
lines.append(f' msgdef = get_msgdef("{desc.args.name}")')
|
||||
lines.append(f' obj, pos = msgdef.{funcname}(rawdata, pos, msgdef.cls)')
|
||||
lines.append(' values.append(obj)')
|
||||
aligned = align_after(desc)
|
||||
|
||||
elif desc.valtype == Valtype.BASE:
|
||||
if desc.args == 'string':
|
||||
lines.append(f' length = unpack_int32_{endianess}(rawdata, pos)[0]')
|
||||
lines.append(' string = bytes(rawdata[pos + 4:pos + 4 + length - 1]).decode()')
|
||||
lines.append(' values.append(string)')
|
||||
lines.append(' pos += 4 + length')
|
||||
aligned = 1
|
||||
else:
|
||||
lines.append(f' value = unpack_{desc.args}_{endianess}(rawdata, pos)[0]')
|
||||
lines.append(' values.append(value)')
|
||||
lines.append(f' pos += {SIZEMAP[desc.args]}')
|
||||
aligned = SIZEMAP[desc.args]
|
||||
|
||||
elif desc.valtype == Valtype.ARRAY:
|
||||
subdesc = desc.args[1]
|
||||
if subdesc.valtype == Valtype.BASE:
|
||||
if subdesc.args == 'string':
|
||||
lines.append(' value = []')
|
||||
for idx in range(desc.args[0]):
|
||||
if idx:
|
||||
lines.append(' pos = (pos + 4 - 1) & -4')
|
||||
lines.append(f' length = unpack_int32_{endianess}(rawdata, pos)[0]')
|
||||
lines.append(
|
||||
' value.append(bytes(rawdata[pos + 4:pos + 4 + length - 1]).decode())',
|
||||
)
|
||||
lines.append(' pos += 4 + length')
|
||||
lines.append(' values.append(value)')
|
||||
aligned = 1
|
||||
else:
|
||||
size = desc.args[0] * SIZEMAP[subdesc.args]
|
||||
lines.append(
|
||||
f' val = numpy.frombuffer(rawdata, '
|
||||
f'dtype=numpy.{subdesc.args}, count={desc.args[0]}, offset=pos)',
|
||||
)
|
||||
if (endianess == 'le') != (sys.byteorder == 'little'):
|
||||
lines.append(' val = val.byteswap()')
|
||||
lines.append(' values.append(val)')
|
||||
lines.append(f' pos += {size}')
|
||||
else:
|
||||
assert subdesc.valtype == Valtype.MESSAGE
|
||||
anext = align(subdesc)
|
||||
anext_after = align_after(subdesc)
|
||||
lines.append(f' msgdef = get_msgdef("{subdesc.args.name}")')
|
||||
lines.append(' value = []')
|
||||
for _ in range(desc.args[0]):
|
||||
if anext > anext_after:
|
||||
lines.append(f' pos = (pos + {anext} - 1) & -{anext}')
|
||||
lines.append(f' obj, pos = msgdef.{funcname}(rawdata, pos, msgdef.cls)')
|
||||
lines.append(' value.append(obj)')
|
||||
lines.append(' values.append(value)')
|
||||
aligned = align_after(subdesc)
|
||||
|
||||
else:
|
||||
assert desc.valtype == Valtype.SEQUENCE
|
||||
lines.append(f' size = unpack_int32_{endianess}(rawdata, pos)[0]')
|
||||
lines.append(' pos += 4')
|
||||
aligned = 4
|
||||
subdesc = desc.args
|
||||
|
||||
if subdesc.valtype == Valtype.BASE:
|
||||
if subdesc.args == 'string':
|
||||
lines.append(' value = []')
|
||||
lines.append(' for _ in range(size):')
|
||||
lines.append(' pos = (pos + 4 - 1) & -4')
|
||||
lines.append(f' length = unpack_int32_{endianess}(rawdata, pos)[0]')
|
||||
lines.append(
|
||||
' value.append(bytes(rawdata[pos + 4:pos + 4 + length - 1])'
|
||||
'.decode())',
|
||||
)
|
||||
lines.append(' pos += 4 + length')
|
||||
lines.append(' values.append(value)')
|
||||
aligned = 1
|
||||
else:
|
||||
lines.append(f' length = size * {SIZEMAP[subdesc.args]}')
|
||||
if aligned < (anext := align(subdesc)):
|
||||
lines.append(f' pos = (pos + {anext} - 1) & -{anext}')
|
||||
lines.append(
|
||||
f' val = numpy.frombuffer(rawdata, '
|
||||
f'dtype=numpy.{subdesc.args}, count=size, offset=pos)',
|
||||
)
|
||||
if (endianess == 'le') != (sys.byteorder == 'little'):
|
||||
lines.append(' val = val.byteswap()')
|
||||
lines.append(' values.append(val)')
|
||||
lines.append(' pos += length')
|
||||
aligned = anext
|
||||
|
||||
if subdesc.valtype == Valtype.MESSAGE:
|
||||
anext = align(subdesc)
|
||||
lines.append(f' msgdef = get_msgdef("{subdesc.args.name}")')
|
||||
lines.append(' value = []')
|
||||
lines.append(' for _ in range(size):')
|
||||
lines.append(f' pos = (pos + {anext} - 1) & -{anext}')
|
||||
lines.append(f' obj, pos = msgdef.{funcname}(rawdata, pos, msgdef.cls)')
|
||||
lines.append(' value.append(obj)')
|
||||
lines.append(' values.append(value)')
|
||||
aligned = align_after(subdesc)
|
||||
|
||||
aligned = min([4, aligned])
|
||||
|
||||
if fnext and aligned < (anext := align(fnext.descriptor)):
|
||||
lines.append(f' pos = (pos + {anext} - 1) & -{anext}')
|
||||
aligned = anext
|
||||
|
||||
lines.append(' return cls(*values), pos')
|
||||
return compile_lines(lines).deserialize_cdr # type: ignore
|
||||
@@ -0,0 +1,72 @@
|
||||
# Copyright 2020-2021 Ternaris.
|
||||
# SPDX-License-Identifier: Apache-2.0
|
||||
"""Runtime message loader and cache."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from typing import TYPE_CHECKING
|
||||
|
||||
from rosbags.typesys import types
|
||||
|
||||
from .cdr import generate_deserialize_cdr, generate_getsize_cdr, generate_serialize_cdr
|
||||
from .ros1 import generate_ros1_to_cdr
|
||||
from .typing import Field, Msgdef
|
||||
from .utils import Descriptor, Valtype
|
||||
|
||||
if TYPE_CHECKING:
|
||||
from typing import Any, Dict
|
||||
|
||||
|
||||
MSGDEFCACHE: Dict[str, Msgdef] = {}
|
||||
|
||||
|
||||
class SerdeError(Exception):
|
||||
"""Serialization and Deserialization Error."""
|
||||
|
||||
|
||||
def get_msgdef(typename: str) -> Msgdef:
|
||||
"""Retrieve message definition for typename.
|
||||
|
||||
Message definitions are cached globally and generated as needed.
|
||||
|
||||
Args:
|
||||
typename: Msgdef type name to load.
|
||||
|
||||
Returns:
|
||||
Message definition.
|
||||
|
||||
"""
|
||||
if typename not in MSGDEFCACHE:
|
||||
entries = types.FIELDDEFS[typename]
|
||||
|
||||
def fixup(entry: Any) -> Descriptor:
|
||||
if entry[0] == Valtype.BASE:
|
||||
return Descriptor(Valtype.BASE, entry[1])
|
||||
if entry[0] == Valtype.MESSAGE:
|
||||
return Descriptor(Valtype.MESSAGE, get_msgdef(entry[1]))
|
||||
if entry[0] == Valtype.ARRAY:
|
||||
return Descriptor(Valtype.ARRAY, (entry[1], fixup(entry[2])))
|
||||
if entry[0] == Valtype.SEQUENCE:
|
||||
return Descriptor(Valtype.SEQUENCE, fixup(entry[1]))
|
||||
raise SerdeError( # pragma: no cover
|
||||
f'Unknown field type {entry[0]!r} encountered.',
|
||||
)
|
||||
|
||||
fields = [Field(name, fixup(desc)) for name, desc in entries]
|
||||
|
||||
getsize_cdr, size_cdr = generate_getsize_cdr(fields)
|
||||
|
||||
MSGDEFCACHE[typename] = Msgdef(
|
||||
typename,
|
||||
fields,
|
||||
getattr(types, typename.replace('/', '__')),
|
||||
size_cdr,
|
||||
getsize_cdr,
|
||||
generate_serialize_cdr(fields, 'le'),
|
||||
generate_serialize_cdr(fields, 'be'),
|
||||
generate_deserialize_cdr(fields, 'le'),
|
||||
generate_deserialize_cdr(fields, 'be'),
|
||||
generate_ros1_to_cdr(fields, typename, False),
|
||||
generate_ros1_to_cdr(fields, typename, True),
|
||||
)
|
||||
return MSGDEFCACHE[typename]
|
||||
@@ -0,0 +1,55 @@
|
||||
# Copyright 2020-2021 Ternaris.
|
||||
# SPDX-License-Identifier: Apache-2.0
|
||||
"""Serialization primitives.
|
||||
|
||||
These functions are used by generated code to serialize and desesialize
|
||||
primitive values.
|
||||
|
||||
"""
|
||||
|
||||
from struct import Struct
|
||||
|
||||
pack_bool_le = Struct('?').pack_into
|
||||
pack_int8_le = Struct('b').pack_into
|
||||
pack_int16_le = Struct('<h').pack_into
|
||||
pack_int32_le = Struct('<i').pack_into
|
||||
pack_int64_le = Struct('<q').pack_into
|
||||
pack_uint8_le = Struct('B').pack_into
|
||||
pack_uint16_le = Struct('<H').pack_into
|
||||
pack_uint32_le = Struct('<I').pack_into
|
||||
pack_uint64_le = Struct('<Q').pack_into
|
||||
pack_float32_le = Struct('<f').pack_into
|
||||
pack_float64_le = Struct('<d').pack_into
|
||||
unpack_bool_le = Struct('?').unpack_from
|
||||
unpack_int8_le = Struct('b').unpack_from
|
||||
unpack_int16_le = Struct('<h').unpack_from
|
||||
unpack_int32_le = Struct('<i').unpack_from
|
||||
unpack_int64_le = Struct('<q').unpack_from
|
||||
unpack_uint8_le = Struct('B').unpack_from
|
||||
unpack_uint16_le = Struct('<H').unpack_from
|
||||
unpack_uint32_le = Struct('<I').unpack_from
|
||||
unpack_uint64_le = Struct('<Q').unpack_from
|
||||
unpack_float32_le = Struct('<f').unpack_from
|
||||
unpack_float64_le = Struct('<d').unpack_from
|
||||
pack_bool_be = Struct('?').pack_into
|
||||
pack_int8_be = Struct('b').pack_into
|
||||
pack_int16_be = Struct('>h').pack_into
|
||||
pack_int32_be = Struct('>i').pack_into
|
||||
pack_int64_be = Struct('>q').pack_into
|
||||
pack_uint8_be = Struct('B').pack_into
|
||||
pack_uint16_be = Struct('>H').pack_into
|
||||
pack_uint32_be = Struct('>I').pack_into
|
||||
pack_uint64_be = Struct('>Q').pack_into
|
||||
pack_float32_be = Struct('>f').pack_into
|
||||
pack_float64_be = Struct('>d').pack_into
|
||||
unpack_bool_be = Struct('?').unpack_from
|
||||
unpack_int8_be = Struct('b').unpack_from
|
||||
unpack_int16_be = Struct('>h').unpack_from
|
||||
unpack_int32_be = Struct('>i').unpack_from
|
||||
unpack_int64_be = Struct('>q').unpack_from
|
||||
unpack_uint8_be = Struct('B').unpack_from
|
||||
unpack_uint16_be = Struct('>H').unpack_from
|
||||
unpack_uint32_be = Struct('>I').unpack_from
|
||||
unpack_uint64_be = Struct('>Q').unpack_from
|
||||
unpack_float32_be = Struct('>f').unpack_from
|
||||
unpack_float64_be = Struct('>d').unpack_from
|
||||
@@ -0,0 +1,180 @@
|
||||
# Copyright 2020-2021 Ternaris.
|
||||
# SPDX-License-Identifier: Apache-2.0
|
||||
"""Code generators for ROS1.
|
||||
|
||||
`ROS1`_ uses a serialization format. This module supports fast byte-level
|
||||
conversion of ROS1 to CDR.
|
||||
|
||||
.. _ROS1: http://wiki.ros.org/ROS/Technical%20Overview
|
||||
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from itertools import tee
|
||||
from typing import TYPE_CHECKING, Iterator, Optional, Tuple, cast
|
||||
|
||||
from .typing import Field
|
||||
from .utils import SIZEMAP, Valtype, align, align_after, compile_lines
|
||||
|
||||
if TYPE_CHECKING:
|
||||
from typing import Callable, List # pylint: disable=ungrouped-imports
|
||||
|
||||
|
||||
def generate_ros1_to_cdr(fields: List[Field], typename: str, copy: bool) -> Callable:
|
||||
"""Generate CDR serialization function.
|
||||
|
||||
Args:
|
||||
fields: Fields of message.
|
||||
typename: Message type name.
|
||||
copy: Generate serialization or sizing function.
|
||||
|
||||
Returns:
|
||||
ROS1 to CDR conversion function.
|
||||
|
||||
"""
|
||||
# pylint: disable=too-many-branches,too-many-locals,too-many-nested-blocks,too-many-statements
|
||||
aligned = 8
|
||||
icurr, inext = cast(Tuple[Iterator[Field], Iterator[Optional[Field]]], tee([*fields, None]))
|
||||
next(inext)
|
||||
funcname = 'ros1_to_cdr' if copy else 'getsize_ros1_to_cdr'
|
||||
lines = [
|
||||
'import sys',
|
||||
'import numpy',
|
||||
'from rosbags.serde.messages import SerdeError, get_msgdef',
|
||||
'from rosbags.serde.primitives import pack_bool_le',
|
||||
'from rosbags.serde.primitives import pack_int8_le',
|
||||
'from rosbags.serde.primitives import pack_int16_le',
|
||||
'from rosbags.serde.primitives import pack_int32_le',
|
||||
'from rosbags.serde.primitives import pack_int64_le',
|
||||
'from rosbags.serde.primitives import pack_uint8_le',
|
||||
'from rosbags.serde.primitives import pack_uint16_le',
|
||||
'from rosbags.serde.primitives import pack_uint32_le',
|
||||
'from rosbags.serde.primitives import pack_uint64_le',
|
||||
'from rosbags.serde.primitives import pack_float32_le',
|
||||
'from rosbags.serde.primitives import pack_float64_le',
|
||||
'from rosbags.serde.primitives import unpack_int32_le',
|
||||
f'def {funcname}(input, ipos, output, opos):',
|
||||
]
|
||||
|
||||
if typename == 'std_msgs/msg/Header':
|
||||
lines.append(' ipos += 4')
|
||||
|
||||
for fcurr, fnext in zip(icurr, inext):
|
||||
_, desc = fcurr
|
||||
|
||||
if desc.valtype == Valtype.MESSAGE:
|
||||
lines.append(f' func = get_msgdef("{desc.args.name}").{funcname}')
|
||||
lines.append(' ipos, opos = func(input, ipos, output, opos)')
|
||||
aligned = align_after(desc)
|
||||
|
||||
elif desc.valtype == Valtype.BASE:
|
||||
if desc.args == 'string':
|
||||
lines.append(' length = unpack_int32_le(input, ipos)[0] + 1')
|
||||
if copy:
|
||||
lines.append(' pack_int32_le(output, opos, length)')
|
||||
lines.append(' ipos += 4')
|
||||
lines.append(' opos += 4')
|
||||
if copy:
|
||||
lines.append(' output[opos:opos + length - 1] = input[ipos:ipos + length - 1]')
|
||||
lines.append(' ipos += length - 1')
|
||||
lines.append(' opos += length')
|
||||
aligned = 1
|
||||
else:
|
||||
size = SIZEMAP[desc.args]
|
||||
if copy:
|
||||
lines.append(f' output[opos:opos + {size}] = input[ipos:ipos + {size}]')
|
||||
lines.append(f' ipos += {size}')
|
||||
lines.append(f' opos += {size}')
|
||||
aligned = size
|
||||
|
||||
elif desc.valtype == Valtype.ARRAY:
|
||||
subdesc = desc.args[1]
|
||||
|
||||
if subdesc.valtype == Valtype.BASE:
|
||||
if subdesc.args == 'string':
|
||||
for _ in range(desc.args[0]):
|
||||
lines.append(' opos = (opos + 4 - 1) & -4')
|
||||
lines.append(' length = unpack_int32_le(input, ipos)[0] + 1')
|
||||
if copy:
|
||||
lines.append(' pack_int32_le(output, opos, length)')
|
||||
lines.append(' ipos += 4')
|
||||
lines.append(' opos += 4')
|
||||
if copy:
|
||||
lines.append(
|
||||
' output[opos:opos + length - 1] = input[ipos:ipos + length - 1]',
|
||||
)
|
||||
lines.append(' ipos += length - 1')
|
||||
lines.append(' opos += length')
|
||||
aligned = 1
|
||||
else:
|
||||
size = desc.args[0] * SIZEMAP[subdesc.args]
|
||||
if copy:
|
||||
lines.append(f' output[opos:opos + {size}] = input[ipos:ipos + {size}]')
|
||||
lines.append(f' ipos += {size}')
|
||||
lines.append(f' opos += {size}')
|
||||
aligned = SIZEMAP[subdesc.args]
|
||||
|
||||
if subdesc.valtype == Valtype.MESSAGE:
|
||||
anext = align(subdesc)
|
||||
anext_after = align_after(subdesc)
|
||||
|
||||
lines.append(f' func = get_msgdef("{subdesc.args.name}").{funcname}')
|
||||
for _ in range(desc.args[0]):
|
||||
if anext > anext_after:
|
||||
lines.append(f' opos = (opos + {anext} - 1) & -{anext}')
|
||||
lines.append(' ipos, opos = func(input, ipos, output, opos)')
|
||||
aligned = anext_after
|
||||
else:
|
||||
assert desc.valtype == Valtype.SEQUENCE
|
||||
lines.append(' size = unpack_int32_le(input, ipos)[0]')
|
||||
if copy:
|
||||
lines.append(' pack_int32_le(output, opos, size)')
|
||||
lines.append(' ipos += 4')
|
||||
lines.append(' opos += 4')
|
||||
subdesc = desc.args
|
||||
aligned = 4
|
||||
|
||||
if subdesc.valtype == Valtype.BASE:
|
||||
if subdesc.args == 'string':
|
||||
lines.append(' for _ in range(size):')
|
||||
lines.append(' length = unpack_int32_le(input, ipos)[0] + 1')
|
||||
lines.append(' opos = (opos + 4 - 1) & -4')
|
||||
if copy:
|
||||
lines.append(' pack_int32_le(output, opos, length)')
|
||||
lines.append(' ipos += 4')
|
||||
lines.append(' opos += 4')
|
||||
if copy:
|
||||
lines.append(
|
||||
' output[opos:opos + length - 1] = input[ipos:ipos + length - 1]',
|
||||
)
|
||||
lines.append(' ipos += length - 1')
|
||||
lines.append(' opos += length')
|
||||
aligned = 1
|
||||
else:
|
||||
if aligned < (anext := align(subdesc)):
|
||||
lines.append(f' opos = (opos + {anext} - 1) & -{anext}')
|
||||
lines.append(f' length = size * {SIZEMAP[subdesc.args]}')
|
||||
if copy:
|
||||
lines.append(' output[opos:opos + length] = input[ipos:ipos + length]')
|
||||
lines.append(' ipos += length')
|
||||
lines.append(' opos += length')
|
||||
aligned = anext
|
||||
|
||||
else:
|
||||
assert subdesc.valtype == Valtype.MESSAGE
|
||||
anext = align(subdesc)
|
||||
lines.append(f' func = get_msgdef("{subdesc.args.name}").{funcname}')
|
||||
lines.append(' for _ in range(size):')
|
||||
lines.append(f' opos = (opos + {anext} - 1) & -{anext}')
|
||||
lines.append(' ipos, opos = func(input, ipos, output, opos)')
|
||||
aligned = align_after(subdesc)
|
||||
|
||||
aligned = min([aligned, 4])
|
||||
|
||||
if fnext and aligned < (anext := align(fnext.descriptor)):
|
||||
lines.append(f' opos = (opos + {anext} - 1) & -{anext}')
|
||||
aligned = anext
|
||||
|
||||
lines.append(' return ipos, opos')
|
||||
return getattr(compile_lines(lines), funcname)
|
||||
@@ -0,0 +1,102 @@
|
||||
# Copyright 2020-2021 Ternaris.
|
||||
# SPDX-License-Identifier: Apache-2.0
|
||||
"""Serialization, deserializion and conversion functions."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import sys
|
||||
from struct import pack_into
|
||||
from typing import TYPE_CHECKING
|
||||
|
||||
from .messages import get_msgdef
|
||||
|
||||
if TYPE_CHECKING:
|
||||
from typing import Any
|
||||
|
||||
|
||||
def deserialize_cdr(rawdata: bytes, typename: str) -> Any:
|
||||
"""Deserialize raw data into a message object.
|
||||
|
||||
Args:
|
||||
rawdata: Serialized data.
|
||||
typename: Message type name.
|
||||
|
||||
Returns:
|
||||
Deserialized message object.
|
||||
|
||||
"""
|
||||
little_endian = bool(rawdata[1])
|
||||
|
||||
msgdef = get_msgdef(typename)
|
||||
func = msgdef.deserialize_cdr_le if little_endian else msgdef.deserialize_cdr_be
|
||||
message, pos = func(rawdata[4:], 0, msgdef.cls)
|
||||
assert pos + 4 + 3 >= len(rawdata)
|
||||
return message
|
||||
|
||||
|
||||
def serialize_cdr(
|
||||
message: Any,
|
||||
typename: str,
|
||||
little_endian: bool = sys.byteorder == 'little',
|
||||
) -> memoryview:
|
||||
"""Serialize message object to bytes.
|
||||
|
||||
Args:
|
||||
message: Message object.
|
||||
typename: Message type name.
|
||||
little_endian: Should use little endianess.
|
||||
|
||||
Returns:
|
||||
Serialized bytes.
|
||||
|
||||
"""
|
||||
msgdef = get_msgdef(typename)
|
||||
size = 4 + msgdef.getsize_cdr(0, message)
|
||||
rawdata = memoryview(bytearray(size))
|
||||
pack_into('BB', rawdata, 0, 0, little_endian)
|
||||
|
||||
func = msgdef.serialize_cdr_le if little_endian else msgdef.serialize_cdr_be
|
||||
|
||||
pos = func(rawdata[4:], 0, message)
|
||||
assert pos + 4 == size
|
||||
return rawdata.toreadonly()
|
||||
|
||||
|
||||
def ros1_to_cdr(raw: bytes, typename: str) -> memoryview:
|
||||
"""Convert serialized ROS1 message directly to CDR.
|
||||
|
||||
This should be reasonably fast as conversions happen on a byte-level
|
||||
without going through deserialization and serialization.
|
||||
|
||||
Args:
|
||||
raw: ROS1 serialized message.
|
||||
typename: Message type name.
|
||||
|
||||
Returns:
|
||||
CDR serialized message.
|
||||
|
||||
"""
|
||||
msgdef = get_msgdef(typename)
|
||||
|
||||
ipos, opos = msgdef.getsize_ros1_to_cdr(
|
||||
raw,
|
||||
0,
|
||||
None,
|
||||
0,
|
||||
)
|
||||
assert ipos == len(raw)
|
||||
|
||||
raw = memoryview(raw)
|
||||
size = 4 + opos
|
||||
rawdata = memoryview(bytearray(size))
|
||||
pack_into('BB', rawdata, 0, 0, True)
|
||||
|
||||
ipos, opos = msgdef.ros1_to_cdr(
|
||||
raw,
|
||||
0,
|
||||
rawdata[4:],
|
||||
0,
|
||||
)
|
||||
assert ipos == len(raw)
|
||||
assert opos + 4 == size
|
||||
return rawdata.toreadonly()
|
||||
@@ -0,0 +1,35 @@
|
||||
# Copyright 2020-2021 Ternaris.
|
||||
# SPDX-License-Identifier: Apache-2.0
|
||||
"""Python types used in this package."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from typing import TYPE_CHECKING, NamedTuple
|
||||
|
||||
if TYPE_CHECKING:
|
||||
from typing import Any, Callable, List # pylint: disable=ungrouped-imports
|
||||
|
||||
from .utils import Descriptor
|
||||
|
||||
|
||||
class Field(NamedTuple):
|
||||
"""Metadata of a field."""
|
||||
|
||||
name: str
|
||||
descriptor: Descriptor
|
||||
|
||||
|
||||
class Msgdef(NamedTuple):
|
||||
"""Metadata of a message."""
|
||||
|
||||
name: str
|
||||
fields: List[Field]
|
||||
cls: Any
|
||||
size_cdr: int
|
||||
getsize_cdr: Callable
|
||||
serialize_cdr_le: Callable
|
||||
serialize_cdr_be: Callable
|
||||
deserialize_cdr_le: Callable
|
||||
deserialize_cdr_be: Callable
|
||||
getsize_ros1_to_cdr: Callable
|
||||
ros1_to_cdr: Callable
|
||||
@@ -0,0 +1,103 @@
|
||||
# Copyright 2020-2021 Ternaris.
|
||||
# SPDX-License-Identifier: Apache-2.0
|
||||
"""Helpers used by code generators."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from enum import IntEnum
|
||||
from importlib.util import module_from_spec, spec_from_loader
|
||||
from typing import TYPE_CHECKING, NamedTuple
|
||||
|
||||
if TYPE_CHECKING:
|
||||
from types import ModuleType
|
||||
from typing import Any, Dict, List
|
||||
|
||||
|
||||
class Valtype(IntEnum):
|
||||
"""Msg field value types."""
|
||||
|
||||
BASE = 1
|
||||
MESSAGE = 2
|
||||
ARRAY = 3
|
||||
SEQUENCE = 4
|
||||
|
||||
|
||||
class Descriptor(NamedTuple):
|
||||
"""Value type descriptor."""
|
||||
|
||||
valtype: Valtype
|
||||
args: Any # Union[Descriptor, Msgdef, Tuple[int, Descriptor], str]
|
||||
|
||||
|
||||
SIZEMAP: Dict[str, int] = {
|
||||
'bool': 1,
|
||||
'int8': 1,
|
||||
'int16': 2,
|
||||
'int32': 4,
|
||||
'int64': 8,
|
||||
'uint8': 1,
|
||||
'uint16': 2,
|
||||
'uint32': 4,
|
||||
'uint64': 8,
|
||||
'float32': 4,
|
||||
'float64': 8,
|
||||
}
|
||||
|
||||
|
||||
def align(entry: Descriptor) -> int:
|
||||
"""Get alignment requirement for entry.
|
||||
|
||||
Args:
|
||||
entry: Field.
|
||||
|
||||
Returns:
|
||||
Required alignment in bytes.
|
||||
|
||||
"""
|
||||
if entry.valtype == Valtype.BASE:
|
||||
if entry.args == 'string':
|
||||
return 4
|
||||
return SIZEMAP[entry.args]
|
||||
if entry.valtype == Valtype.MESSAGE:
|
||||
return align(entry.args.fields[0].descriptor)
|
||||
if entry.valtype == Valtype.ARRAY:
|
||||
return align(entry.args[1])
|
||||
assert entry.valtype == Valtype.SEQUENCE
|
||||
return 4
|
||||
|
||||
|
||||
def align_after(entry: Descriptor) -> int:
|
||||
"""Get alignment after entry.
|
||||
|
||||
Args:
|
||||
entry: Field.
|
||||
|
||||
Returns:
|
||||
Memory alignment after entry.
|
||||
|
||||
"""
|
||||
if entry.valtype == Valtype.BASE:
|
||||
if entry.args == 'string':
|
||||
return 1
|
||||
return SIZEMAP[entry.args]
|
||||
if entry.valtype == Valtype.MESSAGE:
|
||||
return align_after(entry.args.fields[-1].descriptor)
|
||||
if entry.valtype == Valtype.ARRAY:
|
||||
return align_after(entry.args[1])
|
||||
assert entry.valtype == Valtype.SEQUENCE
|
||||
return min([4, align_after(entry.args)])
|
||||
|
||||
|
||||
def compile_lines(lines: List[str]) -> ModuleType:
|
||||
"""Compile lines of code to module.
|
||||
|
||||
Args:
|
||||
lines: Lines of python code.
|
||||
|
||||
Returns:
|
||||
Compiled and loaded module.
|
||||
|
||||
"""
|
||||
module = module_from_spec(spec_from_loader('tmpmod', loader=None))
|
||||
exec('\n'.join(lines), module.__dict__) # pylint: disable=exec-used
|
||||
return module
|
||||
Reference in New Issue
Block a user