From 6efe0a58a005f5a9a53445bc98a21f40631af303 Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Thu, 22 Oct 2026 10:37:54 +1000 Subject: [PATCH] fix: ROSBag finds the end of image data from the message layout, not a guess The Intel RealSense SDK follows every image message in a bag with 4 extra bytes (a float32 depth scale). The reader handled that by assuming 4 trailing bytes on every message of any bag that has a realsense_msgs connection, and only when release="auto". Tested on real bags this had three problems: - ROSBag(path) with the default release="ROS1_NOETIC" read no frames at all from the RealSense recording: SerdeError "ROS1 message size mismatch". Current rosbags raises SerdeError, the code only caught AssertionError. - Intel's own sample bag (single_depth_color_640x480.bag) has realsense_msgs connections but no extra bytes. With release="auto" the reader removed 4 real bytes from every image and silently returned no frames. - The depth scale was discarded. Now sensor_msgs/Image and CompressedImage (ROS 1 and CDR, either byte order) are parsed up to the end of their data array, checking that the array length is height * step. Only those bytes are deserialized and anything after them is the footer. Pixels are unchanged. What to make of the footer is decided by context, because the bytes cannot decide it: about half of all 4-byte patterns are a positive, finite float32. In a bag recognised as RealSense (a realsense_msgs connection), a positive finite float32 is attached to the image as `depth_units` (metres per unit) and 4 zero bytes (a color image) mean no scale. Anything else after an image, such as the same 4 bytes in a bag that is not recognised, another length, or a negative or non-finite value, is ignored with a RuntimeWarning, once per topic. CDR padding (up to 3 zero bytes) is silent. Every other message type must be exactly the length its type says, as a serialization format requires, and is no longer tried with 0, 4 and 8 bytes removed. That guess was unnecessary: all 68,889 non-image messages in the 13 real bags tested decode at their exact length. Verified on every bag in bags/ (14 files, old reader vs new): output is byte-identical for all except the two cases above, which now work; the RealSense bag gives depth_units == 0.001 on its 37 depth frames and no warnings. Adds tests/test_ros_realsense.py: the layout parser and footer decoder, synthetic RealSense-style bags written to a temporary directory by tests/create_ros_fixture.py (ROS 1 and CDR, with and without footers and realsense_msgs), unexpected trailing bytes, exact-length decoding of other messages, and checks against the real bags when they are present. Refs #151. The depth_units=/invalid_is_nan= conversion options are not in this change. The layout-parsing approach follows #154 and the technique for building the synthetic bag follows #155. Co-authored-by: TianHengZhuang <39034691+TianHengZhuang@users.noreply.github.com> Co-authored-by: theluckystrike <51033404+theluckystrike@users.noreply.github.com> Co-Authored-By: Claude Sonnet 5.5 --- src/machinevisiontoolbox/Sources.py | 324 ++++++++++++++++++++++----- tests/create_ros_fixture.py | 102 ++++++++- tests/test_ros_realsense.py | 335 ++++++++++++++++++++++++++++ 3 files changed, 707 insertions(+), 54 deletions(-) create mode 100644 tests/test_ros_realsense.py diff --git a/src/machinevisiontoolbox/Sources.py b/src/machinevisiontoolbox/Sources.py index efe1366a..93d7841d 100644 --- a/src/machinevisiontoolbox/Sources.py +++ b/src/machinevisiontoolbox/Sources.py @@ -49,16 +49,25 @@ try: from rosbags.rosbag1 import Reader as _RosBagReader1 from rosbags.rosbag2 import Reader as _RosBagReader2 + from rosbags.serde import SerdeError as _SerdeError from rosbags.typesys import Stores as _Stores, get_typestore as _get_typestore _rosbags_available = True except ImportError: _RosBagReader1 = None _RosBagReader2 = None + _SerdeError = None _Stores = None _get_typestore = None _rosbags_available = False +# What rosbags raises when a serialized message is not exactly the length its type +# says, eg. there are extra bytes after it. Older versions raise AssertionError, +# newer ones SerdeError. +_LENGTH_ERRORS: tuple[type[BaseException], ...] = ( + (AssertionError,) if _SerdeError is None else (AssertionError, _SerdeError) +) + try: import roslibpy @@ -106,10 +115,108 @@ def _make_image(*args: Any, **kwargs: Any) -> Image: return Image(*args, **kwargs) # pyright: ignore[reportAbstractUsage] -def _set_sample_metadata(sample: Any, timestamp: int, topic: str) -> None: +def _set_sample_metadata( + sample: Any, timestamp: int, topic: str, depth_units: float | None = None +) -> None: """Attach dynamic timestamp/topic metadata used by stream sources.""" setattr(sample, "timestamp", timestamp) setattr(sample, "topic", topic) + if depth_units is not None: + setattr(sample, "depth_units", depth_units) + + +def _wire_byteorder(raw: bytes, cdr: bool) -> str: + """Struct byte order of a serialized ROS message. + + ROS 1 messages are always little-endian. A CDR message says which it is in the + second byte of its 4-byte encapsulation header (0x01 is little-endian). + """ + return "<" if not cdr or (len(raw) > 1 and raw[1] & 0x01) else ">" + + +def _image_payload_end(raw: bytes, msgtype: str, cdr: bool = False) -> int | None: + """Find where the data array of a serialized image message ends + + :param raw: serialized ``sensor_msgs/Image`` or ``sensor_msgs/CompressedImage`` + :param msgtype: ROS message type, ending in ``Image`` or ``CompressedImage`` + :param cdr: ``raw`` is CDR (ROS 2) encoded, otherwise ROS 1 encoded + :return: offset of the first byte after the data array, or ``None`` if ``raw`` does + not have the layout of an image message + :rtype: int or None + + An image message is self-describing: every field before the data array is fixed + size or carries its own length, so the end of the pixel data is computed from the + bytes, not guessed. Anything after it is not part of the message. The Intel + RealSense SDK, for example, appends a 4-byte ``float32`` depth scale to every + image message it records. + """ + order = _wire_byteorder(raw, cdr) + origin = 4 if cdr else 0 # CDR has a 4-byte encapsulation header + pos = origin + + def u32() -> int: + nonlocal pos + if cdr: + pos += (-(pos - origin)) % 4 # CDR aligns fields to their size + if pos + 4 > len(raw): + raise ValueError("truncated") + value = struct.unpack_from(order + "I", raw, pos)[0] + pos += 4 + return value + + def skip(n: int) -> None: + nonlocal pos + pos += n + if pos > len(raw): + raise ValueError("truncated") + + try: + if cdr: + u32() # stamp.sec + u32() # stamp.nanosec + else: + u32() # header.seq (ROS 1 only) + u32() # stamp.sec + u32() # stamp.nsec + skip(u32()) # frame_id, the length of a CDR string includes its NUL + if msgtype.endswith("CompressedImage"): + skip(u32()) # format + skip(u32()) # data + else: + height = u32() + u32() # width + skip(u32()) # encoding + skip(1) # is_bigendian + step = u32() + length = u32() + if length != height * step: + return None + skip(length) + except (ValueError, struct.error): + return None + return pos + + +def _footer_depth_units(footer: bytes, order: str = "<") -> float | None: + """Decode trailing bytes of an image message as a depth scale, if they could be one + + :param footer: the bytes after the data array of an image message + :param order: struct byte order of the message + :return: metres per depth unit, or ``None`` if ``footer`` is not a positive, + finite ``float32`` + :rtype: float or None + + This is only a plausibility test, not a validation. Roughly half of all 4-byte + patterns are a positive, finite ``float32``, so passing it says nothing about + where the bytes came from. What justifies treating them as a depth scale is the + context, which is up to the caller: the bag was recorded by the RealSense SDK, + and the bytes are in the place where the SDK puts its scale (see + :meth:`ROSBag._image_depth_units`). + """ + if len(footer) != 4: + return None + value = struct.unpack(order + "f", footer)[0] + return float(value) if np.isfinite(value) and value > 0 else None class ImageSource(ABC): @@ -2896,6 +3003,33 @@ class ROSBag(ImageSource): Each yielded object carries a ``timestamp`` attribute (ROS nanosecond epoch from the message header) and a ``topic`` attribute (the topic on which it was published). + **Intel RealSense bags** + + The RealSense SDK records a ROS 1 bag in which every image message is followed + by 4 bytes that are not part of ``sensor_msgs/Image``: a ``float32`` holding the + depth scale, in metres per unit, for a depth image and ``0.0`` for a color image. + A message type defines its own length, so a reader that does not know about the + extra bytes cannot decode the images. + + The end of the pixel data is found from the layout of the image message, whose + data array must be ``height * step`` bytes long. The extra bytes are therefore + never mistaken for pixels, and bags without them (such as Intel's own sample + bags) are read correctly too. The pixel values are never changed. + + The extra bytes are interpreted as a depth scale only if the bag is recognised as + coming from the RealSense SDK (it has a ``realsense_msgs`` connection) and they + are a positive, finite ``float32``. The second test alone would prove nothing, + since about half of all 4-byte patterns pass it, so it is the first that + justifies it. A depth image carrying a scale has a ``depth_units`` attribute, + for example 0.001 for pixels in millimetres. Other images do not have the + attribute. + + Any other bytes after an image are ignored, and a ``RuntimeWarning`` is issued, + once for each topic. This covers bytes in a bag that is not recognised as + RealSense, and bytes in one that are not a positive 4-byte scale or ``0.0``. We + do not guess what they are. All other message types must be exactly as long as + their type says. + **Filters** The message and topic filters can be a single string, or a list of strings. @@ -2990,8 +3124,9 @@ def __init__( self.args = kwargs self._tmpfile: str | None = None self._is_ros2: bool = False - self._trim_bytes: int = 0 # trailing bytes to strip (e.g. RealSense SDK footer) self._is_realsense: bool = False + # connections for which unexpected bytes after an image have been reported + self._warned_footers: set[int] = set() def __repr__(self) -> str: return ( @@ -3365,46 +3500,30 @@ def _autodetect_typestore(self, rawdata: bytes, msgtype: str) -> bool: available = {s.name for s in _Stores} # First pass: ROS 1 wire format (ros1 and ros2 typestores). - # RealSense SDK bags are positively identified by their realsense_msgs - # connection types, so the 4-byte trailer is certain — no need to probe - # other trim amounts. - trim_order = (4,) if self._is_realsense else (0, 4, 8) + # For an image the end of the message is computed from its layout, so any + # trailing bytes (eg. the RealSense SDK footer) are ignored. + end = _image_payload_end(rawdata, msgtype) if msgtype.endswith("Image") else None + raw = rawdata if end is None else rawdata[:end] for name in self._ROS1_STORES_ORDERED + self._ROS2_STORES_ORDERED: if name not in available: continue store = _get_typestore(next(s for s in _Stores if s.name == name)) - for trim in trim_order: - raw = rawdata if trim == 0 else rawdata[:-trim] - try: - store.deserialize_ros1(raw, msgtype) - self.typestore = store - self.release = name - self._trim_bytes = trim - suffix = f", {trim}-byte trailer stripped" if trim else "" - print(f" auto-detect: {name} (ros1 encoding{suffix})") - return True - except AssertionError: - # rosbags raises AssertionError when trailing bytes remain - # after parsing (assert pos == len(rawdata)). Try the next - # trim amount. - continue - except (KeyError, UnicodeDecodeError, struct.error) as e: - if self.verbose: - print( - f" auto-detect: {name} (ros1) failed: {type(e).__name__}: {e}" - ) - break # wrong store entirely; move on - except Exception as e: - if self.verbose: - print( - f" auto-detect: {name} (ros1) UNEXPECTED {type(e).__name__}: {e}" - ) - break - else: - # All trim amounts exhausted with AssertionError (trailing bytes) + try: + store.deserialize_ros1(raw, msgtype) + self.typestore = store + self.release = name + print(f" auto-detect: {name} (ros1 encoding)") + return True + except (*_LENGTH_ERRORS, KeyError, UnicodeDecodeError, struct.error) as e: + # wrong store, or the message is the wrong length for it if self.verbose: print( - f" auto-detect: {name} (ros1) failed: AssertionError (trailing bytes)" + f" auto-detect: {name} (ros1) failed: {type(e).__name__}: {e}" + ) + except Exception as e: + if self.verbose: + print( + f" auto-detect: {name} (ros1) UNEXPECTED {type(e).__name__}: {e}" ) # Second pass: CDR wire format (ROS 2 stores only). @@ -3414,14 +3533,25 @@ def _autodetect_typestore(self, rawdata: bytes, msgtype: str) -> bool: continue store = _get_typestore(next(s for s in _Stores if s.name == name)) try: - store.deserialize_cdr(rawdata, msgtype) + cdr_end = ( + _image_payload_end(rawdata, msgtype, cdr=True) + if msgtype.endswith("Image") + else None + ) + store.deserialize_cdr( + rawdata if cdr_end is None else rawdata[:cdr_end], msgtype + ) self.typestore = store self.release = name self._is_ros2 = True # use CDR for all subsequent messages - self._trim_bytes = 0 print(f" auto-detect: {name} (CDR encoding)") return True - except (AssertionError, KeyError, UnicodeDecodeError, struct.error) as e: + except ( + *_LENGTH_ERRORS, + KeyError, + UnicodeDecodeError, + struct.error, + ) as e: if self.verbose: print( f" auto-detect: {name} (CDR) failed: {type(e).__name__}: {e}" @@ -3441,6 +3571,97 @@ def _autodetect_typestore(self, rawdata: bytes, msgtype: str) -> bool: ) return False + def _image_depth_units( + self, connection: Any, rawdata: bytes, footer: bytes + ) -> float | None: + """Interpret the bytes after an image message + + :param connection: bag connection the message was read from + :param rawdata: serialized message, including ``footer`` + :param footer: the bytes after the data array of the image + :return: depth scale in metres per unit, or ``None`` if there is none + :rtype: float or None + + A serialization format cannot have fields that change length, so bytes after + an image are not part of the message. The RealSense SDK adds a 4-byte + ``float32`` to every image it records, though, and we need to know what to + make of it. Whether 4 bytes are a valid scale cannot be decided from the + bytes, since about half of all patterns are a positive, finite ``float32``. + The context is what decides: + + - no footer: ``None``; + - up to 3 zero bytes in a ROS 2 (CDR) message: padding to a 4-byte boundary, + ``None``; + - the bag is recognised as RealSense (it has a ``realsense_msgs`` + connection) and the footer is 4 zero bytes: ``0.0`` is what the SDK writes + for a color image, which has no scale, ``None``; + - the bag is recognised as RealSense and the footer is a positive, finite + ``float32``: that is the depth scale; + - anything else, including 4 bytes in a bag not recognised as RealSense: + not understood, so ignored with a ``RuntimeWarning``, issued once for each + connection. We do not guess. + """ + if not footer: + return None + if self._is_ros2 and len(footer) < 4 and not any(footer): + return None # CDR alignment padding + if self._is_realsense and len(footer) == 4: + if not any(footer): + return None # 0.0, a color image + units = _footer_depth_units( + footer, _wire_byteorder(rawdata, self._is_ros2) + ) + if units is not None: + return units + if connection.id not in self._warned_footers: + self._warned_footers.add(connection.id) + warnings.warn( + f"ROSBag: ignoring {len(footer)} unexpected byte(s) " + f"({footer[:8].hex()}) after the image data on {connection.topic!r}: " + + ( + "not a RealSense depth scale" + if self._is_realsense + else "the bag is not recognised as recorded by the RealSense SDK" + ), + RuntimeWarning, + stacklevel=2, + ) + return None + + def _decode(self, connection: Any, rawdata: bytes) -> tuple[Any, bytes]: + """Deserialize a message and return it with any bytes that follow it + + :param connection: bag connection the message was read from + :param rawdata: serialized message + :raises SerdeError: the message cannot be decoded (or ``AssertionError`` with + older versions of ``rosbags``) + :return: message, bytes after the message + :rtype: tuple(message, bytes) + + For an image message the end of the pixel data is computed from the layout of + the message, so whatever follows is returned as the second element, rather + than being guessed. The RealSense SDK, for example, follows every image with + a 4-byte ``float32`` depth scale. Any other message, or an image that does + not have the expected layout, must be exactly the length its type says, and + nothing follows it. + """ + assert self.typestore is not None + deserialize = ( + self.typestore.deserialize_cdr + if self._is_ros2 + else self.typestore.deserialize_ros1 + ) + msgtype = connection.msgtype + + if msgtype.endswith("Image"): + end = _image_payload_end(rawdata, msgtype, self._is_ros2) + if end is not None: + return cast(Any, deserialize(rawdata[:end], msgtype)), bytes( + rawdata[end:] + ) + + return cast(Any, deserialize(rawdata, msgtype)), b"" + def __iter__(self) -> Iterator[Any]: self._open_reader() assert self.reader is not None @@ -3460,24 +3681,15 @@ def __iter__(self) -> Iterator[Any]: ) continue _auto = False # found it, use for all subsequent messages - _deser = ( - self.typestore.deserialize_cdr - if self._is_ros2 - else self.typestore.deserialize_ros1 - ) - _raw = ( - rawdata - if self._trim_bytes == 0 - else rawdata[: -self._trim_bytes] - ) - msg = cast(Any, _deser(_raw, connection.msgtype)) + msg, footer = self._decode(connection, rawdata) + depth_units = self._image_depth_units(connection, rawdata, footer) except KeyError as e: if self.verbose: print( f"{self.format_local_time(timestamp)} Error occurred while deserializing message: {e}" ) continue - except AssertionError: + except _LENGTH_ERRORS: if self.verbose: print( f"{self.format_local_time(timestamp)} Could not deserialize {connection.msgtype} on {connection.topic!r} " @@ -3520,7 +3732,10 @@ def __iter__(self) -> Iterator[Any]: else: img = _make_image(img_array, **self.args) _set_sample_metadata( - img, self._stamp_to_ns(msg.header.stamp), connection.topic + img, + self._stamp_to_ns(msg.header.stamp), + connection.topic, + depth_units=depth_units, ) yield img @@ -3572,7 +3787,10 @@ def __iter__(self) -> Iterator[Any]: else: img = _make_image(img_array, **self.args) _set_sample_metadata( - img, self._stamp_to_ns(msg.header.stamp), connection.topic + img, + self._stamp_to_ns(msg.header.stamp), + connection.topic, + depth_units=depth_units, ) yield img diff --git a/tests/create_ros_fixture.py b/tests/create_ros_fixture.py index edb6fae6..5e24e66b 100644 --- a/tests/create_ros_fixture.py +++ b/tests/create_ros_fixture.py @@ -29,6 +29,7 @@ import io import shutil +import struct from pathlib import Path import numpy as np @@ -36,7 +37,7 @@ from rosbags.rosbag1 import Writer as Writer1 from rosbags.rosbag2 import Writer as Writer2 -from rosbags.typesys import Stores, get_typestore +from rosbags.typesys import Stores, get_types_from_msg, get_typestore HERE = Path(__file__).parent DATADIR = HERE / "data" @@ -232,6 +233,105 @@ def _make_ros2() -> None: ) +RS_DEPTH_TOPIC = "/device_0/sensor_0/Depth_0/image/data" +RS_COLOR_TOPIC = "/device_0/sensor_1/Color_0/image/data" +RS_DEPTH_UNITS = 0.001 # metres per depth unit, as in a D4xx camera +RS_W, RS_H = 5, 3 # odd sizes, so strides and alignment are exercised + + +def realsense_depth_frame(i: int) -> np.ndarray: + """Expected pixels of depth frame ``i`` in a bag from :func:`make_realsense_bag`.""" + return (np.arange(RS_H * RS_W, dtype=np.uint16).reshape(RS_H, RS_W) * 7 + 100 * i) + + +def realsense_color_frame(i: int) -> np.ndarray: + """Expected pixels of color frame ``i`` in a bag from :func:`make_realsense_bag`.""" + return (np.arange(RS_H * RS_W * 3, dtype=np.uint8).reshape(RS_H, RS_W, 3) + 10 * i) + + +def make_realsense_bag( + path: Path, + *, + footer: bool | bytes = True, + wire: str = "ros1", + streaminfo: bool = True, + n: int = 3, +) -> None: + """Write a small bag like those recorded by the Intel RealSense SDK + + :param path: bag file to write + :param footer: append the 4-byte ``float32`` depth scale that the SDK adds to + every image message (0.0 on the color stream), or give the bytes to append to + every image message instead + :param wire: serialization of the messages, ``"ros1"``, ``"cdr_le"`` or + ``"cdr_be"``. The SDK uses ROS 1, with ROS 2 style message type names. + :param streaminfo: include a ``realsense_msgs/msg/StreamInfo`` connection, which + is how the reader recognises a RealSense bag + :param n: number of frames in each of the depth and color streams + + The topics, the ``mono16`` depth encoding and the layout match a real SDK + recording (`bags/20260430_125558.bag`); the images are tiny. The expected pixels + are given by :func:`realsense_depth_frame` and :func:`realsense_color_frame`. + A bag with ``footer=False`` is what Intel's own sample bag looks like. + """ + ros1 = wire == "ros1" + ts = get_typestore(Stores.ROS1_NOETIC if ros1 else Stores.ROS2_HUMBLE) + Header = ts.types["std_msgs/msg/Header"] + Time = ts.types["builtin_interfaces/msg/Time"] + Image = ts.types["sensor_msgs/msg/Image"] + info_type = "realsense_msgs/msg/StreamInfo" + ts.register(get_types_from_msg("uint32 fps\nstring encoding\n", info_type)) + + def serialize(msg: object, msgtype: str) -> bytes: + if ros1: + return bytes(ts.serialize_ros1(msg, msgtype)) + return bytes(ts.serialize_cdr(msg, msgtype, little_endian=wire == "cdr_le")) + + def footer_bytes(value: float) -> bytes: + if isinstance(footer, bytes): + return footer + return struct.pack(">f" if wire == "cdr_be" else " list: + with redirect_stdout(io.StringIO()): # auto-detection prints what it found + return list(ROSBag(str(path), release=release)) + + +def read_with_warnings(path: Path, release: str) -> tuple[list, list]: + """All the images in a bag and the RuntimeWarnings issued while reading them.""" + with warnings.catch_warnings(record=True) as caught: + warnings.simplefilter("always") + images = read_all(path, release) + return images, [w for w in caught if issubclass(w.category, RuntimeWarning)] + + +# --------------------------------------------------------------------------- +# finding the end of the pixel data from the layout of the message +# --------------------------------------------------------------------------- + +WIRES = ["ros1", "cdr_le", "cdr_be"] + + +def serialize_image( + wire: str, + msgtype: str = "sensor_msgs/msg/Image", + frame_id: str = "", + encoding: str = "mono16", + width: int = 5, + height: int = 3, + data_len: int | None = None, +) -> bytes: + ros1 = wire == "ros1" + ts = get_typestore(Stores.ROS1_NOETIC if ros1 else Stores.ROS2_HUMBLE) + Time = ts.types["builtin_interfaces/msg/Time"] + Header = ts.types["std_msgs/msg/Header"] + stamp = Time(sec=1, nanosec=2) + header = ( + Header(seq=9, stamp=stamp, frame_id=frame_id) + if ros1 + else Header(stamp=stamp, frame_id=frame_id) + ) + if msgtype.endswith("CompressedImage"): + msg = ts.types[msgtype]( + header=header, + format="jpeg", + data=np.arange(11, dtype=np.uint8), + ) + else: + step = width * {"mono16": 2, "rgb8": 3, "mono8": 1}[encoding] + msg = ts.types[msgtype]( + header=header, + height=height, + width=width, + encoding=encoding, + is_bigendian=0, + step=step, + # normally height * step bytes, a different length is not a valid Image + data=np.zeros(height * step if data_len is None else data_len, np.uint8), + ) + if ros1: + return bytes(ts.serialize_ros1(msg, msgtype)) + return bytes(ts.serialize_cdr(msg, msgtype, little_endian=wire == "cdr_le")) + + +@pytest.mark.parametrize("wire", WIRES) +@pytest.mark.parametrize("msgtype", ["sensor_msgs/msg/Image", "sensor_msgs/msg/CompressedImage"]) +@pytest.mark.parametrize("frame_id", ["", "c", "cam", "camera_1"]) # alignment +@pytest.mark.parametrize("encoding", ["mono16", "rgb8", "mono8"]) +@pytest.mark.parametrize("extra", [b"", b"\x00" * 4, b"\x6f\x12\x83\x3a", b"\x01" * 8]) +def test_payload_end_ignores_trailing_bytes(wire, msgtype, frame_id, encoding, extra): + raw = serialize_image(wire, msgtype, frame_id, encoding) + end = _image_payload_end(raw + extra, msgtype, cdr=wire != "ros1") + assert end == len(raw) + + +@pytest.mark.parametrize("wire", WIRES) +def test_payload_end_rejects_what_is_not_an_image(wire): + cdr = wire != "ros1" + msgtype = "sensor_msgs/msg/Image" + raw = serialize_image(wire) + assert _image_payload_end(b"", msgtype, cdr) is None + assert _image_payload_end(b"\x00" * 3, msgtype, cdr) is None + assert _image_payload_end(raw[:-1], msgtype, cdr) is None # data array cut short + assert _image_payload_end(raw[:20], msgtype, cdr) is None # inside the header + # the data array is not height * step bytes long, so this is not a valid Image + for data_len in (3 * 10 - 1, 3 * 10 + 1, 0): + assert _image_payload_end(serialize_image(wire, data_len=data_len), msgtype, cdr) is None + assert _image_payload_end(bytes(range(200)), msgtype, cdr) is None + + +# --------------------------------------------------------------------------- +# the footer +# --------------------------------------------------------------------------- + + +@pytest.mark.parametrize("order", ["<", ">"]) +def test_footer_is_a_positive_finite_float32(order): + assert _footer_depth_units(struct.pack(order + "f", 0.001), order) == pytest.approx( + 0.001, rel=1e-6 + ) + for bad in (0.0, -0.001, float("nan"), float("inf")): + assert _footer_depth_units(struct.pack(order + "f", bad), order) is None + + +@pytest.mark.parametrize("footer", [b"", b"\x00", b"\x6f\x12\x83", b"\x6f\x12\x83\x3a\x00"]) +def test_footer_must_be_four_bytes(footer): + assert _footer_depth_units(footer) is None + + +# --------------------------------------------------------------------------- +# bags +# --------------------------------------------------------------------------- + +CASES = [ + # (wire, footer, streaminfo, release) + *itertools.product(["ros1"], [True, False], [True, False], ["ROS1_NOETIC", "auto"]), + # a CDR message in a ROS 1 bag can only be found by auto-detection + *itertools.product(["cdr_le", "cdr_be"], [True, False], [True, False], ["auto"]), +] + + +@pytest.mark.parametrize("wire, footer, streaminfo, release", CASES) +def test_realsense_style_bag_is_read_exactly(tmp_path, wire, footer, streaminfo, release): + path = tmp_path / "rs.bag" + make_realsense_bag(path, footer=footer, wire=wire, streaminfo=streaminfo, n=3) + + images, caught = read_with_warnings(path, release) + + depth = [i for i in images if i.topic == RS_DEPTH_TOPIC] + color = [i for i in images if i.topic == RS_COLOR_TOPIC] + assert (len(depth), len(color)) == (3, 3) # nothing silently dropped + for k, im in enumerate(depth): + np.testing.assert_array_equal(im.array, realsense_depth_frame(k)) + for k, im in enumerate(color): + np.testing.assert_array_equal(im.array, realsense_color_frame(k)) + + # 4 bytes are only taken to be a depth scale in a bag recognised as RealSense + for im in depth: + if footer and streaminfo: + assert im.depth_units == pytest.approx(RS_DEPTH_UNITS, rel=1e-6) + else: + assert not hasattr(im, "depth_units") + for im in color: # the color stream's footer is 0.0, which is not a scale + assert not hasattr(im, "depth_units") + + # bytes that are not understood are reported, once for each topic + if footer and not streaminfo: + assert len(caught) == 2 + assert all("not recognised" in str(w.message) for w in caught) + else: + assert not caught + + +@pytest.mark.parametrize( + "footer", + [ + b"\x01" * 8, # the wrong length + b"\x00\x00\x80\xbf", # -1.0 + b"\x00\x00\xc0\x7f", # NaN + b"\x00\x00\x80\x7f", # infinity + b"\x6f\x12\x83", # 3 bytes + ], +) +def test_unexpected_bytes_after_an_image_are_ignored_and_reported(tmp_path, footer): + path = tmp_path / "rs.bag" + make_realsense_bag(path, footer=footer, streaminfo=True, n=3) + + images, caught = read_with_warnings(path, "auto") + + assert len(images) == 6 # the images are still read, exactly + depth = [i for i in images if i.topic == RS_DEPTH_TOPIC] + for k, im in enumerate(depth): + np.testing.assert_array_equal(im.array, realsense_depth_frame(k)) + assert not any(hasattr(i, "depth_units") for i in images) + assert len(caught) == 2 # one for each of the two topics, not one per image + assert all("not a RealSense depth scale" in str(w.message) for w in caught) + + +@pytest.mark.parametrize("padding", [b"\x00", b"\x00\x00", b"\x00\x00\x00"]) +def test_cdr_alignment_padding_after_an_image_is_silent(tmp_path, padding): + path = tmp_path / "rs.bag" + make_realsense_bag(path, footer=padding, wire="cdr_le", streaminfo=True, n=3) + + images, caught = read_with_warnings(path, "auto") + + assert len(images) == 6 and not caught + assert not any(hasattr(i, "depth_units") for i in images) + + +def test_realsense_bag_without_footer_is_not_truncated(tmp_path): + """Intel's sample bag has realsense_msgs connections but no footers. + + The reader used to see realsense_msgs, assume a 4-byte footer, remove 4 real bytes + of every image and, with ``release="auto"``, return no images at all. + """ + path = tmp_path / "sample.bag" + make_realsense_bag(path, footer=False, streaminfo=True) + images = read_all(path, "auto") + assert len(images) == 6 + np.testing.assert_array_equal(images[-1].array.shape[:2], (3, 5)) + + +def test_ordinary_bag_has_no_depth_units(): + ros1 = HERE / "data" / "test_ros1.bag" + if not ros1.exists(): + pytest.skip("fixture bag not generated") + for im in read_all(ros1, "ROS1_NOETIC"): + assert not hasattr(im, "depth_units") + + +# --------------------------------------------------------------------------- +# messages that are not images must be exactly the length their type says +# --------------------------------------------------------------------------- + + +def _float32_message(tmp_path): + path = tmp_path / "rs.bag" + make_realsense_bag(path, footer=False) + bag = ROSBag(str(path), release="ROS1_NOETIC") + ts = bag.typestore + msgtype = "std_msgs/msg/Float32" + raw = bytes(ts.serialize_ros1(ts.types[msgtype](data=1.5), msgtype)) + return bag, SimpleNamespace(msgtype=msgtype, id=7), raw + + +def test_non_image_message_is_decoded_at_its_exact_length(tmp_path): + bag, conn, raw = _float32_message(tmp_path) + msg, rest = bag._decode(conn, raw) + assert msg.data == 1.5 + assert rest == b"" + + +@pytest.mark.parametrize("extra", [b"\x00" * 4, b"\x00" * 8]) +def test_non_image_message_with_extra_bytes_is_an_error(tmp_path, extra): + """Nothing is guessed: a message of the wrong length is not decoded.""" + bag, conn, raw = _float32_message(tmp_path) + with pytest.raises(_LENGTH_ERRORS): + bag._decode(conn, raw + extra) + + +# --------------------------------------------------------------------------- +# real bags, only in a development checkout (bags/ is not in the repository) +# --------------------------------------------------------------------------- + + +def _real(name: str) -> Path: + path = REAL_BAGS / name + if not path.exists(): + pytest.skip(f"real bag {name} not available") + return path + + +@pytest.mark.parametrize("release", ["ROS1_NOETIC", "auto"]) +def test_real_realsense_bag(release): + path = _real("20260430_125558.bag") + with redirect_stdout(io.StringIO()), warnings.catch_warnings(): + warnings.simplefilter("error", RuntimeWarning) # a real bag raises no warnings + images = list(itertools.islice(ROSBag(str(path), release=release), 10)) + assert len(images) == 10 + depth = [i for i in images if "Depth" in i.topic] + color = [i for i in images if "Color" in i.topic] + assert depth and color + assert depth[0].shape == (720, 1280) and depth[0].dtype == np.uint16 + assert color[0].shape == (720, 1280, 3) + assert all(i.depth_units == pytest.approx(0.001, rel=1e-6) for i in depth) + assert not any(hasattr(i, "depth_units") for i in color) + + +@pytest.mark.parametrize("release", ["ROS1_NOETIC", "auto"]) +def test_real_intel_sample_bag(release): + path = _real("single_depth_color_640x480.bag") + images = read_all(path, release) + assert len(images) == 3 # 2 depth, 1 color + assert sorted(i.shape for i in images) == [(480, 640), (480, 640), (480, 640, 3)] + assert not any(hasattr(i, "depth_units") for i in images) + + +@pytest.mark.parametrize("release", ["humble", "auto"]) +def test_real_ros2_mcap_bag(release): + """A ROS 2 recording (rosbag2, MCAP, CDR): no footers, so nothing is reported.""" + path = _real("ros2_bouncing_ball") + with redirect_stdout(io.StringIO()), warnings.catch_warnings(): + warnings.simplefilter("error", RuntimeWarning) + images = list(ROSBag(str(path), release=release)) + # The bag's metadata claims 132 + 133 images, but its files only hold 39 + 40. + assert len(images) == 79 + shapes = {i.topic: i.shape for i in images} + assert shapes == {"/image": (200, 320, 3), "/modified_image": (200, 320)} + assert not any(hasattr(i, "depth_units") for i in images)