Implement direct ros1 (de)serialization
This commit is contained in:
parent
1309d42b64
commit
219a4d9846
@ -3,7 +3,7 @@
|
|||||||
import numpy
|
import numpy
|
||||||
|
|
||||||
from rosbags.rosbag1 import Writer
|
from rosbags.rosbag1 import Writer
|
||||||
from rosbags.serde import cdr_to_ros1, serialize_cdr
|
from rosbags.serde import serialize_ros1
|
||||||
from rosbags.typesys.types import builtin_interfaces__msg__Time as Time
|
from rosbags.typesys.types import builtin_interfaces__msg__Time as Time
|
||||||
from rosbags.typesys.types import sensor_msgs__msg__CompressedImage as CompressedImage
|
from rosbags.typesys.types import sensor_msgs__msg__CompressedImage as CompressedImage
|
||||||
from rosbags.typesys.types import std_msgs__msg__Header as Header
|
from rosbags.typesys.types import std_msgs__msg__Header as Header
|
||||||
@ -39,8 +39,5 @@ def save_images() -> None:
|
|||||||
writer.write(
|
writer.write(
|
||||||
conn,
|
conn,
|
||||||
timestamp,
|
timestamp,
|
||||||
cdr_to_ros1(
|
serialize_ros1(message, CompressedImage.__msgtype__),
|
||||||
serialize_cdr(message, CompressedImage.__msgtype__),
|
|
||||||
CompressedImage.__msgtype__,
|
|
||||||
),
|
|
||||||
)
|
)
|
||||||
|
|||||||
@ -15,6 +15,16 @@ Deserialize a CDR bytes object using :py:func:`deserialize_cdr() <rosbags.serde.
|
|||||||
# rawdata is of type bytes and contains serialized message
|
# rawdata is of type bytes and contains serialized message
|
||||||
msg = deserialize_cdr(rawdata, 'geometry_msgs/msg/Quaternion')
|
msg = deserialize_cdr(rawdata, 'geometry_msgs/msg/Quaternion')
|
||||||
|
|
||||||
|
Deserialize a ROS1 bytes object using :py:func:`deserialize_ros1() <rosbags.serde.deserialize_ros1>`:
|
||||||
|
|
||||||
|
.. code-block:: python
|
||||||
|
|
||||||
|
from rosbags.serde import deserialize_ros1
|
||||||
|
|
||||||
|
# rawdata is of type bytes and contains serialized message
|
||||||
|
msg = deserialize_ros1(rawdata, 'geometry_msgs/msg/Quaternion')
|
||||||
|
|
||||||
|
|
||||||
Serialization
|
Serialization
|
||||||
---------------
|
---------------
|
||||||
|
|
||||||
@ -29,3 +39,11 @@ Serialize a message with CDR using :py:func:`serialize_cdr() <rosbags.serde.seri
|
|||||||
|
|
||||||
# serialize message with explicit endianess
|
# serialize message with explicit endianess
|
||||||
serialized = serialize_cdr(msg, 'geometry_msgs/msg/Quaternion', little_endian=False)
|
serialized = serialize_cdr(msg, 'geometry_msgs/msg/Quaternion', little_endian=False)
|
||||||
|
|
||||||
|
Serialize a message with ROS1 using :py:func:`serialize_ros1() <rosbags.serde.serialize_ros1>`:
|
||||||
|
|
||||||
|
.. code-block:: python
|
||||||
|
|
||||||
|
from rosbags.serde import serialize_ros1
|
||||||
|
|
||||||
|
serialized = serialize_ros1(msg, 'geometry_msgs/msg/Quaternion')
|
||||||
|
|||||||
@ -15,7 +15,7 @@ from rosbags.rosbag1 import Reader as Reader1
|
|||||||
from rosbags.rosbag1 import ReaderError as ReaderError1
|
from rosbags.rosbag1 import ReaderError as ReaderError1
|
||||||
from rosbags.rosbag2 import Reader as Reader2
|
from rosbags.rosbag2 import Reader as Reader2
|
||||||
from rosbags.rosbag2 import ReaderError as ReaderError2
|
from rosbags.rosbag2 import ReaderError as ReaderError2
|
||||||
from rosbags.serde import deserialize_cdr, ros1_to_cdr
|
from rosbags.serde import deserialize_cdr, deserialize_ros1
|
||||||
from rosbags.typesys import get_types_from_msg, register_types, types
|
from rosbags.typesys import get_types_from_msg, register_types, types
|
||||||
|
|
||||||
if TYPE_CHECKING:
|
if TYPE_CHECKING:
|
||||||
@ -101,7 +101,7 @@ class AnyReader:
|
|||||||
|
|
||||||
def _deser_ros1(self, rawdata: bytes, typ: str) -> object:
|
def _deser_ros1(self, rawdata: bytes, typ: str) -> object:
|
||||||
"""Deserialize ROS1 message."""
|
"""Deserialize ROS1 message."""
|
||||||
return deserialize_cdr(ros1_to_cdr(rawdata, typ, self.typestore), typ, self.typestore)
|
return deserialize_ros1(rawdata, typ, self.typestore)
|
||||||
|
|
||||||
def _deser_ros2(self, rawdata: bytes, typ: str) -> object:
|
def _deser_ros2(self, rawdata: bytes, typ: str) -> object:
|
||||||
"""Deserialize CDR message."""
|
"""Deserialize CDR message."""
|
||||||
|
|||||||
@ -9,12 +9,21 @@ convert directly between different serialization formats.
|
|||||||
"""
|
"""
|
||||||
|
|
||||||
from .messages import SerdeError
|
from .messages import SerdeError
|
||||||
from .serdes import cdr_to_ros1, deserialize_cdr, ros1_to_cdr, serialize_cdr
|
from .serdes import (
|
||||||
|
cdr_to_ros1,
|
||||||
|
deserialize_cdr,
|
||||||
|
deserialize_ros1,
|
||||||
|
ros1_to_cdr,
|
||||||
|
serialize_cdr,
|
||||||
|
serialize_ros1,
|
||||||
|
)
|
||||||
|
|
||||||
__all__ = [
|
__all__ = [
|
||||||
'SerdeError',
|
'SerdeError',
|
||||||
'cdr_to_ros1',
|
'cdr_to_ros1',
|
||||||
'deserialize_cdr',
|
'deserialize_cdr',
|
||||||
|
'deserialize_ros1',
|
||||||
'ros1_to_cdr',
|
'ros1_to_cdr',
|
||||||
'serialize_cdr',
|
'serialize_cdr',
|
||||||
|
'serialize_ros1',
|
||||||
]
|
]
|
||||||
|
|||||||
@ -7,7 +7,13 @@ from __future__ import annotations
|
|||||||
from typing import TYPE_CHECKING
|
from typing import TYPE_CHECKING
|
||||||
|
|
||||||
from .cdr import generate_deserialize_cdr, generate_getsize_cdr, generate_serialize_cdr
|
from .cdr import generate_deserialize_cdr, generate_getsize_cdr, generate_serialize_cdr
|
||||||
from .ros1 import generate_cdr_to_ros1, generate_ros1_to_cdr
|
from .ros1 import (
|
||||||
|
generate_cdr_to_ros1,
|
||||||
|
generate_deserialize_ros1,
|
||||||
|
generate_getsize_ros1,
|
||||||
|
generate_ros1_to_cdr,
|
||||||
|
generate_serialize_ros1,
|
||||||
|
)
|
||||||
from .typing import Descriptor, Field, Msgdef
|
from .typing import Descriptor, Field, Msgdef
|
||||||
from .utils import Valtype
|
from .utils import Valtype
|
||||||
|
|
||||||
@ -62,6 +68,7 @@ def get_msgdef(typename: str, typestore: Typestore) -> Msgdef:
|
|||||||
fields = [Field(name, fixup(desc)) for name, desc in entries]
|
fields = [Field(name, fixup(desc)) for name, desc in entries]
|
||||||
|
|
||||||
getsize_cdr, size_cdr = generate_getsize_cdr(fields)
|
getsize_cdr, size_cdr = generate_getsize_cdr(fields)
|
||||||
|
getsize_ros1, size_ros1 = generate_getsize_ros1(fields, typename)
|
||||||
|
|
||||||
cache[typename] = Msgdef(
|
cache[typename] = Msgdef(
|
||||||
typename,
|
typename,
|
||||||
@ -73,6 +80,10 @@ def get_msgdef(typename: str, typestore: Typestore) -> Msgdef:
|
|||||||
generate_serialize_cdr(fields, 'be'),
|
generate_serialize_cdr(fields, 'be'),
|
||||||
generate_deserialize_cdr(fields, 'le'),
|
generate_deserialize_cdr(fields, 'le'),
|
||||||
generate_deserialize_cdr(fields, 'be'),
|
generate_deserialize_cdr(fields, 'be'),
|
||||||
|
size_ros1,
|
||||||
|
getsize_ros1,
|
||||||
|
generate_serialize_ros1(fields, typename),
|
||||||
|
generate_deserialize_ros1(fields, typename),
|
||||||
generate_ros1_to_cdr(fields, typename, False), # type: ignore
|
generate_ros1_to_cdr(fields, typename, False), # type: ignore
|
||||||
generate_ros1_to_cdr(fields, typename, True), # type: ignore
|
generate_ros1_to_cdr(fields, typename, True), # type: ignore
|
||||||
generate_cdr_to_ros1(fields, typename, False), # type: ignore
|
generate_cdr_to_ros1(fields, typename, False), # type: ignore
|
||||||
|
|||||||
@ -11,6 +11,7 @@ conversion of ROS1 to CDR.
|
|||||||
|
|
||||||
from __future__ import annotations
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import sys
|
||||||
from itertools import tee
|
from itertools import tee
|
||||||
from typing import TYPE_CHECKING, Iterator, cast
|
from typing import TYPE_CHECKING, Iterator, cast
|
||||||
|
|
||||||
@ -20,7 +21,7 @@ from .utils import SIZEMAP, Valtype, align, align_after, compile_lines
|
|||||||
if TYPE_CHECKING:
|
if TYPE_CHECKING:
|
||||||
from typing import Union
|
from typing import Union
|
||||||
|
|
||||||
from .typing import Bitcvt, BitcvtSize
|
from .typing import Bitcvt, BitcvtSize, CDRDeser, CDRSer, CDRSerSize
|
||||||
|
|
||||||
|
|
||||||
def generate_ros1_to_cdr(
|
def generate_ros1_to_cdr(
|
||||||
@ -331,3 +332,349 @@ def generate_cdr_to_ros1(
|
|||||||
|
|
||||||
lines.append(' return ipos, opos')
|
lines.append(' return ipos, opos')
|
||||||
return getattr(compile_lines(lines), funcname) # type: ignore
|
return getattr(compile_lines(lines), funcname) # type: ignore
|
||||||
|
|
||||||
|
|
||||||
|
def generate_getsize_ros1(fields: list[Field], typename: str) -> tuple[CDRSerSize, int]:
|
||||||
|
"""Generate ros1 size calculation function.
|
||||||
|
|
||||||
|
Args:
|
||||||
|
fields: Fields of message.
|
||||||
|
typename: Message type name.
|
||||||
|
|
||||||
|
Returns:
|
||||||
|
Size calculation function and static size.
|
||||||
|
|
||||||
|
"""
|
||||||
|
# pylint: disable=too-many-branches,too-many-statements
|
||||||
|
size = 0
|
||||||
|
is_stat = True
|
||||||
|
|
||||||
|
lines = [
|
||||||
|
'import sys',
|
||||||
|
'from rosbags.serde.messages import get_msgdef',
|
||||||
|
'def getsize_ros1(pos, message, typestore):',
|
||||||
|
]
|
||||||
|
|
||||||
|
if typename == 'std_msgs/msg/Header':
|
||||||
|
lines.append(' pos += 4')
|
||||||
|
|
||||||
|
for fcurr in fields:
|
||||||
|
fieldname, desc = fcurr
|
||||||
|
|
||||||
|
if desc.valtype == Valtype.MESSAGE:
|
||||||
|
if desc.args.size_ros1:
|
||||||
|
lines.append(f' pos += {desc.args.size_ros1}')
|
||||||
|
size += desc.args.size_ros1
|
||||||
|
else:
|
||||||
|
lines.append(f' func = get_msgdef("{desc.args.name}", typestore).getsize_ros1')
|
||||||
|
lines.append(f' pos = func(pos, message.{fieldname}, typestore)')
|
||||||
|
is_stat = False
|
||||||
|
|
||||||
|
elif desc.valtype == Valtype.BASE:
|
||||||
|
if desc.args == 'string':
|
||||||
|
lines.append(f' pos += 4 + len(message.{fieldname}.encode())')
|
||||||
|
is_stat = False
|
||||||
|
else:
|
||||||
|
lines.append(f' pos += {SIZEMAP[desc.args]}')
|
||||||
|
size += SIZEMAP[desc.args]
|
||||||
|
|
||||||
|
elif desc.valtype == Valtype.ARRAY:
|
||||||
|
subdesc, length = desc.args
|
||||||
|
|
||||||
|
if subdesc.valtype == Valtype.BASE:
|
||||||
|
if subdesc.args == 'string':
|
||||||
|
lines.append(f' val = message.{fieldname}')
|
||||||
|
for idx in range(length):
|
||||||
|
lines.append(f' pos += 4 + len(val[{idx}].encode())')
|
||||||
|
is_stat = False
|
||||||
|
else:
|
||||||
|
lines.append(f' pos += {length * SIZEMAP[subdesc.args]}')
|
||||||
|
size += length * SIZEMAP[subdesc.args]
|
||||||
|
|
||||||
|
else:
|
||||||
|
assert subdesc.valtype == Valtype.MESSAGE
|
||||||
|
if subdesc.args.size_ros1:
|
||||||
|
for _ in range(length):
|
||||||
|
lines.append(f' pos += {subdesc.args.size_ros1}')
|
||||||
|
size += subdesc.args.size_ros1
|
||||||
|
else:
|
||||||
|
lines.append(
|
||||||
|
f' func = get_msgdef("{subdesc.args.name}", typestore).getsize_ros1',
|
||||||
|
)
|
||||||
|
lines.append(f' val = message.{fieldname}')
|
||||||
|
for idx in range(length):
|
||||||
|
lines.append(f' pos = func(pos, val[{idx}], typestore)')
|
||||||
|
is_stat = False
|
||||||
|
else:
|
||||||
|
assert desc.valtype == Valtype.SEQUENCE
|
||||||
|
lines.append(' pos += 4')
|
||||||
|
subdesc = desc.args[0]
|
||||||
|
if subdesc.valtype == Valtype.BASE:
|
||||||
|
if subdesc.args == 'string':
|
||||||
|
lines.append(f' for val in message.{fieldname}:')
|
||||||
|
lines.append(' pos += 4 + len(val.encode())')
|
||||||
|
else:
|
||||||
|
lines.append(f' pos += len(message.{fieldname}) * {SIZEMAP[subdesc.args]}')
|
||||||
|
|
||||||
|
else:
|
||||||
|
assert subdesc.valtype == Valtype.MESSAGE
|
||||||
|
lines.append(f' val = message.{fieldname}')
|
||||||
|
if subdesc.args.size_ros1:
|
||||||
|
lines.append(f' pos += {subdesc.args.size_ros1} * len(val)')
|
||||||
|
|
||||||
|
else:
|
||||||
|
lines.append(
|
||||||
|
f' func = get_msgdef("{subdesc.args.name}", typestore).getsize_ros1',
|
||||||
|
)
|
||||||
|
lines.append(' for item in val:')
|
||||||
|
lines.append(' pos = func(pos, item, typestore)')
|
||||||
|
|
||||||
|
is_stat = False
|
||||||
|
lines.append(' return pos')
|
||||||
|
return compile_lines(lines).getsize_ros1, is_stat * size
|
||||||
|
|
||||||
|
|
||||||
|
def generate_serialize_ros1(fields: list[Field], typename: str) -> CDRSer:
|
||||||
|
"""Generate ros1 serialization function.
|
||||||
|
|
||||||
|
Args:
|
||||||
|
fields: Fields of message.
|
||||||
|
typename: Message type name.
|
||||||
|
|
||||||
|
Returns:
|
||||||
|
Serializer function.
|
||||||
|
|
||||||
|
"""
|
||||||
|
# pylint: disable=too-many-branches,too-many-statements
|
||||||
|
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',
|
||||||
|
'def serialize_ros1(rawdata, pos, message, typestore):',
|
||||||
|
]
|
||||||
|
|
||||||
|
if typename == 'std_msgs/msg/Header':
|
||||||
|
lines.append(' pos += 4')
|
||||||
|
|
||||||
|
be_syms = ('>',) if sys.byteorder == 'little' else ('=', '>')
|
||||||
|
|
||||||
|
for fcurr in fields:
|
||||||
|
fieldname, desc = fcurr
|
||||||
|
|
||||||
|
lines.append(f' val = message.{fieldname}')
|
||||||
|
if desc.valtype == Valtype.MESSAGE:
|
||||||
|
name = desc.args.name
|
||||||
|
lines.append(f' func = get_msgdef("{name}", typestore).serialize_ros1')
|
||||||
|
lines.append(' pos = func(rawdata, pos, val, typestore)')
|
||||||
|
|
||||||
|
elif desc.valtype == Valtype.BASE:
|
||||||
|
if desc.args == 'string':
|
||||||
|
lines.append(' bval = memoryview(val.encode())')
|
||||||
|
lines.append(' length = len(bval)')
|
||||||
|
lines.append(' pack_int32_le(rawdata, pos, length)')
|
||||||
|
lines.append(' pos += 4')
|
||||||
|
lines.append(' rawdata[pos:pos + length] = bval')
|
||||||
|
lines.append(' pos += length')
|
||||||
|
else:
|
||||||
|
lines.append(f' pack_{desc.args}_le(rawdata, pos, val)')
|
||||||
|
lines.append(f' pos += {SIZEMAP[desc.args]}')
|
||||||
|
|
||||||
|
elif desc.valtype == Valtype.ARRAY:
|
||||||
|
subdesc, length = desc.args
|
||||||
|
lines.append(f' if len(val) != {length}:')
|
||||||
|
lines.append(' raise SerdeError(\'Unexpected array length\')')
|
||||||
|
|
||||||
|
if subdesc.valtype == Valtype.BASE:
|
||||||
|
if subdesc.args == 'string':
|
||||||
|
for idx in range(length):
|
||||||
|
lines.append(f' bval = memoryview(val[{idx}].encode())')
|
||||||
|
lines.append(' length = len(bval)')
|
||||||
|
lines.append(' pack_int32_le(rawdata, pos, length)')
|
||||||
|
lines.append(' pos += 4')
|
||||||
|
lines.append(' rawdata[pos:pos + length] = bval')
|
||||||
|
lines.append(' pos += length')
|
||||||
|
else:
|
||||||
|
lines.append(f' if val.dtype.byteorder in {be_syms}:')
|
||||||
|
lines.append(' val = val.byteswap()')
|
||||||
|
size = length * 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
|
||||||
|
name = subdesc.args.name
|
||||||
|
lines.append(f' func = get_msgdef("{name}", typestore).serialize_ros1')
|
||||||
|
for idx in range(length):
|
||||||
|
lines.append(f' pos = func(rawdata, pos, val[{idx}], typestore)')
|
||||||
|
else:
|
||||||
|
assert desc.valtype == Valtype.SEQUENCE
|
||||||
|
lines.append(' pack_int32_le(rawdata, pos, len(val))')
|
||||||
|
lines.append(' pos += 4')
|
||||||
|
subdesc = desc.args[0]
|
||||||
|
|
||||||
|
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)')
|
||||||
|
lines.append(' pack_int32_le(rawdata, pos, length)')
|
||||||
|
lines.append(' pos += 4')
|
||||||
|
lines.append(' rawdata[pos:pos + length] = bval')
|
||||||
|
lines.append(' pos += length')
|
||||||
|
else:
|
||||||
|
lines.append(f' size = len(val) * {SIZEMAP[subdesc.args]}')
|
||||||
|
lines.append(f' if val.dtype.byteorder in {be_syms}:')
|
||||||
|
lines.append(' val = val.byteswap()')
|
||||||
|
lines.append(' rawdata[pos:pos + size] = val.view(numpy.uint8)')
|
||||||
|
lines.append(' pos += size')
|
||||||
|
|
||||||
|
if subdesc.valtype == Valtype.MESSAGE:
|
||||||
|
name = subdesc.args.name
|
||||||
|
lines.append(f' func = get_msgdef("{name}", typestore).serialize_ros1')
|
||||||
|
lines.append(' for item in val:')
|
||||||
|
lines.append(' pos = func(rawdata, pos, item, typestore)')
|
||||||
|
|
||||||
|
lines.append(' return pos')
|
||||||
|
return compile_lines(lines).serialize_ros1 # type: ignore
|
||||||
|
|
||||||
|
|
||||||
|
def generate_deserialize_ros1(fields: list[Field], typename: str) -> CDRDeser:
|
||||||
|
"""Generate ros1 deserialization function.
|
||||||
|
|
||||||
|
Args:
|
||||||
|
fields: Fields of message.
|
||||||
|
typename: Message type name.
|
||||||
|
|
||||||
|
Returns:
|
||||||
|
Deserializer function.
|
||||||
|
|
||||||
|
"""
|
||||||
|
# pylint: disable=too-many-branches,too-many-statements
|
||||||
|
lines = [
|
||||||
|
'import sys',
|
||||||
|
'import numpy',
|
||||||
|
'from rosbags.serde.messages import SerdeError, get_msgdef',
|
||||||
|
'from rosbags.serde.primitives import unpack_bool_le',
|
||||||
|
'from rosbags.serde.primitives import unpack_int8_le',
|
||||||
|
'from rosbags.serde.primitives import unpack_int16_le',
|
||||||
|
'from rosbags.serde.primitives import unpack_int32_le',
|
||||||
|
'from rosbags.serde.primitives import unpack_int64_le',
|
||||||
|
'from rosbags.serde.primitives import unpack_uint8_le',
|
||||||
|
'from rosbags.serde.primitives import unpack_uint16_le',
|
||||||
|
'from rosbags.serde.primitives import unpack_uint32_le',
|
||||||
|
'from rosbags.serde.primitives import unpack_uint64_le',
|
||||||
|
'from rosbags.serde.primitives import unpack_float32_le',
|
||||||
|
'from rosbags.serde.primitives import unpack_float64_le',
|
||||||
|
'def deserialize_ros1(rawdata, pos, cls, typestore):',
|
||||||
|
]
|
||||||
|
|
||||||
|
if typename == 'std_msgs/msg/Header':
|
||||||
|
lines.append(' pos += 4')
|
||||||
|
|
||||||
|
be_syms = ('>',) if sys.byteorder == 'little' else ('=', '>')
|
||||||
|
|
||||||
|
funcname = 'deserialize_ros1'
|
||||||
|
lines.append(' values = []')
|
||||||
|
for fcurr in fields:
|
||||||
|
desc = fcurr[1]
|
||||||
|
|
||||||
|
if desc.valtype == Valtype.MESSAGE:
|
||||||
|
lines.append(f' msgdef = get_msgdef("{desc.args.name}", typestore)')
|
||||||
|
lines.append(f' obj, pos = msgdef.{funcname}(rawdata, pos, msgdef.cls, typestore)')
|
||||||
|
lines.append(' values.append(obj)')
|
||||||
|
|
||||||
|
elif desc.valtype == Valtype.BASE:
|
||||||
|
if desc.args == 'string':
|
||||||
|
lines.append(' length = unpack_int32_le(rawdata, pos)[0]')
|
||||||
|
lines.append(' string = bytes(rawdata[pos + 4:pos + 4 + length]).decode()')
|
||||||
|
lines.append(' values.append(string)')
|
||||||
|
lines.append(' pos += 4 + length')
|
||||||
|
else:
|
||||||
|
lines.append(f' value = unpack_{desc.args}_le(rawdata, pos)[0]')
|
||||||
|
lines.append(' values.append(value)')
|
||||||
|
lines.append(f' pos += {SIZEMAP[desc.args]}')
|
||||||
|
|
||||||
|
elif desc.valtype == Valtype.ARRAY:
|
||||||
|
subdesc, length = desc.args
|
||||||
|
if subdesc.valtype == Valtype.BASE:
|
||||||
|
if subdesc.args == 'string':
|
||||||
|
lines.append(' value = []')
|
||||||
|
for _ in range(length):
|
||||||
|
lines.append(' length = unpack_int32_le(rawdata, pos)[0]')
|
||||||
|
lines.append(
|
||||||
|
' value.append(bytes(rawdata[pos + 4:pos + 4 + length]).decode())',
|
||||||
|
)
|
||||||
|
lines.append(' pos += 4 + length')
|
||||||
|
lines.append(' values.append(value)')
|
||||||
|
else:
|
||||||
|
size = length * SIZEMAP[subdesc.args]
|
||||||
|
lines.append(
|
||||||
|
f' val = numpy.frombuffer(rawdata, '
|
||||||
|
f'dtype=numpy.{subdesc.args}, count={length}, offset=pos)',
|
||||||
|
)
|
||||||
|
lines.append(f' if val.dtype.byteorder in {be_syms}:')
|
||||||
|
lines.append(' val = val.byteswap()')
|
||||||
|
lines.append(' values.append(val)')
|
||||||
|
lines.append(f' pos += {size}')
|
||||||
|
else:
|
||||||
|
assert subdesc.valtype == Valtype.MESSAGE
|
||||||
|
lines.append(f' msgdef = get_msgdef("{subdesc.args.name}", typestore)')
|
||||||
|
lines.append(' value = []')
|
||||||
|
for _ in range(length):
|
||||||
|
lines.append(
|
||||||
|
f' obj, pos = msgdef.{funcname}(rawdata, pos, msgdef.cls, typestore)',
|
||||||
|
)
|
||||||
|
lines.append(' value.append(obj)')
|
||||||
|
lines.append(' values.append(value)')
|
||||||
|
|
||||||
|
else:
|
||||||
|
assert desc.valtype == Valtype.SEQUENCE
|
||||||
|
lines.append(' size = unpack_int32_le(rawdata, pos)[0]')
|
||||||
|
lines.append(' pos += 4')
|
||||||
|
subdesc = desc.args[0]
|
||||||
|
|
||||||
|
if subdesc.valtype == Valtype.BASE:
|
||||||
|
if subdesc.args == 'string':
|
||||||
|
lines.append(' value = []')
|
||||||
|
lines.append(' for _ in range(size):')
|
||||||
|
lines.append(' length = unpack_int32_le(rawdata, pos)[0]')
|
||||||
|
lines.append(
|
||||||
|
' value.append(bytes(rawdata[pos + 4:pos + 4 + length])'
|
||||||
|
'.decode())',
|
||||||
|
)
|
||||||
|
lines.append(' pos += 4 + length')
|
||||||
|
lines.append(' values.append(value)')
|
||||||
|
else:
|
||||||
|
lines.append(f' length = size * {SIZEMAP[subdesc.args]}')
|
||||||
|
lines.append(
|
||||||
|
f' val = numpy.frombuffer(rawdata, '
|
||||||
|
f'dtype=numpy.{subdesc.args}, count=size, offset=pos)',
|
||||||
|
)
|
||||||
|
lines.append(f' if val.dtype.byteorder in {be_syms}:')
|
||||||
|
lines.append(' val = val.byteswap()')
|
||||||
|
lines.append(' values.append(val)')
|
||||||
|
lines.append(' pos += length')
|
||||||
|
|
||||||
|
if subdesc.valtype == Valtype.MESSAGE:
|
||||||
|
lines.append(f' msgdef = get_msgdef("{subdesc.args.name}", typestore)')
|
||||||
|
lines.append(' value = []')
|
||||||
|
lines.append(' for _ in range(size):')
|
||||||
|
lines.append(
|
||||||
|
f' obj, pos = msgdef.{funcname}(rawdata, pos, msgdef.cls, typestore)',
|
||||||
|
)
|
||||||
|
lines.append(' value.append(obj)')
|
||||||
|
lines.append(' values.append(value)')
|
||||||
|
|
||||||
|
lines.append(' return cls(*values), pos')
|
||||||
|
return compile_lines(lines).deserialize_ros1 # type: ignore
|
||||||
|
|||||||
@ -73,6 +73,54 @@ def serialize_cdr(
|
|||||||
return rawdata.toreadonly()
|
return rawdata.toreadonly()
|
||||||
|
|
||||||
|
|
||||||
|
def deserialize_ros1(
|
||||||
|
rawdata: bytes,
|
||||||
|
typename: str,
|
||||||
|
typestore: Typestore = types,
|
||||||
|
) -> Any: # noqa: ANN401
|
||||||
|
"""Deserialize raw data into a message object.
|
||||||
|
|
||||||
|
Args:
|
||||||
|
rawdata: Serialized data.
|
||||||
|
typename: Message type name.
|
||||||
|
typestore: Type store.
|
||||||
|
|
||||||
|
Returns:
|
||||||
|
Deserialized message object.
|
||||||
|
|
||||||
|
"""
|
||||||
|
msgdef = get_msgdef(typename, typestore)
|
||||||
|
func = msgdef.deserialize_ros1
|
||||||
|
message, pos = func(rawdata, 0, msgdef.cls, typestore)
|
||||||
|
assert pos == len(rawdata)
|
||||||
|
return message
|
||||||
|
|
||||||
|
|
||||||
|
def serialize_ros1(
|
||||||
|
message: object,
|
||||||
|
typename: str,
|
||||||
|
typestore: Typestore = types,
|
||||||
|
) -> memoryview:
|
||||||
|
"""Serialize message object to bytes.
|
||||||
|
|
||||||
|
Args:
|
||||||
|
message: Message object.
|
||||||
|
typename: Message type name.
|
||||||
|
typestore: Type store.
|
||||||
|
|
||||||
|
Returns:
|
||||||
|
Serialized bytes.
|
||||||
|
|
||||||
|
"""
|
||||||
|
msgdef = get_msgdef(typename, typestore)
|
||||||
|
size = msgdef.getsize_ros1(0, message, typestore)
|
||||||
|
rawdata = memoryview(bytearray(size))
|
||||||
|
func = msgdef.serialize_ros1
|
||||||
|
pos = func(rawdata, 0, message, typestore)
|
||||||
|
assert pos == size
|
||||||
|
return rawdata.toreadonly()
|
||||||
|
|
||||||
|
|
||||||
def ros1_to_cdr(raw: bytes, typename: str, typestore: Typestore = types) -> memoryview:
|
def ros1_to_cdr(raw: bytes, typename: str, typestore: Typestore = types) -> memoryview:
|
||||||
"""Convert serialized ROS1 message directly to CDR.
|
"""Convert serialized ROS1 message directly to CDR.
|
||||||
|
|
||||||
|
|||||||
@ -45,6 +45,10 @@ class Msgdef(NamedTuple):
|
|||||||
serialize_cdr_be: CDRSer
|
serialize_cdr_be: CDRSer
|
||||||
deserialize_cdr_le: CDRDeser
|
deserialize_cdr_le: CDRDeser
|
||||||
deserialize_cdr_be: CDRDeser
|
deserialize_cdr_be: CDRDeser
|
||||||
|
size_ros1: int
|
||||||
|
getsize_ros1: CDRSerSize
|
||||||
|
serialize_ros1: CDRSer
|
||||||
|
deserialize_ros1: CDRDeser
|
||||||
getsize_ros1_to_cdr: BitcvtSize
|
getsize_ros1_to_cdr: BitcvtSize
|
||||||
ros1_to_cdr: Bitcvt
|
ros1_to_cdr: Bitcvt
|
||||||
getsize_cdr_to_ros1: BitcvtSize
|
getsize_cdr_to_ros1: BitcvtSize
|
||||||
|
|||||||
@ -10,7 +10,15 @@ from unittest.mock import MagicMock, patch
|
|||||||
import numpy
|
import numpy
|
||||||
import pytest
|
import pytest
|
||||||
|
|
||||||
from rosbags.serde import SerdeError, cdr_to_ros1, deserialize_cdr, ros1_to_cdr, serialize_cdr
|
from rosbags.serde import (
|
||||||
|
SerdeError,
|
||||||
|
cdr_to_ros1,
|
||||||
|
deserialize_cdr,
|
||||||
|
deserialize_ros1,
|
||||||
|
ros1_to_cdr,
|
||||||
|
serialize_cdr,
|
||||||
|
serialize_ros1,
|
||||||
|
)
|
||||||
from rosbags.serde.messages import get_msgdef
|
from rosbags.serde.messages import get_msgdef
|
||||||
from rosbags.typesys import get_types_from_msg, register_types, types
|
from rosbags.typesys import get_types_from_msg, register_types, types
|
||||||
from rosbags.typesys.types import builtin_interfaces__msg__Time as Time
|
from rosbags.typesys.types import builtin_interfaces__msg__Time as Time
|
||||||
@ -206,6 +214,8 @@ def _comparable() -> Generator[None, None, None]:
|
|||||||
|
|
||||||
def __init__(self, *args: Any, **kwargs: Any): # noqa: ANN401
|
def __init__(self, *args: Any, **kwargs: Any): # noqa: ANN401
|
||||||
super().__init__(*args, **kwargs)
|
super().__init__(*args, **kwargs)
|
||||||
|
self.dtype = kwargs['wraps'].dtype
|
||||||
|
self.reshape = kwargs['wraps'].reshape
|
||||||
self.__eq__ = arreq # type: ignore
|
self.__eq__ = arreq # type: ignore
|
||||||
|
|
||||||
def byteswap(self, *args: Any) -> CNDArray: # noqa: ANN401
|
def byteswap(self, *args: Any) -> CNDArray: # noqa: ANN401
|
||||||
@ -230,6 +240,11 @@ def test_serde(message: tuple[bytes, str, bool]) -> None:
|
|||||||
assert len(rawdata) - len(serdeser) < 4
|
assert len(rawdata) - len(serdeser) < 4
|
||||||
assert all(x == 0 for x in rawdata[len(serdeser):])
|
assert all(x == 0 for x in rawdata[len(serdeser):])
|
||||||
|
|
||||||
|
if rawdata[1] == 1:
|
||||||
|
rawdata = cdr_to_ros1(rawdata, typ)
|
||||||
|
serdeser = serialize_ros1(deserialize_ros1(rawdata, typ), typ)
|
||||||
|
assert serdeser == rawdata
|
||||||
|
|
||||||
|
|
||||||
@pytest.mark.usefixtures('_comparable')
|
@pytest.mark.usefixtures('_comparable')
|
||||||
def test_deserializer() -> None:
|
def test_deserializer() -> None:
|
||||||
@ -244,6 +259,8 @@ def test_deserializer() -> None:
|
|||||||
assert msg.points[1].x == 1.25
|
assert msg.points[1].x == 1.25
|
||||||
assert msg.points[1].y == 2.25
|
assert msg.points[1].y == 2.25
|
||||||
assert msg.points[1].z == 3.25
|
assert msg.points[1].z == 3.25
|
||||||
|
msg_ros1 = deserialize_ros1(cdr_to_ros1(*MSG_POLY[:2]), MSG_POLY[1])
|
||||||
|
assert msg_ros1 == msg
|
||||||
|
|
||||||
msg = deserialize_cdr(*MSG_MAGN[:2])
|
msg = deserialize_cdr(*MSG_MAGN[:2])
|
||||||
assert msg == deserialize(*MSG_MAGN[:2])
|
assert msg == deserialize(*MSG_MAGN[:2])
|
||||||
@ -256,6 +273,8 @@ def test_deserializer() -> None:
|
|||||||
assert (field.x, field.y, field.z) == (128., 128., 128.)
|
assert (field.x, field.y, field.z) == (128., 128., 128.)
|
||||||
diag = numpy.diag(msg.magnetic_field_covariance.reshape(3, 3))
|
diag = numpy.diag(msg.magnetic_field_covariance.reshape(3, 3))
|
||||||
assert (diag == [1., 1., 1.]).all()
|
assert (diag == [1., 1., 1.]).all()
|
||||||
|
msg_ros1 = deserialize_ros1(cdr_to_ros1(*MSG_MAGN[:2]), MSG_MAGN[1])
|
||||||
|
assert msg_ros1 == msg
|
||||||
|
|
||||||
msg_big = deserialize_cdr(*MSG_MAGN_BIG[:2])
|
msg_big = deserialize_cdr(*MSG_MAGN_BIG[:2])
|
||||||
assert msg_big == deserialize(*MSG_MAGN_BIG[:2])
|
assert msg_big == deserialize(*MSG_MAGN_BIG[:2])
|
||||||
@ -384,10 +403,14 @@ def test_custom_type() -> None:
|
|||||||
assert res == deserialize(serialize(msg, cname), cname)
|
assert res == deserialize(serialize(msg, cname), cname)
|
||||||
assert res == msg
|
assert res == msg
|
||||||
|
|
||||||
|
res = deserialize_ros1(serialize_ros1(msg, cname), cname)
|
||||||
|
assert res == msg
|
||||||
|
|
||||||
|
|
||||||
def test_ros1_to_cdr() -> None:
|
def test_ros1_to_cdr() -> None:
|
||||||
"""Test ROS1 to CDR conversion."""
|
"""Test ROS1 to CDR conversion."""
|
||||||
register_types(dict(get_types_from_msg(STATIC_16_64, 'test_msgs/msg/static_16_64')))
|
msgtype = 'test_msgs/msg/static_16_64'
|
||||||
|
register_types(dict(get_types_from_msg(STATIC_16_64, msgtype)))
|
||||||
msg_ros = (b'\x01\x00'
|
msg_ros = (b'\x01\x00'
|
||||||
b'\x00\x00\x00\x00\x00\x00\x00\x02')
|
b'\x00\x00\x00\x00\x00\x00\x00\x02')
|
||||||
msg_cdr = (
|
msg_cdr = (
|
||||||
@ -396,9 +419,11 @@ def test_ros1_to_cdr() -> None:
|
|||||||
b'\x00\x00\x00\x00\x00\x00'
|
b'\x00\x00\x00\x00\x00\x00'
|
||||||
b'\x00\x00\x00\x00\x00\x00\x00\x02'
|
b'\x00\x00\x00\x00\x00\x00\x00\x02'
|
||||||
)
|
)
|
||||||
assert ros1_to_cdr(msg_ros, 'test_msgs/msg/static_16_64') == msg_cdr
|
assert ros1_to_cdr(msg_ros, msgtype) == msg_cdr
|
||||||
|
assert serialize_cdr(deserialize_ros1(msg_ros, msgtype), msgtype) == msg_cdr
|
||||||
|
|
||||||
register_types(dict(get_types_from_msg(DYNAMIC_S_64, 'test_msgs/msg/dynamic_s_64')))
|
msgtype = 'test_msgs/msg/dynamic_s_64'
|
||||||
|
register_types(dict(get_types_from_msg(DYNAMIC_S_64, msgtype)))
|
||||||
msg_ros = (b'\x01\x00\x00\x00X'
|
msg_ros = (b'\x01\x00\x00\x00X'
|
||||||
b'\x00\x00\x00\x00\x00\x00\x00\x02')
|
b'\x00\x00\x00\x00\x00\x00\x00\x02')
|
||||||
msg_cdr = (
|
msg_cdr = (
|
||||||
@ -407,12 +432,14 @@ def test_ros1_to_cdr() -> None:
|
|||||||
b'\x00\x00'
|
b'\x00\x00'
|
||||||
b'\x00\x00\x00\x00\x00\x00\x00\x02'
|
b'\x00\x00\x00\x00\x00\x00\x00\x02'
|
||||||
)
|
)
|
||||||
assert ros1_to_cdr(msg_ros, 'test_msgs/msg/dynamic_s_64') == msg_cdr
|
assert ros1_to_cdr(msg_ros, msgtype) == msg_cdr
|
||||||
|
assert serialize_cdr(deserialize_ros1(msg_ros, msgtype), msgtype) == msg_cdr
|
||||||
|
|
||||||
|
|
||||||
def test_cdr_to_ros1() -> None:
|
def test_cdr_to_ros1() -> None:
|
||||||
"""Test CDR to ROS1 conversion."""
|
"""Test CDR to ROS1 conversion."""
|
||||||
register_types(dict(get_types_from_msg(STATIC_16_64, 'test_msgs/msg/static_16_64')))
|
msgtype = 'test_msgs/msg/static_16_64'
|
||||||
|
register_types(dict(get_types_from_msg(STATIC_16_64, msgtype)))
|
||||||
msg_ros = (b'\x01\x00'
|
msg_ros = (b'\x01\x00'
|
||||||
b'\x00\x00\x00\x00\x00\x00\x00\x02')
|
b'\x00\x00\x00\x00\x00\x00\x00\x02')
|
||||||
msg_cdr = (
|
msg_cdr = (
|
||||||
@ -421,9 +448,11 @@ def test_cdr_to_ros1() -> None:
|
|||||||
b'\x00\x00\x00\x00\x00\x00'
|
b'\x00\x00\x00\x00\x00\x00'
|
||||||
b'\x00\x00\x00\x00\x00\x00\x00\x02'
|
b'\x00\x00\x00\x00\x00\x00\x00\x02'
|
||||||
)
|
)
|
||||||
assert cdr_to_ros1(msg_cdr, 'test_msgs/msg/static_16_64') == msg_ros
|
assert cdr_to_ros1(msg_cdr, msgtype) == msg_ros
|
||||||
|
assert serialize_ros1(deserialize_cdr(msg_cdr, msgtype), msgtype) == msg_ros
|
||||||
|
|
||||||
register_types(dict(get_types_from_msg(DYNAMIC_S_64, 'test_msgs/msg/dynamic_s_64')))
|
msgtype = 'test_msgs/msg/dynamic_s_64'
|
||||||
|
register_types(dict(get_types_from_msg(DYNAMIC_S_64, msgtype)))
|
||||||
msg_ros = (b'\x01\x00\x00\x00X'
|
msg_ros = (b'\x01\x00\x00\x00X'
|
||||||
b'\x00\x00\x00\x00\x00\x00\x00\x02')
|
b'\x00\x00\x00\x00\x00\x00\x00\x02')
|
||||||
msg_cdr = (
|
msg_cdr = (
|
||||||
@ -432,7 +461,8 @@ def test_cdr_to_ros1() -> None:
|
|||||||
b'\x00\x00'
|
b'\x00\x00'
|
||||||
b'\x00\x00\x00\x00\x00\x00\x00\x02'
|
b'\x00\x00\x00\x00\x00\x00\x00\x02'
|
||||||
)
|
)
|
||||||
assert cdr_to_ros1(msg_cdr, 'test_msgs/msg/dynamic_s_64') == msg_ros
|
assert cdr_to_ros1(msg_cdr, msgtype) == msg_ros
|
||||||
|
assert serialize_ros1(deserialize_cdr(msg_cdr, msgtype), msgtype) == msg_ros
|
||||||
|
|
||||||
header = Header(stamp=Time(42, 666), frame_id='frame')
|
header = Header(stamp=Time(42, 666), frame_id='frame')
|
||||||
msg_ros = cdr_to_ros1(serialize_cdr(header, 'std_msgs/msg/Header'), 'std_msgs/msg/Header')
|
msg_ros = cdr_to_ros1(serialize_cdr(header, 'std_msgs/msg/Header'), 'std_msgs/msg/Header')
|
||||||
|
|||||||
Loading…
x
Reference in New Issue
Block a user