diff --git a/src/machinevisiontoolbox/Sources.py b/src/machinevisiontoolbox/Sources.py index 6f539fd0..6ad9fd08 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): @@ -2914,6 +3021,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. @@ -3008,8 +3142,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 ( @@ -3383,46 +3518,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). @@ -3432,14 +3551,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}" @@ -3459,6 +3589,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 @@ -3478,24 +3699,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} " @@ -3538,7 +3750,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 @@ -3590,7 +3805,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)