diff --git a/tools/compare/Dockerfile b/tools/compare/Dockerfile new file mode 100644 index 00000000..9e5fb59e --- /dev/null +++ b/tools/compare/Dockerfile @@ -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"] diff --git a/tools/compare/README.rst b/tools/compare/README.rst new file mode 100644 index 00000000..8b581e53 --- /dev/null +++ b/tools/compare/README.rst @@ -0,0 +1,11 @@ +======= +Compare +======= + +Check if the contents of a ``rosbag1`` and ``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 rosbag1 and rosbag2 files to be mounted ``/rosbag1`` and ``/rosbag2`` respectively:: + + $ docker run --rm -v /path/to/rosbag1.bag:/rosbag1 -v /path/to/rosbag2:/rosbag2 rosbags/compare diff --git a/tools/compare/compare.py b/tools/compare/compare.py new file mode 100644 index 00000000..d79ba820 --- /dev/null +++ b/tools/compare/compare.py @@ -0,0 +1,138 @@ +# Copyright 2020-2021 Ternaris. +# SPDX-License-Identifier: Apache-2.0 +"""Tool checking if Rosbag1 and Rosbag2 contents are equal.""" + +# pylint: disable=import-error + +from __future__ import annotations + +import array +import math +import sys +from typing import TYPE_CHECKING +from unittest.mock import Mock + +import genpy # type: ignore +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 Any, List + + from rosbag.bag import _Connection_Info + + +class Reader: # pylint: disable=too-few-public-methods + """Mimimal shim using rosbag2_py to emulate rosbag2 API.""" + + def __init__(self, path: str): + """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): + """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[_Connection_Info]): + """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: T001 + # pylint: disable=assignment-from-no-return,too-many-function-args + 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: Any, msg: Any): + """Compare message to its reference. + + Args: + ref: Reference ROS1 message. + msg: Converted ROS2 message. + + Return: + True if messages are identical. + + """ + if hasattr(msg, 'get_fields_and_field_types'): + 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 (msg == ref).all() + + elif isinstance(msg, list): + 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(ref): + assert math.isnan(msg) + + else: + assert ref == msg + + +def main(path1: str, path2: str): + """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: T001 + + +if __name__ == '__main__': + if len(sys.argv) != 3: + print(f'Usage: {sys.argv} [rosbag1] [rosbag2]') # noqa: T001 + sys.exit(1) + main(sys.argv[1], sys.argv[2])