diff --git a/.github/workflows/coverage.yml b/.github/workflows/coverage.yml index 2ade89c0..39b6a390 100644 --- a/.github/workflows/coverage.yml +++ b/.github/workflows/coverage.yml @@ -60,7 +60,7 @@ jobs: # tested package depends on them (--packages-up-to), just not measured. - uses: ros-tooling/action-ros-ci@v0.4 with: - package-name: rtabmap_conversions rtabmap_util rtabmap_sync + package-name: rtabmap_conversions rtabmap_util rtabmap_sync rtabmap_python target-ros2-distro: humble # RTAB-Map is installed in the image, not as an apt package, so rosdep # cannot resolve the key and must not try. @@ -75,6 +75,9 @@ jobs: { "build": { "mixin": ["coverage-gcc"] + }, + "test": { + "pytest-with-coverage": true } } # Pinned so a change in the mixin repository cannot break this job. @@ -115,6 +118,8 @@ jobs: # `source` does not exist. GitHub picks sh whenever it cannot find # bash in the image's PATH, and says so in the log ("shell: sh -e"). . /opt/ros/humble/setup.sh + # C++ packages only -- rtabmap_python emits no .gcno for lcov to read, + # and is measured by the coveragepy step below instead. PKGS="rtabmap_conversions rtabmap_util rtabmap_sync" # Baseline from the .gcno files. Without it a source file that no test # ever loaded is missing from the report altogether rather than @@ -129,10 +134,34 @@ jobs: '*CompilerId*' '*/CMakeFiles/*' || true test -s lcov/total_coverage.info - # Fails the job if the step above produced nothing -- the failure mode - # this workflow has hit twice already is a green run that measured zero. - - name: Coverage summary - run: lcov --summary ros_ws/lcov/total_coverage.info + # Python coverage is a separate mechanism: the coverage-gcc mixin only adds + # --coverage to the compiler, which does nothing for an ament_python package. + # colcon test --pytest-with-coverage (set above) writes a Cobertura report into + # the package's own build directory instead. + # + # Its paths are relative to the package rather than the repository, so + # cv_compression.py arrives as "rtabmap_python/cv_compression.py" -- one level + # short of where it really lives. Rewrite them here rather than leave Codecov to + # guess, which it does by suffix and can get wrong. + - name: Python coverage report + working-directory: ros_ws + run: | + python3 - <<'EOF' + import pathlib + import xml.etree.ElementTree as ET + p = pathlib.Path('build/rtabmap_python/coverage.xml') + if not p.is_file(): + print('::warning::no python coverage produced for rtabmap_python') + raise SystemExit(0) + tree = ET.parse(p) + root = tree.getroot() + for source in root.iter('source'): + source.text = '.' + for cls in root.iter('class'): + cls.set('filename', 'rtabmap_python/' + cls.get('filename')) + tree.write(p, xml_declaration=True, encoding='utf-8') + print('rewrote', p, 'to repository-relative paths') + EOF # colcon lcov-result runs genhtml itself, into the same lcov/ directory. - name: Upload HTML coverage artifact @@ -146,7 +175,7 @@ jobs: if: ${{ env.CODECOV_TOKEN != '' }} uses: codecov/codecov-action@v5 with: - files: ros_ws/lcov/total_coverage.info + files: ros_ws/lcov/total_coverage.info,ros_ws/build/rtabmap_python/coverage.xml # Upload ONLY the aggregated lcov file. By default the CLI also walks # the tree and runs gcov over every .gcno it finds, which re-adds the # test sources the --filter above just dropped. diff --git a/README.md b/README.md index 0be9987b..51bb9c3c 100644 --- a/README.md +++ b/README.md @@ -52,7 +52,7 @@ The stack is split into small packages so a pipeline only pulls in what it uses. |---|---| | `rtabmap_msgs` | Message, service and action definitions used across the stack. | | [`rtabmap_conversions`](rtabmap_conversions/README.md) | C++ library converting between RTAB-Map library types and ROS 2 messages. | -| `rtabmap_python` | Python helpers, currently image compression matching RTAB-Map's own format. | +| [`rtabmap_python`](rtabmap_python/README.md) | Python helpers for RTAB-Map's own binary formats, currently the compressed matrices carried in `rtabmap_msgs` fields and database blobs. | ### Visualization diff --git a/codecov.yml b/codecov.yml index a826350c..2e315d02 100644 --- a/codecov.yml +++ b/codecov.yml @@ -56,13 +56,18 @@ component_management: name: rtabmap_sync paths: - rtabmap_sync/** + - component_id: rtabmap_python + name: rtabmap_python + paths: + - rtabmap_python/** comment: layout: "condensed_header, diff, components, files" behavior: default require_changes: true # stay quiet when coverage doesn't move -# Only rtabmap_conversions, rtabmap_util and rtabmap_sync have tests today, so +# Only rtabmap_conversions, rtabmap_util, rtabmap_sync and rtabmap_python have tests +# today, so # everything else would report as 0% and drag the total down to a number that # says nothing. As a package gains tests, drop its line here and add it to # individual_components above. @@ -73,8 +78,8 @@ ignore: - "rtabmap_launch/**" - "rtabmap_msgs/**" - "rtabmap_odom/**" - - "rtabmap_python/**" - "rtabmap_rviz_plugins/**" - "rtabmap_slam/**" - "rtabmap_viz/**" - "**/test/**" + - "**/setup.py" # packaging scaffolding, not code under test diff --git a/rtabmap_python/README.md b/rtabmap_python/README.md new file mode 100644 index 00000000..a57dbf58 --- /dev/null +++ b/rtabmap_python/README.md @@ -0,0 +1,49 @@ +# rtabmap_python + +Python helpers for reading and writing the binary formats [RTAB-Map](https://github.com/introlab/rtabmap) uses. + +RTAB-Map is a C++ library, and the data it hands to ROS is not always plain ROS types. Several `rtabmap_msgs` fields — and every blob in an `.db` database — carry a matrix in RTAB-Map's own compressed encoding rather than as a `sensor_msgs/Image` or an array. This package is the Python side of that encoding, for scripts that read those fields without going through the C++ library. + +There are no nodes here. It is an `ament_python` package that installs one importable module. + +## Module + +`rtabmap_python.cv_compression` — a single-channel `cv::Mat` to and from bytes. + +| Function | Description | +|---|---| +| `compress(data)` | 1-D or 2-D numpy array → `bytearray`. | +| `uncompress(data)` | those bytes → 2-D numpy array. | + +```python +import numpy as np +from rtabmap_python.cv_compression import compress, uncompress + +scan = np.zeros((360, 2), dtype=np.float32) +blob = compress(scan) # what the message field carries +restored = uncompress(blob) # (360, 2) float32 +``` + +The encoding is a zlib stream followed by a 12-byte trailer holding rows, cols and the element type as three `int32`. It matches `compressData()` and `uncompressData()` in RTAB-Map's `corelib/src/Compression.cpp` byte for byte, so either side can read what the other wrote. The [module docstring](rtabmap_python/cv_compression.py) has the exact layout, and the generated [Python API reference](https://docs.ros.org/en/jazzy/p/rtabmap_python/) renders it alongside the two functions. + +## Things worth knowing + +**The result is read-only.** `uncompress` views the decompressed buffer instead of copying it, so the array it returns has `writeable=False` and assigning into it raises. Call `.copy()` if you need to modify it. + +**Single-channel only.** The C++ encoder packs the channel count into the type code; the tables here cover the single-channel depths `CV_8U` through `CV_64F`. A multi-channel matrix written by the C++ side raises `KeyError` rather than decoding wrongly. So does an unsupported dtype on the way in — `int64` and `float16` have no encoding. + +**The trailer is host-endian**, because the C++ side writes raw `int`s. A blob is not portable between machines of opposite endianness. + +## Building and testing + +```bash +colcon build --packages-select rtabmap_python +colcon test --packages-select rtabmap_python +colcon test-result --verbose +``` + +The tests cover the round trip for every supported depth, the exact trailer bytes against the codes RTAB-Map's `serializeMatType()` produces, and each of the behaviours above. Alongside them, the standard `ament_copyright`, `ament_flake8` and `ament_pep257` linters run over the package. + +## License + +BSD-3-Clause. See the [repository root](https://github.com/introlab/rtabmap_ros#license). diff --git a/rtabmap_python/package.xml b/rtabmap_python/package.xml index 4a5d7e84..c8054481 100644 --- a/rtabmap_python/package.xml +++ b/rtabmap_python/package.xml @@ -10,6 +10,8 @@ https://github.com/introlab/rtabmap_ros/issues https://github.com/introlab/rtabmap_ros + python3-numpy + ament_copyright ament_flake8 ament_pep257 @@ -17,5 +19,6 @@ ament_python + rosdoc2.yaml diff --git a/rtabmap_python/rosdoc2.yaml b/rtabmap_python/rosdoc2.yaml new file mode 100644 index 00000000..ae1e28c9 --- /dev/null +++ b/rtabmap_python/rosdoc2.yaml @@ -0,0 +1,29 @@ +## Configuration for rosdoc2, the documentation generator used by docs.ros.org. +## Regenerate the annotated default with: +## rosdoc2 default_config --package-path rtabmap_python +## Build the docs locally with: +## rosdoc2 build --package-path rtabmap_python --output-directory doc_output + +## This 'attic section' self-documents this file's type and version. +type: 'rosdoc2 config' +version: 1 + +--- + +settings: + ## Generate the standard index page from package.xml (description, maintainer, + ## license, links) and a table of contents for the builders below. + generate_package_index: true + + ## This is an ament_python package: there are no C/C++ headers to parse, and the + ## API reference comes from the module docstrings via sphinx-apidoc. + always_run_doxygen: false + always_run_sphinx_apidoc: true + +builders: + ## Sphinx renders the landing page and the autodoc pages sphinx-apidoc produces + ## from rtabmap_python/. + - sphinx: { + name: 'rtabmap_python', + output_dir: '' + } diff --git a/rtabmap_python/rtabmap_python/cv_compression.py b/rtabmap_python/rtabmap_python/cv_compression.py index 0b8e1604..82769d89 100644 --- a/rtabmap_python/rtabmap_python/cv_compression.py +++ b/rtabmap_python/rtabmap_python/cv_compression.py @@ -27,6 +27,35 @@ # POSSIBILITY OF SUCH DAMAGE. +""" +Compress numpy arrays into RTAB-Map's ``cv::Mat`` wire format. + +RTAB-Map stores and transmits matrices -- images, laser scans, descriptors -- as a zlib +payload followed by a 12-byte trailer recording the shape and the element type. Database +blobs and the compressed fields of ``rtabmap_msgs`` messages both use it. + +The layout is:: + + [ zlib stream of the elements in C order ][ rows ][ cols ][ type ] + int32 int32 int32 + +The three trailer fields are written with ``struct`` format ``'iii'`` -- native byte order +and size, matching the C++ side's raw ``int`` writes. That makes the encoding +**host-endian**, so a blob does not travel between machines of opposite endianness. + +``type`` is the OpenCV depth of the elements: 0 ``CV_8U``, 1 ``CV_8S``, 2 ``CV_16U``, +3 ``CV_16S``, 4 ``CV_32S``, 5 ``CV_32F``, 6 ``CV_64F``. + +These two functions are the Python side of that format. They match ``compressData()`` and +``uncompressData()`` in RTAB-Map's ``corelib/src/Compression.cpp`` byte for byte, so a +matrix written by either side can be read by the other. + +Single-channel matrices only. The C++ encoder packs the channel count into the type code +alongside the depth; the tables here cover the single-channel depths ``CV_8U`` through +``CV_64F``, which is what the codes 0 to 6 mean. +""" + + import struct import zlib @@ -34,6 +63,20 @@ import numpy as np def compress(data): + """ + Compress a 1-D or 2-D array into RTAB-Map's format. + + :param data: a single-channel array whose dtype is one of ``uint8``, ``int8``, + ``uint16``, ``int16``, ``int32``, ``float32`` or ``float64``. A 1-D array of + length ``n`` is recorded as a 1-by-``n`` matrix, which is the shape + :func:`uncompress` gives back. Any memory layout is accepted; the bytes are + always written in C order. + :returns: a ``bytearray`` holding the zlib payload followed by the trailer described + in the module docstring. + :raises AssertionError: if ``data`` has more than two dimensions. + :raises KeyError: if its dtype is not one of the seven above -- ``int64`` and + ``float16`` have no encoding in this format. + """ assert data.ndim == 1 or data.ndim == 2 dim1 = 1 @@ -63,6 +106,16 @@ def compress(data): def uncompress(data): + """ + Restore an array written by :func:`compress` or by RTAB-Map's C++ side. + + :param data: a bytes-like object laid out as :func:`compress` returns. + :returns: a 2-D array of the recorded shape and dtype. It is 2-D even when + :func:`compress` was handed a 1-D array, and it is **read-only**: it views the + decompressed buffer instead of copying it, so call ``.copy()`` before writing. + :raises KeyError: if the trailer's type code is not a single-channel depth 0 to 6, + which is what a multi-channel matrix from the C++ side encodes to. + """ cvtype_to_numpy_type = { 0: 'uint8', 1: 'int8', diff --git a/rtabmap_python/test/test_cv_compression.py b/rtabmap_python/test/test_cv_compression.py new file mode 100644 index 00000000..55d46c47 --- /dev/null +++ b/rtabmap_python/test/test_cv_compression.py @@ -0,0 +1,178 @@ +# Copyright 2025 matlabbe +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the matlabbe nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + + +"""Tests for :mod:`rtabmap_python.cv_compression`.""" + +import struct +import zlib + +import numpy as np +import pytest + +from rtabmap_python.cv_compression import compress, uncompress + + +# The single-channel depths the format encodes, with the codes RTAB-Map's +# serializeMatType() gives them: CV_8U, CV_8S, CV_16U, CV_16S, CV_32S, CV_32F, CV_64F. +SUPPORTED_TYPES = [ + ('uint8', 0), + ('int8', 1), + ('uint16', 2), + ('int16', 3), + ('int32', 4), + ('float32', 5), + ('float64', 6), +] + +# Three int32: rows, cols, type code. +TRAILER_SIZE = 3 * 4 + + +@pytest.mark.parametrize('dtype,code', SUPPORTED_TYPES) +def test_roundtrip_preserves_shape_dtype_and_values(dtype, code): + """Every supported depth survives a compress/uncompress cycle unchanged.""" + data = np.arange(12, dtype=dtype).reshape(3, 4) + + result = uncompress(compress(data)) + + assert result.shape == (3, 4) + assert result.dtype == np.dtype(dtype) + assert np.array_equal(result, data) + + +@pytest.mark.parametrize('dtype,code', SUPPORTED_TYPES) +def test_trailer_records_rows_cols_and_type_code(dtype, code): + """The last 12 bytes are rows, cols and the type code, as the C++ side writes them.""" + data = np.zeros((3, 4), dtype=dtype) + + trailer = bytes(compress(data)[-TRAILER_SIZE:]) + + assert trailer == struct.pack('iii', 3, 4, code) + + +def test_payload_is_plain_zlib_of_the_c_order_bytes(): + """Everything before the trailer is a zlib stream, so the C++ side can inflate it.""" + data = np.arange(6, dtype=np.uint8) + + payload = bytes(compress(data)[:-TRAILER_SIZE]) + + assert zlib.decompress(payload) == data.tobytes() + + +def test_compress_returns_a_bytearray(): + """The return type is a bytearray, which is what the message fields expect.""" + assert isinstance(compress(np.zeros(4, dtype=np.uint8)), bytearray) + + +def test_one_dimensional_input_comes_back_as_a_single_row(): + """A 1-D array is recorded as 1-by-n, so the roundtrip is not shape-preserving.""" + data = np.arange(5, dtype=np.float32) + + result = uncompress(compress(data)) + + assert result.shape == (1, 5) + assert np.array_equal(result.ravel(), data) + + +@pytest.mark.parametrize('shape', [(1, 5), (5, 1), (2, 3)]) +def test_two_dimensional_shapes_are_preserved_exactly(shape): + """Rows and cols are recorded separately, so no 2-D shape is transposed or flattened.""" + data = np.arange(5 if 1 in shape else 6, dtype=np.uint8).reshape(shape) + + assert uncompress(compress(data)).shape == shape + + +def test_non_contiguous_input_roundtrips(): + """A transposed view is written in C order, so it reads back as the same matrix.""" + data = np.arange(12, dtype=np.int16).reshape(3, 4).T + assert not data.flags.c_contiguous + + result = uncompress(compress(data)) + + assert result.shape == (4, 3) + assert np.array_equal(result, data) + + +def test_empty_array_roundtrips_as_an_empty_row(): + """An empty array is not a special case; it comes back as a 1-by-0 matrix.""" + result = uncompress(compress(np.array([], dtype=np.uint8))) + + assert result.shape == (1, 0) + assert result.dtype == np.uint8 + + +def test_uncompressed_array_is_read_only(): + """uncompress() views the decompressed buffer rather than copying it.""" + result = uncompress(compress(np.arange(4, dtype=np.uint8))) + + assert not result.flags.writeable + with pytest.raises(ValueError): + result[0, 0] = 1 + # .copy() is the way out, as the docstring says. + assert result.copy().flags.writeable + + +def test_accepts_bytes_as_well_as_bytearray(): + """uncompress() reads whatever compress() produced, converted or not.""" + data = np.arange(8, dtype=np.uint16).reshape(2, 4) + + result = uncompress(bytes(compress(data))) + + assert np.array_equal(result, data) + + +def test_larger_matrix_roundtrips(): + """A matrix big enough to actually exercise zlib, with non-trivial content.""" + rng = np.random.default_rng(42) + data = rng.integers(0, 255, size=(120, 160), dtype=np.uint8) + + assert np.array_equal(uncompress(compress(data)), data) + + +def test_rejects_more_than_two_dimensions(): + """The format has no encoding for a third dimension, so compress() refuses one.""" + with pytest.raises(AssertionError): + compress(np.zeros((2, 2, 2), dtype=np.uint8)) + + +@pytest.mark.parametrize('dtype', ['int64', 'uint64', 'float16']) +def test_rejects_unsupported_dtype(dtype): + """Depths outside CV_8U..CV_64F have no type code and raise rather than truncate.""" + with pytest.raises(KeyError): + compress(np.zeros(4, dtype=dtype)) + + +def test_rejects_unknown_type_code(): + """A trailer from a multi-channel C++ matrix encodes a code this table lacks.""" + payload = bytes(compress(np.zeros((2, 2), dtype=np.uint8))[:-TRAILER_SIZE]) + # What serializeMatType() returns for a 3-channel CV_8U matrix: depth + ((cn - 1) << 3). + forged = bytearray(payload) + struct.pack('iii', 2, 2, 0 + ((3 - 1) << 3)) + + with pytest.raises(KeyError): + uncompress(forged)