Add 'rosbags/' from commit 'c80625df279c154c6ec069cbac30faa319755e47'
git-subtree-dir: rosbags git-subtree-mainline:48df1fbdf4git-subtree-split:c80625df27
This commit is contained in:
@@ -0,0 +1,13 @@
|
||||
FROM ros:rolling
|
||||
|
||||
RUN apt-get update \
|
||||
&& apt-get upgrade -y \
|
||||
&& apt-get install -y \
|
||||
python3-pip
|
||||
|
||||
RUN python3 -m pip install ruamel.yaml zstandard
|
||||
|
||||
COPY src/rosbags /opt/ros/rolling/lib/python3.8/site-packages/rosbags
|
||||
COPY tools/bench/bench.py /
|
||||
|
||||
CMD ["/usr/bin/python3", "/bench.py", "/rosbag2"]
|
||||
@@ -0,0 +1,11 @@
|
||||
=====
|
||||
Bench
|
||||
=====
|
||||
|
||||
Check and benchmark ``rosbags.rosbag2`` agains ``rosbag2_py``. The provided Dockerfile creates an execution environment for the script. Run from the root of this repository::
|
||||
|
||||
$ docker build -t rosbags/bench -f tools/bench/Dockerfile .
|
||||
|
||||
The docker image expects that the rosbag2 file to benchmark is mounted under ``/rosbag2``::
|
||||
|
||||
$ docker run --rm -v /path/to/bag:/rosbag2 rosbags/bench
|
||||
@@ -0,0 +1,154 @@
|
||||
# Copyright 2020-2023 Ternaris.
|
||||
# SPDX-License-Identifier: Apache-2.0
|
||||
"""Check and benchmark rosbag2 read implementations."""
|
||||
|
||||
# pylint: disable=import-error
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import sys
|
||||
from math import isnan
|
||||
from pathlib import Path
|
||||
from timeit import timeit
|
||||
from typing import TYPE_CHECKING
|
||||
|
||||
import numpy
|
||||
from rclpy.serialization import deserialize_message # type: ignore
|
||||
from rosbag2_py import ConverterOptions, SequentialReader, StorageOptions # type: ignore
|
||||
from rosidl_runtime_py.utilities import get_message # type: ignore
|
||||
|
||||
from rosbags.rosbag2 import Reader
|
||||
from rosbags.serde import deserialize_cdr
|
||||
|
||||
if TYPE_CHECKING:
|
||||
from typing import Generator, Protocol
|
||||
|
||||
class NativeMSG(Protocol): # pylint: disable=too-few-public-methods
|
||||
"""Minimal native ROS message interface used for benchmark."""
|
||||
|
||||
def get_fields_and_field_types(self) -> dict[str, str]:
|
||||
"""Introspect message type."""
|
||||
raise NotImplementedError
|
||||
|
||||
|
||||
class ReaderPy: # pylint: disable=too-few-public-methods
|
||||
"""Mimimal shim using rosbag2_py to emulate rosbag2 API."""
|
||||
|
||||
def __init__(self, path: Path):
|
||||
"""Initialize reader shim."""
|
||||
soptions = StorageOptions(str(path), 'sqlite3')
|
||||
coptions = ConverterOptions('', '')
|
||||
self.reader = SequentialReader()
|
||||
self.reader.open(soptions, coptions)
|
||||
self.typemap = {x.name: x.type for x in self.reader.get_all_topics_and_types()}
|
||||
|
||||
def messages(self) -> Generator[tuple[str, str, int, bytes], None, None]:
|
||||
"""Expose rosbag2 like generator behavior."""
|
||||
while self.reader.has_next():
|
||||
topic, data, timestamp = self.reader.read_next()
|
||||
yield topic, self.typemap[topic], timestamp, data
|
||||
|
||||
|
||||
def deserialize_py(data: bytes, msgtype: str) -> NativeMSG:
|
||||
"""Deserialization helper for rosidl_runtime_py + rclpy."""
|
||||
pytype = get_message(msgtype)
|
||||
return deserialize_message(data, pytype) # type: ignore
|
||||
|
||||
|
||||
def compare_msg(lite: object, native: NativeMSG) -> None:
|
||||
"""Compare rosbag2 (lite) vs rosbag2_py (native) message content.
|
||||
|
||||
Args:
|
||||
lite: Message from rosbag2.
|
||||
native: Message from rosbag2_py.
|
||||
|
||||
Raises:
|
||||
AssertionError: If messages are not identical.
|
||||
|
||||
"""
|
||||
for fieldname in native.get_fields_and_field_types().keys():
|
||||
native_val = getattr(native, fieldname)
|
||||
lite_val = getattr(lite, fieldname)
|
||||
|
||||
if hasattr(lite_val, '__dataclass_fields__'):
|
||||
compare_msg(lite_val, native_val)
|
||||
|
||||
elif isinstance(lite_val, numpy.ndarray):
|
||||
assert not (native_val != lite_val).any(), f'{fieldname}: {native_val} != {lite_val}'
|
||||
|
||||
elif isinstance(lite_val, list):
|
||||
assert len(native_val) == len(lite_val), f'{fieldname} length mismatch'
|
||||
for sub1, sub2 in zip(native_val, lite_val):
|
||||
compare_msg(sub2, sub1)
|
||||
elif isinstance(lite_val, float) and isnan(lite_val):
|
||||
assert isnan(native_val)
|
||||
else:
|
||||
assert native_val == lite_val, f'{fieldname}: {native_val} != {lite_val}'
|
||||
|
||||
|
||||
def compare(path: Path) -> None:
|
||||
"""Compare raw and deserialized messages."""
|
||||
with Reader(path) as reader:
|
||||
gens = (reader.messages(), ReaderPy(path).messages())
|
||||
for item, item_py in zip(*gens):
|
||||
connection, timestamp, data = item
|
||||
topic_py, msgtype_py, timestamp_py, data_py = item_py
|
||||
|
||||
assert connection.topic == topic_py
|
||||
assert connection.msgtype == msgtype_py
|
||||
assert timestamp == timestamp_py
|
||||
assert data == data_py
|
||||
|
||||
msg_py = deserialize_py(data_py, msgtype_py)
|
||||
msg = deserialize_cdr(data, connection.msgtype)
|
||||
|
||||
compare_msg(msg, msg_py)
|
||||
assert not list(gens[0])
|
||||
assert not list(gens[1])
|
||||
|
||||
|
||||
def read_deser_rosbag2_py(path: Path) -> None:
|
||||
"""Read testbag with rosbag2_py."""
|
||||
soptions = StorageOptions(str(path), 'sqlite3')
|
||||
coptions = ConverterOptions('', '')
|
||||
reader = SequentialReader()
|
||||
reader.open(soptions, coptions)
|
||||
typemap = {x.name: x.type for x in reader.get_all_topics_and_types()}
|
||||
|
||||
while reader.has_next():
|
||||
topic, rawdata, _ = reader.read_next()
|
||||
msgtype = typemap[topic]
|
||||
pytype = get_message(msgtype)
|
||||
deserialize_message(rawdata, pytype)
|
||||
|
||||
|
||||
def read_deser_rosbag2(path: Path) -> None:
|
||||
"""Read testbag with rosbag2lite."""
|
||||
with Reader(path) as reader:
|
||||
for connection, _, data in reader.messages():
|
||||
deserialize_cdr(data, connection.msgtype)
|
||||
|
||||
|
||||
def main() -> None:
|
||||
"""Benchmark rosbag2 against rosbag2_py."""
|
||||
path = Path(sys.argv[1])
|
||||
try:
|
||||
print('Comparing messages from rosbag2 and rosbag2_py.') # noqa: T201
|
||||
compare(path)
|
||||
except AssertionError as err:
|
||||
print(f'Comparison failed {err!r}') # noqa: T201
|
||||
sys.exit(1)
|
||||
|
||||
print('Measuring execution times of rosbag2 and rosbag2_py.') # noqa: T201
|
||||
time_py = timeit(lambda: read_deser_rosbag2_py(path), number=1)
|
||||
time = timeit(lambda: read_deser_rosbag2(path), number=1)
|
||||
print( # noqa: T201
|
||||
f'Processing times:\n'
|
||||
f'rosbag2_py {time_py:.3f}\n'
|
||||
f'rosbag2 {time:.3f}\n'
|
||||
f'speedup {time_py / time:.2f}\n',
|
||||
)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -0,0 +1,11 @@
|
||||
FROM ros:rolling
|
||||
|
||||
RUN apt-get update \
|
||||
&& apt-get upgrade -y \
|
||||
&& apt-get install -y \
|
||||
python3-pip \
|
||||
python3-rosbag
|
||||
|
||||
COPY tools/compare/compare.py /
|
||||
|
||||
CMD ["/usr/bin/python3", "/compare.py", "/rosbag1", "/rosbag2"]
|
||||
@@ -0,0 +1,11 @@
|
||||
=======
|
||||
Compare
|
||||
=======
|
||||
|
||||
Check if the contents of a ``rosbag1`` and another ``rosbag1`` or ``rosbag2`` file are identical. The provided Dockerfile creates an execution environment for the script. Run from the root of this repository::
|
||||
|
||||
$ docker build -t rosbags/compare -f tools/compare/Dockerfile .
|
||||
|
||||
The docker image expects that the first rosbag1 and second rosbag1 or rosbag2 files to be mounted at ``/rosbag1`` and ``/rosbag2`` respectively::
|
||||
|
||||
$ docker run --rm -v /path/to/rosbag1.bag:/rosbag1 -v /path/to/rosbag2:/rosbag2 rosbags/compare
|
||||
@@ -0,0 +1,173 @@
|
||||
# Copyright 2020-2023 Ternaris.
|
||||
# SPDX-License-Identifier: Apache-2.0
|
||||
"""Tool checking if contents of two rosbags are equal."""
|
||||
|
||||
# pylint: disable=import-error
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import array
|
||||
import math
|
||||
import sys
|
||||
from pathlib import Path
|
||||
from typing import TYPE_CHECKING
|
||||
from unittest.mock import Mock
|
||||
|
||||
import genpy # type: ignore
|
||||
import numpy
|
||||
import rosgraph_msgs.msg # type: ignore
|
||||
from rclpy.serialization import deserialize_message # type: ignore
|
||||
from rosbag2_py import ConverterOptions, SequentialReader, StorageOptions # type: ignore
|
||||
from rosidl_runtime_py.utilities import get_message # type: ignore
|
||||
|
||||
rosgraph_msgs.msg.Log = Mock()
|
||||
rosgraph_msgs.msg.TopicStatistics = Mock()
|
||||
|
||||
import rosbag.bag # type:ignore # noqa: E402 pylint: disable=wrong-import-position
|
||||
|
||||
if TYPE_CHECKING:
|
||||
from typing import Generator, List, Protocol, Union, runtime_checkable
|
||||
|
||||
@runtime_checkable
|
||||
class NativeMSG(Protocol): # pylint: disable=too-few-public-methods
|
||||
"""Minimal native ROS message interface used for benchmark."""
|
||||
|
||||
def get_fields_and_field_types(self) -> dict[str, str]:
|
||||
"""Introspect message type."""
|
||||
raise NotImplementedError
|
||||
|
||||
|
||||
class Reader: # pylint: disable=too-few-public-methods
|
||||
"""Mimimal shim using rosbag2_py to emulate rosbags API."""
|
||||
|
||||
def __init__(self, path: Union[str, Path]):
|
||||
"""Initialize reader shim."""
|
||||
self.reader = SequentialReader()
|
||||
self.reader.open(StorageOptions(path, 'sqlite3'), ConverterOptions('', ''))
|
||||
self.typemap = {x.name: x.type for x in self.reader.get_all_topics_and_types()}
|
||||
|
||||
def messages(self) -> Generator[tuple[str, int, bytes], None, None]:
|
||||
"""Expose rosbag2 like generator behavior."""
|
||||
while self.reader.has_next():
|
||||
topic, data, timestamp = self.reader.read_next()
|
||||
pytype = get_message(self.typemap[topic])
|
||||
yield topic, timestamp, deserialize_message(data, pytype)
|
||||
|
||||
|
||||
def fixup_ros1(conns: List[rosbag.bag._Connection_Info]) -> None:
|
||||
"""Monkeypatch ROS2 fieldnames onto ROS1 objects.
|
||||
|
||||
Args:
|
||||
conns: Rosbag1 connections.
|
||||
|
||||
"""
|
||||
genpy.Time.sec = property(lambda x: x.secs)
|
||||
genpy.Time.nanosec = property(lambda x: x.nsecs)
|
||||
genpy.Duration.sec = property(lambda x: x.secs)
|
||||
genpy.Duration.nanosec = property(lambda x: x.nsecs)
|
||||
|
||||
if conn := next((x for x in conns if x.datatype == 'sensor_msgs/CameraInfo'), None):
|
||||
print('Patching CameraInfo') # noqa: T201
|
||||
cls = rosbag.bag._get_message_type(conn) # pylint: disable=protected-access
|
||||
cls.d = property(lambda x: x.D, lambda x, y: setattr(x, 'D', y)) # noqa: B010
|
||||
cls.k = property(lambda x: x.K, lambda x, y: setattr(x, 'K', y)) # noqa: B010
|
||||
cls.r = property(lambda x: x.R, lambda x, y: setattr(x, 'R', y)) # noqa: B010
|
||||
cls.p = property(lambda x: x.P, lambda x, y: setattr(x, 'P', y)) # noqa: B010
|
||||
|
||||
|
||||
def compare(ref: object, msg: object) -> None:
|
||||
"""Compare message to its reference.
|
||||
|
||||
Args:
|
||||
ref: Reference ROS1 message.
|
||||
msg: Converted ROS2 message.
|
||||
|
||||
"""
|
||||
if isinstance(msg, NativeMSG):
|
||||
for name in msg.get_fields_and_field_types():
|
||||
refval = getattr(ref, name)
|
||||
msgval = getattr(msg, name)
|
||||
compare(refval, msgval)
|
||||
|
||||
elif isinstance(msg, array.array):
|
||||
if isinstance(ref, bytes):
|
||||
assert msg.tobytes() == ref
|
||||
else:
|
||||
assert isinstance(msg, numpy.ndarray)
|
||||
assert (msg == ref).all()
|
||||
|
||||
elif isinstance(msg, list):
|
||||
assert isinstance(ref, (list, numpy.ndarray))
|
||||
assert len(msg) == len(ref)
|
||||
for refitem, msgitem in zip(ref, msg):
|
||||
compare(refitem, msgitem)
|
||||
|
||||
elif isinstance(msg, str):
|
||||
assert msg == ref
|
||||
|
||||
elif isinstance(msg, float) and math.isnan(msg):
|
||||
assert isinstance(ref, float)
|
||||
assert math.isnan(ref)
|
||||
|
||||
else:
|
||||
assert ref == msg
|
||||
|
||||
|
||||
def main_bag1_bag1(path1: Path, path2: Path) -> None:
|
||||
"""Compare rosbag1 to rosbag1 message by message.
|
||||
|
||||
Args:
|
||||
path1: Rosbag1 filename.
|
||||
path2: Rosbag1 filename.
|
||||
|
||||
"""
|
||||
reader1 = rosbag.bag.Bag(path1)
|
||||
reader2 = rosbag.bag.Bag(path2)
|
||||
src1 = reader1.read_messages(raw=True, return_connection_header=True)
|
||||
src2 = reader2.read_messages(raw=True, return_connection_header=True)
|
||||
|
||||
for msg1, msg2 in zip(src1, src2):
|
||||
assert msg1.connection_header == msg2.connection_header
|
||||
assert msg1.message[:-2] == msg2.message[:-2]
|
||||
assert msg1.timestamp == msg2.timestamp
|
||||
assert msg1.topic == msg2.topic
|
||||
|
||||
assert next(src1, None) is None
|
||||
assert next(src2, None) is None
|
||||
|
||||
print('Bags are identical.') # noqa: T201
|
||||
|
||||
|
||||
def main_bag1_bag2(path1: Path, path2: Path) -> None:
|
||||
"""Compare rosbag1 to rosbag2 message by message.
|
||||
|
||||
Args:
|
||||
path1: Rosbag1 filename.
|
||||
path2: Rosbag2 filename.
|
||||
|
||||
"""
|
||||
reader1 = rosbag.bag.Bag(path1)
|
||||
src1 = reader1.read_messages()
|
||||
src2 = Reader(path2).messages()
|
||||
|
||||
fixup_ros1(reader1._connections.values()) # pylint: disable=protected-access
|
||||
|
||||
for msg1, msg2 in zip(src1, src2):
|
||||
assert msg1.topic == msg2[0]
|
||||
assert msg1.timestamp.to_nsec() == msg2[1]
|
||||
compare(msg1.message, msg2[2])
|
||||
|
||||
assert next(src1, None) is None
|
||||
assert next(src2, None) is None
|
||||
|
||||
print('Bags are identical.') # noqa: T201
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
if len(sys.argv) != 3:
|
||||
print(f'Usage: {sys.argv} [rosbag1] [rosbag2]') # noqa: T201
|
||||
sys.exit(1)
|
||||
arg1 = Path(sys.argv[1])
|
||||
arg2 = Path(sys.argv[2])
|
||||
main = main_bag1_bag2 if arg2.is_dir() else main_bag1_bag1
|
||||
main(arg1, arg2)
|
||||
Reference in New Issue
Block a user