From 7815f42936733973a28bae893f7cb7839fdf02d1 Mon Sep 17 00:00:00 2001 From: Tom Slankard Date: Wed, 3 Jul 2024 16:08:07 -0700 Subject: [PATCH] Release 0.12.0 (#600) See CHANGELOG.rst for details! --- CHANGELOG.rst | 82 ++ CMakeLists.txt | 2 +- cmake/FindCURL.cmake | 2 +- cmake/Findglfw3.cmake | 2 +- conanfile.py | 92 +- docs/Doxyfile | 4 +- docs/cli/clip-sessions.rst | 74 ++ docs/cli/getting-started.rst | 37 +- docs/cli/mapping-sessions.rst | 29 +- docs/cli/sample-sessions.rst | 12 +- docs/cpp/building.rst | 4 +- docs/cpp/ouster_client/types.rst | 2 - docs/index.rst | 2 +- docs/installation.rst | 4 +- docs/overview.rst | 22 +- docs/python/api/client.rst | 12 +- docs/python/api/pcap.rst | 8 +- docs/python/api/viz.rst | 8 +- docs/python/devel.rst | 22 +- docs/python/quickstart.rst | 239 ++++- docs/python/slam-api-example.rst | 30 +- docs/python/using-scan-source.rst | 197 ----- docs/python/viz/index.rst | 2 +- docs/python/viz/viz-run.rst | 73 +- docs/python/viz/viz-scans-accum.rst | 35 +- docs/versions.json | 7 + examples/async_client_example.cpp | 18 +- examples/client_example.cpp | 15 +- examples/config_example.cpp | 4 +- examples/helpers.cpp | 8 +- examples/lidar_scan_example.cpp | 21 +- examples/mtp_client_example.cpp | 4 +- examples/representations_example.cpp | 3 +- ouster_client/CMakeLists.txt | 9 +- ouster_client/include/ouster/array_view.h | 490 +++++++++++ ouster_client/include/ouster/client.h | 8 +- ouster_client/include/ouster/field.h | 747 ++++++++++++++++ .../include/ouster/impl/cuda_macros.h | 13 + ouster_client/include/ouster/impl/idx_range.h | 155 ++++ .../include/ouster/impl/lidar_scan_impl.h | 352 ++++---- .../{src => include/ouster/impl}/logging.h | 7 +- .../include/ouster/impl/packet_writer.h | 6 +- .../include/ouster/impl/profile_extension.h | 2 +- ouster_client/include/ouster/lidar_scan.h | 231 +++-- ouster_client/include/ouster/sensor_http.h | 110 ++- ouster_client/include/ouster/strings.h | 94 ++ ouster_client/include/ouster/types.h | 314 ++++--- ouster_client/include/ouster/version.h | 39 +- ouster_client/src/client.cpp | 50 +- ouster_client/src/curl_client.h | 91 +- ouster_client/src/field.cpp | 236 +++++ ouster_client/src/http_client.h | 40 +- ouster_client/src/lidar_scan.cpp | 569 ++++++++---- ouster_client/src/logging.cpp | 38 +- ouster_client/src/parsing.cpp | 376 ++++++-- ouster_client/src/profile_extension.cpp | 29 +- ouster_client/src/sensor_http.cpp | 17 +- ouster_client/src/sensor_http_imp.cpp | 181 ++-- ouster_client/src/sensor_http_imp.h | 117 ++- ouster_client/src/sensor_info.cpp | 744 +++++----------- ouster_client/src/sensor_tcp_imp.cpp | 66 +- ouster_client/src/sensor_tcp_imp.h | 91 +- ouster_client/src/types.cpp | 238 +++-- ouster_client/src/udp_packet_source.cpp | 2 +- ouster_osf/CMakeLists.txt | 1 - ouster_osf/fb/os_sensor/common.fbs | 34 + ouster_osf/fb/os_sensor/lidar_scan_stream.fbs | 18 +- ouster_osf/include/ouster/osf/basics.h | 32 +- ouster_osf/include/ouster/osf/metadata.h | 12 +- ouster_osf/include/ouster/osf/pcap_source.h | 144 --- .../include/ouster/osf/stream_lidar_scan.h | 20 +- ouster_osf/include/ouster/osf/writer.h | 41 +- ouster_osf/src/basics.cpp | 59 +- ouster_osf/src/compat_ops.cpp | 38 +- ouster_osf/src/fb_utils.cpp | 7 +- ouster_osf/src/file.cpp | 5 +- ouster_osf/src/meta_lidar_sensor.cpp | 5 +- ouster_osf/src/metadata.cpp | 20 +- ouster_osf/src/operations.cpp | 9 +- ouster_osf/src/pcap_source.cpp | 79 -- ouster_osf/src/png_tools.cpp | 314 ++++--- ouster_osf/src/png_tools.h | 41 +- ouster_osf/src/reader.cpp | 11 +- ouster_osf/src/stream_lidar_scan.cpp | 340 +++++-- ouster_osf/src/writer.cpp | 102 ++- ouster_osf/tests/CMakeLists.txt | 1 - ouster_osf/tests/common.h | 86 +- ouster_osf/tests/operations_test.cpp | 17 +- ouster_osf/tests/pcap_source_test.cpp | 45 - ouster_osf/tests/png_tools_test.cpp | 71 +- ouster_osf/tests/reader_test.cpp | 20 +- ouster_osf/tests/writer_test.cpp | 109 ++- ouster_osf/tests/writerv2_test.cpp | 14 +- ouster_pcap/CMakeLists.txt | 2 +- ouster_pcap/src/indexed_pcap_reader.cpp | 2 +- ouster_pcap/src/ip_reassembler.cpp | 190 ++++ ouster_pcap/src/ip_reassembler.h | 186 ++++ ouster_pcap/src/pcap.cpp | 10 +- python/.flake8 | 20 + python/Dockerfile | 33 +- python/README.rst | 2 +- python/docker_install_reqs.sh | 22 + python/pyproject.toml | 2 +- python/setup.py | 161 ++-- python/src/cpp/_client.cpp | 832 ++++++++++++------ python/src/cpp/_osf.cpp | 107 +-- python/src/cpp/_viz.cpp | 94 +- python/src/ouster/cli/core/__init__.py | 9 +- python/src/ouster/cli/core/util.py | 26 +- python/src/ouster/cli/plugins/source.py | 215 ++++- .../src/ouster/cli/plugins/source_mapping.py | 430 +++++++++ python/src/ouster/cli/plugins/source_osf.py | 16 +- python/src/ouster/cli/plugins/source_save.py | 21 +- .../src/ouster/cli/plugins/source_sensor.py | 27 +- python/src/ouster/cli/plugins/source_util.py | 2 +- python/src/ouster/client/__init__.py | 5 - python/src/ouster/osf/__init__.py | 5 - python/src/ouster/pcap/__init__.py | 5 - python/src/ouster/sdk/__init__.py | 4 +- python/src/ouster/sdk/client/__init__.py | 22 +- python/src/ouster/sdk/client/_client.pyi | 271 +++--- python/src/ouster/sdk/client/_digest.py | 42 +- python/src/ouster/sdk/client/core.py | 100 ++- python/src/ouster/sdk/client/data.py | 328 +------ python/src/ouster/sdk/client/multi.py | 37 +- .../ouster/sdk/client/multi_scan_source.py | 33 +- .../sdk/client/multi_sliced_scan_source.py | 111 +++ python/src/ouster/sdk/client/scan_source.py | 29 +- .../ouster/sdk/client/scan_source_adapter.py | 55 +- python/src/ouster/sdk/convert_to_legacy.py | 31 - python/src/ouster/sdk/examples/client.py | 6 +- python/src/ouster/sdk/examples/open3d.py | 4 +- python/src/ouster/sdk/examples/osf.py | 9 +- python/src/ouster/sdk/examples/pcap.py | 19 +- python/src/ouster/sdk/mapping/__init__.py | 45 + .../sdk/mapping/internal/calc_loop_drift.py | 122 +++ .../sdk/mapping/internal/pcaps_to_osf.py | 125 +++ .../src/ouster/sdk/mapping/internal/stitch.py | 341 +++++++ .../ouster/sdk/mapping/internal/traj_eval.py | 368 ++++++++ python/src/ouster/sdk/mapping/kiss_backend.py | 135 +++ .../src/ouster/sdk/mapping/ouster_kiss_icp.py | 172 ++++ python/src/ouster/sdk/mapping/ply_to_png.py | 208 +++++ python/src/ouster/sdk/mapping/py.typed | 0 .../src/ouster/sdk/mapping/requirements.json | 17 + python/src/ouster/sdk/mapping/slam.py | 2 + python/src/ouster/sdk/mapping/slam_backend.py | 21 + python/src/ouster/sdk/mapping/util.py | 87 ++ python/src/ouster/sdk/open_source.py | 3 +- python/src/ouster/sdk/osf/_osf.pyi | 8 +- python/src/ouster/sdk/osf/data.py | 24 +- python/src/ouster/sdk/osf/osf_scan_source.py | 90 +- python/src/ouster/sdk/pcap/packet_iter.py | 42 +- python/src/ouster/sdk/pcap/pcap.py | 147 +--- .../sdk/pcap/pcap_multi_packet_reader.py | 122 ++- .../src/ouster/sdk/pcap/pcap_scan_source.py | 60 +- .../sdk/sensor/sensor_multi_packet_reader.py | 30 +- python/src/ouster/sdk/util/__init__.py | 3 - python/src/ouster/sdk/util/forward_slicer.py | 58 +- python/src/ouster/sdk/util/metadata.py | 32 +- python/src/ouster/sdk/util/parsing.py | 547 +----------- python/src/ouster/sdk/viz/__init__.py | 3 +- python/src/ouster/sdk/viz/core.py | 123 +-- python/src/ouster/sdk/viz/multi_viz.py | 31 +- python/src/ouster/sdk/viz/scans_accum.py | 53 +- python/src/ouster/sdk/viz/view_mode.py | 79 +- python/src/ouster/viz/__init__.py | 5 - python/tests/conftest.py | 10 +- python/tests/osf/test_osf_basics.py | 20 +- python/tests/test_batching.py | 69 +- python/tests/test_cli.py | 45 +- python/tests/test_cli_util.py | 25 - python/tests/test_core.py | 141 ++- python/tests/test_data.py | 558 ++++++------ python/tests/test_extended_profiles.py | 22 +- python/tests/test_forward_slicer.py | 20 +- python/tests/test_metadata.py | 99 ++- python/tests/test_packet_iter.py | 2 +- python/tests/test_parsing.py | 10 +- python/tests/test_pcap.py | 149 ++-- python/tests/test_plugins.py | 2 +- python/tests/test_scan_source_slice.py | 205 +++++ python/tests/test_sdk_utils.py | 43 +- python/tox.ini | 82 -- test_package/CMakeLists.txt | 9 + test_package/conanfile.py | 26 + test_package/src/example.cpp | 4 + tests/CMakeLists.txt | 11 + tests/array_view_test.cpp | 458 ++++++++++ tests/bcompat_meta_json_test.cpp | 108 ++- tests/field_test.cpp | 647 ++++++++++++++ tests/fusa_profile_test.cpp | 22 +- tests/lidar_scan_test.cpp | 122 ++- tests/metadata_test.cpp | 261 ++++-- tests/packet_writer_test.cpp | 42 +- tests/parsing_benchmark_test.cpp | 21 +- tests/pcaps/OS-0-128_v3.0.1_1024x10.2.json | 529 +++++++++++ tests/pcaps/OS-0-128_v3.0.1_1024x10.json | 529 +++++++++++ tests/pcaps/OS-0-128_v3.0.1_1024x10.pcap | 0 ...-128_v3.0.1_1024x10_20240321_125947.2.json | 529 +++++++++++ ...-0-128_v3.0.1_1024x10_20240321_125947.json | 530 +++++++++++ ...-0-128_v3.0.1_1024x10_20240321_125947.pcap | 0 tests/pcaps/duplicate_id.json | 506 +++++++++++ tests/pcaps/duplicate_id.pcap | Bin 0 -> 10310 bytes tests/profile_extension_test.cpp | 38 +- tests/scan_batcher_test.cpp | 120 ++- tests/udp_queue_test.cpp | 23 +- 206 files changed, 15141 insertions(+), 5757 deletions(-) create mode 100644 docs/cli/clip-sessions.rst delete mode 100644 docs/python/using-scan-source.rst create mode 100644 ouster_client/include/ouster/array_view.h create mode 100644 ouster_client/include/ouster/field.h create mode 100644 ouster_client/include/ouster/impl/cuda_macros.h create mode 100644 ouster_client/include/ouster/impl/idx_range.h rename ouster_client/{src => include/ouster/impl}/logging.h (82%) create mode 100644 ouster_client/include/ouster/strings.h create mode 100644 ouster_client/src/field.cpp create mode 100644 ouster_osf/fb/os_sensor/common.fbs delete mode 100644 ouster_osf/include/ouster/osf/pcap_source.h delete mode 100644 ouster_osf/src/pcap_source.cpp delete mode 100644 ouster_osf/tests/pcap_source_test.cpp create mode 100644 ouster_pcap/src/ip_reassembler.cpp create mode 100644 ouster_pcap/src/ip_reassembler.h create mode 100644 python/.flake8 create mode 100755 python/docker_install_reqs.sh create mode 100644 python/src/ouster/cli/plugins/source_mapping.py delete mode 100644 python/src/ouster/client/__init__.py delete mode 100644 python/src/ouster/osf/__init__.py delete mode 100644 python/src/ouster/pcap/__init__.py create mode 100644 python/src/ouster/sdk/client/multi_sliced_scan_source.py delete mode 100644 python/src/ouster/sdk/convert_to_legacy.py create mode 100644 python/src/ouster/sdk/mapping/__init__.py create mode 100644 python/src/ouster/sdk/mapping/internal/calc_loop_drift.py create mode 100644 python/src/ouster/sdk/mapping/internal/pcaps_to_osf.py create mode 100644 python/src/ouster/sdk/mapping/internal/stitch.py create mode 100644 python/src/ouster/sdk/mapping/internal/traj_eval.py create mode 100644 python/src/ouster/sdk/mapping/kiss_backend.py create mode 100644 python/src/ouster/sdk/mapping/ouster_kiss_icp.py create mode 100644 python/src/ouster/sdk/mapping/ply_to_png.py create mode 100644 python/src/ouster/sdk/mapping/py.typed create mode 100644 python/src/ouster/sdk/mapping/requirements.json create mode 100644 python/src/ouster/sdk/mapping/slam.py create mode 100644 python/src/ouster/sdk/mapping/slam_backend.py create mode 100644 python/src/ouster/sdk/mapping/util.py delete mode 100644 python/src/ouster/viz/__init__.py create mode 100644 python/tests/test_scan_source_slice.py delete mode 100644 python/tox.ini create mode 100644 test_package/CMakeLists.txt create mode 100644 test_package/conanfile.py create mode 100644 test_package/src/example.cpp create mode 100644 tests/array_view_test.cpp create mode 100644 tests/field_test.cpp create mode 100644 tests/pcaps/OS-0-128_v3.0.1_1024x10.2.json create mode 100644 tests/pcaps/OS-0-128_v3.0.1_1024x10.json create mode 100644 tests/pcaps/OS-0-128_v3.0.1_1024x10.pcap create mode 100644 tests/pcaps/OS-0-128_v3.0.1_1024x10_20240321_125947.2.json create mode 100644 tests/pcaps/OS-0-128_v3.0.1_1024x10_20240321_125947.json create mode 100644 tests/pcaps/OS-0-128_v3.0.1_1024x10_20240321_125947.pcap create mode 100644 tests/pcaps/duplicate_id.json create mode 100644 tests/pcaps/duplicate_id.pcap diff --git a/CHANGELOG.rst b/CHANGELOG.rst index f1df3424..b4007adf 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,6 +2,84 @@ Changelog ========= +[20240702] [0.12.0] +=================== + +**Important: ouster-sdk installed from pypi now requires glibc >= 2.28.** + +ouster_client/Python SDK +------------------------ + +* Add support for adding custom fields to ``LidarScan`` s with ``add_field`` and ``del_field`` +* Added per-request timeout arguments to ``SensorHttp`` +* Added sensor user_data to ``sensor_info/SensorInfo`` and metadata files +* Removed ``updated_metadata_string()`` and ``original_string()`` from ``sensor_info`` +* Added ``to_json_string()`` to ``sensor_info`` to convert a ``sensor_info`` to a non-legacy + metadata JSON string +* Unified Python and C++ ``Packet`` and ``PacketFormat`` classes +* Added ``validate`` function to ``LidarPacket`` and ``ImuPacket`` to check for ID and size mismatches +* [BREAKING] LidarScan's width and height have been switched to size_t from ptrdiff_t in C++ +* Refactor metadata parsing +* Add ``get_version`` to ``sensor_info/SensorInfo`` to retrieve parsed version information +* Add ``get_product_info`` to ``sensor_info/SensorInfo`` to retrieve parsed lidar model information +* Raise an exception rather than throw an unrelated error when multiple viable metadata files are found for a given PCAP +* Add ability to slice a scan source, returning a new sliced ScanSource + +* [BREAKING] Removed ``hostname`` in Python ``SensorInfo`` and ``name`` from C++ ``sensor_info`` +* [BREAKING] Removed ``udp_port_lidar``, ``udp_port_imu`` and ``mode`` from C++ ``sensor_info`` +* [BREAKING] Deprecated ``udp_port_lidar``, ``udp_port_imu`` and ``mode`` in Python ``SensorInfo``. + These fields now point to the equivalent fields inside of ``SensorInfo::config``. +* [BREAKING] Removed ``cols`` and ``frequency`` from ``LidarMode`` in Python +* [BREAKING] Deprecated ```data``` and ``capture_timestamp`` from Python ``Packet`` +* [BREAKING] Removed methods from Python ``ImuPacket`` and ``LidarPacket`` classes that simply wrapped ``PacketFormat`` +* [BREAKING] Removed ``begin()`` and ``end()`` iterators of ``LidarScan`` in C++ +* [BREAKING] Remove deprecated package stubs added in previous 0.11 release. +* [BREAKING] Replaced integer backed ``ChanField`` enumerations with strings. +* [BREAKING] Removed ``CUSTOM0`` through ``CUSTOM9`` ChanField enumerations. +* [BREAKING] Extra fields in sensor metadata are now ignored and discarded if saved from the resulting ``sensor_info/SensorInfo`` + +* [BUGFIX] Prevent last scan from being emitted twice for PCAP +* [BUGFIX] Fix corrupted packets due to poor handling of fragmented packet drop in PCAPs +* [BUGFIX] Fix possible crash when working with custom UDPProfileLidars + +ouster_viz +---------- +* Support viewing custom ``LidarScan`` fields in viz +* Support viewing custom ``LidarScan`` 3 channel fields in viz as RGB + +* [BUGFIX] Prevent OpenBLAS from using high amounts of CPU spin waiting + +ouster_osf +---------- + +* Support saving custom ``LidarScan`` fields to OSF files + +* [BREAKING] OsfWriter now takes in an optional list of fields to save rather than a list of fields and ChanFieldTypes to cast to + +ouster-cli +---------- + +* Added support for slicing using time to ``ouster-cli source ... slice`` +* Add sensor ``ouster-cli source ... userdata`` command to set and retrieve userdata on a sensor +* Add chainable ``ouster-cli source ... stats`` command +* Add chainable ``ouster-cli source ... clip`` command to discard points outside a provided range +* Add ``--rate max`` option to ``ouster-cli source ... viz``` +* Improve argument naming and descriptions for ``ouster-cli source ... viz`` map and accum options + +* [BUGFIX] Prevent dropped frames from live sensors by consuming scans as fast as they come in rather than sleeping + +mapping +------- + +* Move mapping into the sdk as ``ouster.sdk.mapping`` +* Better handle looping while mapping +* Improve automatic downsample voxel size calculation + +other +----- + +* Updated VCPKG libraries to 2024.04.26 + [20240510] [0.11.1] =================== @@ -15,6 +93,10 @@ Python SDK * Updated the ``open_source`` documentation. * Fixed an issue that caused the viz to redraw when the mouse cursor is moved. +* [BREAKING] The python slice ``[::]`` operator now returns a ``MultiScanSource`` / ``ScanSource`` + instead of a ``List``. Thus, invoking a ``scan_source[x:x+n]`` yields a fully functional scan source + that is scoped to the range ``[x, x+n]``. +* [BREAKING] The python slice ``[::]`` operator no longer support negative step ouster_client ------------- diff --git a/CMakeLists.txt b/CMakeLists.txt index 5ebbdd0b..99945315 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -16,7 +16,7 @@ include(VcpkgEnv) project(ouster_example VERSION 20231031) # generate version header -set(OusterSDK_VERSION_STRING 0.11.1) +set(OusterSDK_VERSION_STRING 0.12.0) include(VersionGen) # ==== Options ==== diff --git a/cmake/FindCURL.cmake b/cmake/FindCURL.cmake index 826c27c5..0c3d0591 100644 --- a/cmake/FindCURL.cmake +++ b/cmake/FindCURL.cmake @@ -1,4 +1,4 @@ -# Define forwards-compatible imported target for old platforms (Ubuntu 18.04) +# Define forwards-compatible imported target for old platforms # Note: curl from VCPKG appears to completely ignore curl find modules despite # CMAKE_MODULE_PATH settings diff --git a/cmake/Findglfw3.cmake b/cmake/Findglfw3.cmake index da49773b..cb473cff 100644 --- a/cmake/Findglfw3.cmake +++ b/cmake/Findglfw3.cmake @@ -5,7 +5,7 @@ include(FindPackageHandleStandardArgs) find_package(glfw3 CONFIG REQUIRED) -# Package on >=18.04 sets a target +# Package on >=20.04 sets a target if(TARGET glfw) get_target_property(GLFW3_LIBRARIES glfw LOCATION) get_target_property(GLFW3_INCLUDE_DIRS glfw INTERFACE_INCLUDE_DIRECTORIES) diff --git a/conanfile.py b/conanfile.py index 2db6364e..c2e53082 100644 --- a/conanfile.py +++ b/conanfile.py @@ -1,12 +1,14 @@ import os import re -from conans import ConanFile, CMake, tools +from conan import ConanFile +from conan.tools.cmake import CMakeToolchain, CMake, cmake_layout, CMakeDeps +from conan.tools.files import collect_libs, load +from conan.tools.scm import Git -from pprint import pformat - -class OusterSDKConan(ConanFile): +class ousterSdkRecipe(ConanFile): name = "ouster_sdk" + package_type = "library" license = "BSD 3-Clause License" author = "Ouster, Inc." url = "https://github.com/ouster-lidar/ouster_example" @@ -33,7 +35,6 @@ class OusterSDKConan(ConanFile): "eigen_max_align_bytes": False, } - generators = "cmake_paths", "cmake_find_package" exports_sources = [ "cmake/*", "conan/*", @@ -49,27 +50,34 @@ class OusterSDKConan(ConanFile): "README.rst" ] - # https://docs.conan.io/en/1.51/howtos/capture_version.html#how-to-capture-package-version-from-text-or-build-files + # https://docs.conan.io/2/reference/conanfile/methods/set_version.html def set_version(self): - content = tools.load(os.path.join(self.recipe_folder, "CMakeLists.txt")) - version = re.search(r"set\(OusterSDK_VERSION_STRING (.*)\)", content).group(1) + content = load(self, os.path.join(self.recipe_folder, "CMakeLists.txt")) + version = re.search("set\(OusterSDK_VERSION_STRING (.*)\)", content).group(1) self.version = version.strip() def config_options(self): if self.settings.os == "Windows": - del self.options.fPIC + self.options.rm_safe("fPIC") + + def configure(self): + if self.options.shared: + self.options.rm_safe("fPIC") def requirements(self): - # not required directly here but because boost and openssl pulling - # slightly different versions of zlib we need to set it - # here explicitly + # not required directly here but because boost and openssl pulling + # slightly different versions of zlib we need to set it + # here explicitly self.requires("zlib/1.3") - self.requires("eigen/3.4.0") + # Since Eigen is a header only library, and the SDK includes Eigen + # headers in its headers, we must set transitive_headers=True so that + # packages consuming the SDK will also have access to the Eigen headers. + self.requires("eigen/3.4.0", transitive_headers=True) self.requires("jsoncpp/1.9.5") - self.requires("spdlog/1.11.0") - self.requires("fmt/9.1.0") - self.requires("libcurl/7.84.0") + self.requires("spdlog/1.12.0") + self.requires("fmt/9.1.0", override=True) + self.requires("libcurl/7.86.0") if self.options.build_pcap: self.requires("libtins/4.3") @@ -86,41 +94,45 @@ def requirements(self): # maybe needed for cpp examples, but not for the lib # self.requires("tclap/1.2.4") - def configure_cmake(self): - cmake = CMake(self) - cmake.definitions["BUILD_VIZ"] = self.options.build_viz - cmake.definitions["BUILD_PCAP"] = self.options.build_pcap - cmake.definitions["BUILD_OSF"] = self.options.build_osf - cmake.definitions["OUSTER_USE_EIGEN_MAX_ALIGN_BYTES_32"] = self.options.eigen_max_align_bytes - # alt way, but we use CMAKE_TOOLCHAIN_FILE in other pipeline so avoid overwrite - # cmake.definitions["CMAKE_TOOLCHAIN_FILE"] = os.path.join(self.build_folder, "conan_paths.cmake") - cmake.definitions[ - "CMAKE_PROJECT_ouster_example_INCLUDE"] = os.path.join( - self.build_folder, "conan_paths.cmake") - cmake.definitions["BUILD_SHARED_LIBS"] = True if self.options.shared else False - cmake.definitions["CMAKE_POSITION_INDEPENDENT_CODE"] = ( + def build_requirements(self): + if self.options.build_osf: + self.build_requires("flatbuffers/") + + def layout(self): + cmake_layout(self) + + def generate(self): + deps = CMakeDeps(self) + deps.generate() + tc = CMakeToolchain(self) + tc.variables["BUILD_VIZ"] = self.options.build_viz + tc.variables["BUILD_PCAP"] = self.options.build_pcap + tc.variables["BUILD_OSF"] = self.options.build_osf + tc.variables[ + "OUSTER_USE_EIGEN_MAX_ALIGN_BYTES_32" + ] = self.options.eigen_max_align_bytes + tc.variables["BUILD_SHARED_LIBS"] = True if self.options.shared else False + tc.variables["CMAKE_POSITION_INDEPENDENT_CODE"] = ( True if "fPIC" in self.options and self.options.fPIC else False ) # we use this option until we remove nonstd::optional from SDK codebase (soon) if self.options.ensure_cpp17: - cmake.definitions["CMAKE_CXX_STANDARD"] = 17 + tc.variables["CMAKE_CXX_STANDARD"] = 17 - self.output.info("Cmake definitions: ") - self.output.info(pformat(cmake.definitions)) - cmake.configure() - return cmake + tc.generate() def build(self): - cmake = self.configure_cmake() + cmake = CMake(self) + cmake.configure() cmake.build() def package(self): - cmake = self.configure_cmake() + cmake = CMake(self) cmake.install() def package_info(self): - self.cpp_info.libs = tools.collect_libs(self) + self.cpp_info.libs = collect_libs(self) self.cpp_info.includedirs = [ "include", "include/optional-lite" @@ -129,8 +141,14 @@ def package_info(self): "lib/cmake/OusterSDK/OusterSDKConfig.cmake" ) + self.cpp_info.set_property( + "cmake_build_modules", + [os.path.join("lib", "cmake", "OusterSDK", "OusterSDKConfig.cmake")], + ) self.cpp_info.set_property("cmake_file_name", "OusterSDK") + self.cpp_info.set_property("cmake_target_name", "OusterSDK") + # TODO: to remove in conan v2 once cmake_find_package* generators removed self.cpp_info.filenames["cmake_find_package"] = "OusterSDK" self.cpp_info.filenames["cmake_find_package_multi"] = "OusterSDK" self.cpp_info.names["cmake_find_package"] = "OusterSDK" diff --git a/docs/Doxyfile b/docs/Doxyfile index d279d7c6..beebfba9 100644 --- a/docs/Doxyfile +++ b/docs/Doxyfile @@ -934,7 +934,9 @@ RECURSIVE = YES # Note that relative paths are relative to the directory from which doxygen is # run. -EXCLUDE = ../ouster_client/include/optional-lite +EXCLUDE = ../ouster_client/include/optional-lite \ + ../ouster_pcap/src/ip_reassembler.h \ + ../ouster_pcap/src/ip_reassembler.cpp # The EXCLUDE_SYMLINKS tag can be used to select whether or not files or # directories that are symbolic links (a Unix file system feature) are excluded diff --git a/docs/cli/clip-sessions.rst b/docs/cli/clip-sessions.rst new file mode 100644 index 00000000..7a4f4b31 --- /dev/null +++ b/docs/cli/clip-sessions.rst @@ -0,0 +1,74 @@ +Clip PointCloud in the Ouster-CLI +================================= + +.. _ouster-cli-clip: + + +Clip Command +------------ + +The clip command clips the lidar scan by the given range and streams down the modified lidar +scan to the subsequent commands (like slam, viz, save, etc.). The position of the clip command +in the ouster-cli command chain makes a difference as it only affects the commands that follow it. + +To explore the parameters you can use with the clip command, you can use the --help flag: + +.. code:: bash + + ouster-cli source / clip --help + + +Example Usage +------------- + +To keep the points within the 20 m to 50 m range and save the modified lidar scan into a PCAP file +with the visualizer on, run the following command: + +.. code:: bash + + ouster-cli source / clip --min-range 20 --max-range 50 viz save clipped.pcap + + +Remember, the clip command only affects the commands after it. In the following example, the +viz command runs before the clip command, which means the point cloud modification won't be reflected +in the visualizer but will affect the subsequent save command and the saved PCAP file: + +.. code:: bash + + ouster-cli source / viz clip --min-range 20 --max-range 50 save clipped_2.pcap + +In addition to the ``min-range`` and ``max-range`` parameters, the ``clip`` command also includes the +``percent-range`` parameter. This parameter discards points with ranges greater than a specified percentile +in each lidar scan, helping to filter out noise points. + +.. code:: bash + + ouster-cli source / clip --percent-range 99 viz + +Combined with SLAM Command +-------------------------- + +The ``slam`` command also has ``min-range`` and ``max-range`` parameters. When the clip command is +used after the ``slam`` command, the ``clip`` command will, by default, use the range settings specified +in the slam command. However, you can explicitly pass in the range settings to the ``clip`` command to +apply different ranges to the clip operation. + +Note that the range settings in the ``slam`` command only affect the point cloud within the SLAM algorithm. +The slam range settings will not modify the lidarscan and will not affect the other following commands. + + +Example Usage +------------- + +Experiment with the following commands using a pre-recorded PCAP or OSF file: + +.. code:: bash + + ouster-cli source slam clip --min-range 20 --max-range 50 viz save clipped_3.ply + ouster-cli source slam --min-range 10 --max-range 100 clip --min-range 20 --max-range 50 viz save clipped_4.ply + +You can view the output PLY files using the open source software `CloudCompare`_ +For more details about the slam command, refer to the :ref:`SLAM Command ` + + +.. _CloudCompare: https://www.cloudcompare.org/ diff --git a/docs/cli/getting-started.rst b/docs/cli/getting-started.rst index db361cf8..8a2bc49d 100644 --- a/docs/cli/getting-started.rst +++ b/docs/cli/getting-started.rst @@ -82,7 +82,7 @@ auto-configuration, respectively. visualized But ``ouster-cli source viz``, or ``ouster-cli source - record`` still can't receive any packets and get the following error:: + save`` still can't receive any packets and get the following error:: ouster.client.core.ClientTimeout: No packets received within 1.0s @@ -96,10 +96,39 @@ auto-configuration, respectively. .. _virtual environment: https://docs.python.org/3/library/venv.html -What next? ----------- +Full Command List +================= + +By default, ``ouster-cli`` includes the following commands. Some of these +commands also have subcommands that further extend or specify what +``ouster-cli`` will do when launched. + + * ``discover`` - uses mDNS to locate Ouster sensors on you local networks. + * ``source`` - Read lidar data from a sensor or file and use it as input to one or more commands. + Most subcommands can be "chained", meaning the output of a subcommand will become the input of the next subcommand. + * Sensors and files: + * ``viz`` - visualizes data in a 3D point cloud viewer. + * ``slam`` - computes trajectories by determining the change in pose between lidar frames. + * ``slice`` - reads a subset of lidar frames from the source using counts or time durations. + * ``clip`` - restrict the minimum or maximum range of lidar measurements in the source data. + * ``stats`` - calculates statistics from the source data. + * ``metadata`` - displays the metadata (e.g. sensor information) associated with the source data. + * ``save`` - saves the source data, optionally converting to a new format. + * Pcap and OSF files only + * ``info`` - prints information about a pcap or OSF file. + * Sensors only + * ``config`` - configures a sensor. + * ``userdata`` - displays the userdata from a sensor. + * OSF files only + * ``dump`` - prints metadata from an OSF file. + * ``parse`` - prints message types from an OSF file. + * ``util`` - Miscellaneous utilities. + * ``benchmark`` - runs a performance benchmark for ouster-sdk. + * ``benchmark-sensor`` - runs a performance benchmark for ouster-sdk using a sensor. + * ``system-info`` - generates system diagnostic information as a JSON string, useful to Ouster support staff when providing customer support. + You can now to use ``ouster-cli`` as you please, exploring available utilities with the handy -``---help``. If you'd prefer something more documented, you can check out our :ref:`sample sessions` +``---help``. If you'd prefer some more detailed examples, you can check out our :ref:`sample sessions` to see what an ``ouster-cli`` workflow might look like, or you can read through :ref:`common commands`. diff --git a/docs/cli/mapping-sessions.rst b/docs/cli/mapping-sessions.rst index bb9ecb81..fc772949 100644 --- a/docs/cli/mapping-sessions.rst +++ b/docs/cli/mapping-sessions.rst @@ -116,14 +116,24 @@ following printout for each output file: .. code:: bash Output file: output-000.ply + Point Cloud status info 3932160 points accumulated during this period, - 154228 near points are removed [3.92 %], - 1475955 down sampling points are removed [37.54 %], - 2213506 zero range points are removed [56.29 %], - 88471 points are saved [2.25 %]. + 1629212 down sampling points are removed [41.43 %], + 2213506 out range points are removed [56.29 %], + 89442 points are saved [2.27 %] -Use the ``--help`` flag for more information such as adjusting the minimal range, selecting -different fields as values, and changing the point cloud downsampling scale etc. + +Use the ``--help`` flag for more information such as selecting different fields as values, +and changing the point cloud downsampling scale etc. + +To filter out the point cloud, you can using the ``clip`` command. Coverting the SLAM output OSF +file to a PLY file and keep only the point within 20 to 80 meteter range you can run: + +.. code:: bash + + ouster-cli source sample.osf clip --min-range 20 --max-range 80 save clipped_output.ply + +More details about the clip command usage can be found in the :ref:`Clip Command ` You can use an open source software `CloudCompare`_ to import and view the generated point cloud data files. @@ -213,12 +223,13 @@ Overall map view (with poses) ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ One of the main tasks we frequently need is a preview of the overall map. We can test this by using -the generated OSF file, which was created with the above command and contains the -SLAM-generated ``LidarScan.pose`` property. +the SLAM-generated OSF file, which was created with the above command and contains the +SLAM trajectory in ``LidarScan.pose``. If you are using a SLAM-generated OSF, you can directly use +viz with scan accumulator feature without appending the ``slam`` option. :: ouster-cli source sample.osf viz --accum-num 20 \ - --accum-every 0 --accum-every-m 10.5 --accum-map -r 0 -e stop + --accum-every 0 --accum-every-m 10.5 --accum-map -r 3 -e stop Here is a preview example of the overall map generated from the accumulated scan results. diff --git a/docs/cli/sample-sessions.rst b/docs/cli/sample-sessions.rst index 401ad7e5..4409c771 100644 --- a/docs/cli/sample-sessions.rst +++ b/docs/cli/sample-sessions.rst @@ -48,9 +48,13 @@ Let's see what the sensor is seeing in a pretty visualizer: That looked nice! Let's record some data to a pcap so we can view it on repeat! +Because many of ``ouster-cli`` commands can be chained together, we'll add the ``slice`` command before the ``save`` command to limit the number of frames we save to 100. + +If the ``slice`` is not present before ``save``, you can stop recording at anytime with ``CTRL-C``. + .. code:: bash - $ ouster-cli source save .pcap + $ ouster-cli source slice 0:100 save .pcap That should produce screen output that looks something like: @@ -61,6 +65,12 @@ That should produce screen output that looks something like: Go ahead and look in the current directory for the named pcap file and associated metadata file. +The ``slice`` command also allows recording for fixed time durations. For example, the following will record 30 seconds of pcap data. + +.. code:: bash + + $ ouster-cli source slice 0s:30s save .pcap + Continue to the next section, `Sample pcap session`_ to see what you can do with your new pcap file. diff --git a/docs/cpp/building.rst b/docs/cpp/building.rst index 6d1b9ab0..807d454e 100644 --- a/docs/cpp/building.rst +++ b/docs/cpp/building.rst @@ -77,10 +77,10 @@ for dependencies. Follow the official documentation to set up your build environ `_ * `Visual Studio CPP Support `_ -* `Vcpkg, at tag "2023.10.19" installed and integrated with Visual Studio +* `Vcpkg, at tag "2024.04.26" installed and integrated with Visual Studio `_ -**Note** You'll need to run ``git checkout 2023.10.19`` in the vcpkg directory before bootstrapping +**Note** You'll need to run ``git checkout 2024.04.26`` in the vcpkg directory before bootstrapping to use the correct versions of the dependencies. Building may fail unexpectedly if you skip this step. diff --git a/docs/cpp/ouster_client/types.rst b/docs/cpp/ouster_client/types.rst index b7d90cf9..6aa9aa38 100644 --- a/docs/cpp/ouster_client/types.rst +++ b/docs/cpp/ouster_client/types.rst @@ -115,8 +115,6 @@ Sensor Info .. doxygenfunction:: ouster::sensor::default_sensor_info -.. doxygenfunction:: ouster::sensor::parse_metadata - .. doxygenfunction:: ouster::sensor::metadata_from_json .. doxygenfunction:: ouster::sensor::convert_to_legacy diff --git a/docs/index.rst b/docs/index.rst index a11d27a6..476bca60 100644 --- a/docs/index.rst +++ b/docs/index.rst @@ -12,7 +12,6 @@ :hidden: Developer Quick Start - Using ScanSource API Examples SLAM Quickstart Point Cloud Visualizer @@ -49,6 +48,7 @@ Getting Started Sample Sessions Common Use Cases + Clip Sessions Mapping Sessions .. toctree:: diff --git a/docs/installation.rst b/docs/installation.rst index 457f1150..3573db74 100644 --- a/docs/installation.rst +++ b/docs/installation.rst @@ -29,9 +29,7 @@ Supported Platforms Installation -------------- -The Ouster Python SDK binary packages require Python >= 3.8 and pip >= 19.0 on most platforms. On -Ubuntu 18.04, the default Python 3 version is is 3.6, so you'll have to install and use -``python3.8`` explicitly. On Apple M1, you'll need need Python >= 3.8. +The Ouster Python SDK binary packages require Python >= 3.8 and pip >= 19.0 on most platforms. .. note:: diff --git a/docs/overview.rst b/docs/overview.rst index f99df151..24160832 100644 --- a/docs/overview.rst +++ b/docs/overview.rst @@ -21,8 +21,21 @@ Quick links Ouster SDK CLI visualization of OS1 128 Rev 7 sample data -Compatibility with FW ---------------------- +Supported Devices +----------------- + +The sdk supports the following Ouster sensors: + +* OS0_ +* OS1_ +* OS2_ +* OSDome_ + +You can obtain detailed specs sheets for the sensors and obtain updated +firmware through the website downloads_ section. + +Compatibility with Firmware (FW) +-------------------------------- The Ouster SDK currently provides backwards compatibility with any FW 2.0 and later for all releases. Older SDK versions are not, however, generally forward-compatible with later FW releases, @@ -82,3 +95,8 @@ announcements`_ .. _Changelog: https://github.com/ouster-lidar/ouster_example/blob/master/CHANGELOG.rst .. _Ouster ROS 1 driver: https://github.com/ouster-lidar/ouster-ros .. _Lifecycle Policies: https://github.com/ouster-lidar/ouster_example/discussions/532 +.. _OS0: https://ouster.com/products/hardware/os0-lidar-sensor +.. _OS1: https://ouster.com/products/hardware/os1-lidar-sensor +.. _OS2: https://ouster.com/products/hardware/os2-lidar-sensor +.. _OSDome: https://ouster.com/products/hardware/osdome-lidar-sensor +.. _downloads: https://ouster.com/downloads diff --git a/docs/python/api/client.rst b/docs/python/api/client.rst index e6be1d2f..ef25f315 100644 --- a/docs/python/api/client.rst +++ b/docs/python/api/client.rst @@ -1,13 +1,13 @@ -=========================== -Module :mod:`ouster.client` -=========================== +=============================== +Module :mod:`ouster.sdk.client` +=============================== .. contents:: :local: :depth: 4 -.. automodule:: ouster.client +.. automodule:: ouster.sdk.client ---- @@ -91,9 +91,9 @@ Metadata Data ===== -.. autodata:: ouster.client.data.BufferT +.. autodata:: ouster.sdk.client.data.BufferT -.. autodata:: ouster.client.data.Packet +.. autodata:: ouster.sdk.client.data.Packet .. autoclass:: ImuPacket :members: diff --git a/docs/python/api/pcap.rst b/docs/python/api/pcap.rst index 9e3fe968..e8a07f5d 100644 --- a/docs/python/api/pcap.rst +++ b/docs/python/api/pcap.rst @@ -1,8 +1,8 @@ -========================= -Module :mod:`ouster.pcap` -========================= +============================= +Module :mod:`ouster.sdk.pcap` +============================= -.. automodule:: ouster.pcap +.. automodule:: ouster.sdk.pcap .. autofunction:: record diff --git a/docs/python/api/viz.rst b/docs/python/api/viz.rst index 65ef5f46..735d55a3 100644 --- a/docs/python/api/viz.rst +++ b/docs/python/api/viz.rst @@ -1,12 +1,12 @@ ============================ -Module :mod:`ouster.viz` +Module :mod:`ouster.sdk.viz` ============================ .. contents:: :local: :depth: 4 -.. py:currentmodule:: ouster.viz +.. py:currentmodule:: ouster.sdk.viz Ouster sensor data visualization tools. Implemented in C++ OpenGL and wrapped with Python bindings. @@ -31,10 +31,10 @@ Core .. autoclass:: ScansAccumulator :members: -.. autoattribute:: ouster.viz.spezia_palette +.. autoattribute:: ouster.sdk.viz.spezia_palette :annotation: = spezia colors -.. autoattribute:: ouster.viz.calref_palette +.. autoattribute:: ouster.sdk.viz.calref_palette :annotation: = calref colors diff --git a/docs/python/devel.rst b/docs/python/devel.rst index a431cbbf..d498eeeb 100644 --- a/docs/python/devel.rst +++ b/docs/python/devel.rst @@ -82,7 +82,7 @@ package manager and run: PS > vcpkg install --triplet=x64-windows curl eigen3 jsoncpp libtins glfw3 glad[gl-api-33] spdlog libpng flatbuffers -The currently tested vcpkg tag is ``2023.10.19``. After that, using a developer powershell prompt: +The currently tested vcpkg tag is ``2024.04.26``. After that, using a developer powershell prompt: .. code:: powershell @@ -167,23 +167,3 @@ To run tests against multiple Python versions simultaneously, use the ``tox`` pa This will take longer, since it will build the package from a source distribution for each supported Python version available. - -Using Dockerfile ----------------- - -To simplify testing on multiple linux distros, a Dockerfile is included for running ``tox`` on a -variety of Debian-based distros with all packaged Python versions pre-installed. To build a test -image, run: - -.. code:: console - - $ docker build ${OUSTER_SDK_PATH} -f ${OUSTER_SDK_PATH}/python/Dockerfile \ - --build-arg BASE=ubuntu:20.04 \ - -t ouster-sdk-tox - -the ``BASE`` argument will default to ``ubuntu:20.04``, but can also be set to other docker tags, -e.g. ``ubuntu:22.04`` or ``debian:11``. Then, run the container to invoke tox: - -.. code:: console - - $ docker run -it --rm ouster-sdk-tox diff --git a/docs/python/quickstart.rst b/docs/python/quickstart.rst index a52ecf1a..eb37c49a 100644 --- a/docs/python/quickstart.rst +++ b/docs/python/quickstart.rst @@ -4,10 +4,15 @@ Developer's Quick Start with the Ouster Python SDK ================================================== +.. contents:: + :local: + :depth: 3 + This quickstart guide will walk you through visualizing Ouster sensor data quickly with Python code you write yourself. It assumes that you have followed the steps in :ref:`Python Installation ` to install the Ouster Python SDK. + Using this Guide ================ @@ -53,19 +58,241 @@ In your open python session, save the two paths to variables: >>> pcap_path = '' >>> metadata_path = '' -Because our pcap file contains the UDP packet stream but not the sensor metadata, we load the sensor -information from ``metadata_path`` first, using the client module: - .. note:: Starting with ouster-sdk v0.11.0, most of core sdk objects have been moved from the ``ouster`` namespace into the ``ouster.sdk`` namespace. + +Using the ScanSource interface +============================== + +.. _scan-source-example: + +In this example we are going to demonstrate the use of the ScanSource API. + + +Using the open_source method +---------------------------- + +The ``ScanSource`` API introduces a new method name ``open_source`` which allows users to handle different source types +using the same API. Current supported source types are live sensor, pcap file, or osf file. +For example, opening the sample pcap file can be accomplished as follows: + +.. code:: python + + >>> pcap_path = '' + >>> metadata_path = '' + >>> from ouster.sdk import open_source + >>> source = open_source(pcap_path, meta=[metadata_path]) + + +The ``source`` object returned by ``open_source`` provides access to ``LidarScan`` objects, +regardless of whether the source data comes from a sensor, pcap, or osf file. + +Notice here that rather than we try to load and parse the metadata ourselves we only need to pass to metadata +to the method through ``meta`` parameter and the method will take care of loading it and associating it with the +source object. The ``meta`` parameter however is optional and can be omitted. When the ``meta`` parameter is not +set explicity the ``open_source`` method will attempt to locate the metadata automatically for us and we can reduce +the call to: + +.. code:: python + >>> source = open_source(pcap_path) + +However if metadata file is not in the same folder as the pcap and don't have a shared name prefix the method will +fail. + + +.. note:: + Another optional but important parameter for the ``open_source`` method is ``sensor_idx``. This parameter is set to + zero by default, which should always be the case unless the pcap file that you are using (or osf or any LidarScan + storage) contains scans from more than one sensor, in this case, users can set the ``sensor_idx`` to a any value + between zero and ``sensors_count -1`` to access and manipulate scans from a specific sensor by the order they appear + in the file. Alternatively, if users set the value of ``sensor_idx`` to ``-1`` then ``open_source`` will return a + slightly differnt interface from ``ScanSource`` which is the ``MultiScanSource`` this interface and as the name + suggests allows users to work with sensor data collected from multiple sensors at the same time. + + The main different between the ``MultiScanSource`` and the ``ScanSource`` is the expected return of some of the object + methods. For example, when creating an iterator for a ``ScanSource`` object, the user would get a single ``LidarScan`` + object per iteration. Iterating over the contents of a ``MultiScanSource`` object always yields a **list** of + ``LidarScan(s)`` per iteration corresponding to the number of sensors stored in the pcap file or whatever source type + is being used. This is true even when the pcap file contains data for a single sensor. + + +On the other hand, if the user wants to open an osf file or access the a live sensor, all that changes is url +of the source. For example, to interact with a live sensor the user can execute the following snippet: + +.. code:: python + + >>> sensor_url = '' + >>> from ouster.sdk import open_source + >>> source = open_source(sensor_url) + + +Obtaining sensor metadata +------------------------- + +Every ScanSource holds a reference to the sensor metadata, which has crucial information that is important when +when processing the invidivual scans. Continuing the example, a user this can access the metadata through the +``metadata`` property of a ``ScanSource`` object: + +.. code:: python + + >>> print(source.metadata) + + +Iterating over Scans +-------------------- + +Once we have successfully obtain a handle to the ScanSource we can iterate over ``LidarScan`` objects stored in the +pcap file and manipulate each one individually. For example, let's say we want to print the frame id of the first 10 +scans. We can achieve that using: + +.. code:: python + + >>> ctr = 0 + >>> source_iter = iter(source) + >>> for scan in source_iter: + ... print(scan.frame_id) + ... ctr += 1 + ... if ctr == 10: + ... break + +As we noted earlier, if we set ``sensor_idx=-1`` when invoking ``open_source`` method, the method will construct a +``MultiScanSource``, which always addresses a group of sensors. Thus, when iterating over the ``source`` the user +receives a collated set of scans from the addressed sensors per iteration. The ``MultiScanSource`` examines the +timestamp of every scan from every sensor and returns a list of scans that fit within the same time window as single +batch. The size of the batch is fixed corresponding to how many sensors contained in the pcap or osf file. However, +the collation could yield a null value if one or more of the sensors didn't produce a ``LidarScan`` object that fits +within the time frame of current batch or iteration. Thus, depending on the operation at hand it is crticial to check +if we got a valid ``LidarScan`` object when examining the iteration output of a ``MultiScanSource``. If we are to +perform the same example as above when ``source`` is a handle to ``MultiScanSource`` and display the frame_id of +``LidarScan`` objects the belongs to the same batch on the same line the code needs to updated to the following: + +.. code:: python + + >>> ctr = 0 + >>> source_iter = iter(source) + >>> for scans in source_iter: + ... for scan in scans: # source_iter here returns a list of scans + ... if scan: # check if invidiual scan object is valid + ... print(scan.frame_id, end=', ') + ... print() # new line for next batch + ... ctr += 1 + ... if ctr == 10: + ... break + + +Note that when iterating over a ``MultiScanSource`` object, it always a list of scans, even when the underlying scan +source has only a single sensor. In this case, the iterator will yield a list with a single element per iteration. + + +Using indexing and slicing capabilities of a ScanSource +------------------------------------------------------- + +One of the most prominent new features of the ScanSource API, (besides being able to address multi sensors), is the +ability to use indexing and slicing when accessing the stored scans within the ``LidarScan`` source. Currently, this +capability is only supported for indexable sources. That is to say, the functionality we are discussing can only be used +when accessing a pcap or an osf file with indexing turned on. To turn on indexing simply add the ``index`` flag and set +it ``True`` when opening a pcap or osf file: + + +.. code:: python + + >>> pcap_path = '' + >>> from ouster.sdk import open_source + >>> source = open_source(pcap_path, index=True) + .. note:: - Starting with ouster-sdk v0.11.0 we introduce a unified ``ScanSource`` interface that easier to utilize - and is more capable than original objects demonstrated in this quick guide. Please refer to - :ref:`Using ScanSource API ` for details on how to use the new interface. + We omitted the ``meta`` parameter since it can be populated automatically as we explained earlier. + +Depending on the file size and the underlying file format there can be some delay before the file is fully indexed (OSF +file take much less time than pcap file to index). A progress bar will appear to indicate progress of the indexing. + +Once the index is built up, then we can start using utilizing and interact with the ``ScanSource`` object to access scans +in the same manner we are dealing with a python list that holds reference to LidarScan objects. + +For example to access the 10th LidarScan and print its frame id, we can do the following: + +.. code:: python + + >>> print(source[10].frame_id) + +Similarly we can access the last LidarScan object and print its frame_id using: + +.. code:: python + + >>> print(source[-1].frame_id) + + +Alternatively we can instead request a range of scans using the python slice operator. For example, to request the first 10 +scans from a ScanSource and print their frame ids, we can do the following: + +.. code:: python + + >>> for scan in source[0:10]: + ... print(scan.frame_id) +Note we don't need to add any break here since the operation `source[0:10]` will only yield the first 10 ``LidarScan(s)``. + +To print frame_id of the last 10 LidarScans we do: + +.. code:: python + + >>> for scan in source[-11:-1]: + ... print(scan.frame_id) + + +Finally, as you would expect from a typical slice operation, you can also using negative step and also use a reversed +iteration as shown in the following example: + +.. code:: python + + >>> for scan in source[0:10:2]: # prints the frame_id of every second scan of the first 10 scans + ... print(scan.frame_id) + + >>> for scan in source[10:0:-1]: # prints the frame_id of every scan of the first 10 scans in reverse + ... print(scan.frame_id) + + +Slicing operator as a ScanSource +-------------------------------- + +In ouster-sdk 0.11.0 release, the slice ``[::]`` operator used to return a list of ``LidarScan`` objects (or list of +lists for the ``MultiScanSource`` case). However, starting with ouster-sdk 0.12.0 the ScanSource slice operator ``[::]`` +returns a ``ScanSource`` scoped to the indicated slice range. This means that the users can pass the object returned by +the slice operator ``[::]`` to any function or code that expects a ``ScanSource`` object (or ``MultiScanSource``). The +following snippet shows few examples to demonstrate this capability: + +.. code:: python + + >>> # ... continuing with the `source` object from the previous example + >>> source2 = source[5:10] + >>> print("source2 length:", len(source2)) # This should print 5 since source2 is scoped to the range [5, 10) + >>> print(source2[0].frame_id) # This is equivalent to calling `print(source[5].frame_id)` + >>> print(source2[4].frame_id) # This is equivalent to calling `print(source[9].frame_id)` + >>> # invoking source2[10].frame_id would result in an `out of range exception`` since source2 is scoped to 5 frames + >>> source_iter = iter(source2) # Use `source2` as an iterator similar to the main `source` + >>> for scan in source_iter: + ... print(scan.frame_id) + >>> # it is possible to sub slice, meaning take the result of a previous slice operation and slice it + >>> # Thus, the following statement is valid + >>> source3 = source2[2:4] # this would yield the same frames as `source3 = source[7:9]` + >>> print("source3 length:", len(source3)) # Should print 2 + + +Using the client API +==================== + +The client API provides :py:class:`.PacketSource` implementations, as well as access to methods for configuring a sensor or reading metadata. + +As such, you can use it instead of the ``ScanSource`` API if you prefer to work with individual packets rather than lidar scans. + +Reading packets from a pcap file +-------------------------------- + +Because our sample pcap file contains the UDP packet stream but not the sensor metadata, we load the sensor +information from ``metadata_path`` first, using the client module: .. code:: python diff --git a/docs/python/slam-api-example.rst b/docs/python/slam-api-example.rst index a9eaeb3e..59909ff1 100644 --- a/docs/python/slam-api-example.rst +++ b/docs/python/slam-api-example.rst @@ -11,9 +11,6 @@ SLAM Quickstart This guide provides examples of using the SLAM API for development purposes. Users can run SLAM with an OS sensor's hostname or IP for real-time processing, or with a recorded PCAP/OSF file for offline processing. -.. warning:: - Due to missing upstream dependency support on python3.12, slam does not work on python3.12. - Obtain Lidar Pose and Calculate Pose Difference =============================================== @@ -24,19 +21,21 @@ between consecutive scans .. code:: python from ouster.sdk import open_source - from ouster.mapping.slam import KissBackend + from ouster.sdk.mapping.slam import KissBackend import numpy as np - scans = open_source(pcap_path) - slam = KissBackend(scans.metadata, max_range=75, min_range=1, voxel_size=1.0) + import ouster.sdk.client as client + data_source = open_source(pcap_path) + slam = KissBackend(data_source.metadata, max_range=75, min_range=1, voxel_size=1.0) last_scan_pose = np.eye(4) - for idx, scan in enumerate(scans): + for idx, scan in enumerate(data_source): scan_w_poses = slam.update(scan) col = client.first_valid_column(scan_w_poses) # scan_w_poses.pose is a list where each pose represents a column points' pose. # use the first valid scan's column pose as the scan pose scan_pose = scan_w_poses.pose[col] - print(f"idx = {idx} and Scan Pose {scan_pose}") + scan_ts = scan.timestamp[col] + print(f"idx = {idx} at timestamp {scan_ts} has the pose {scan_pose}") # calculate the inverse transformation of the last scan pose inverse_last = np.linalg.inv(last_scan_pose) @@ -56,20 +55,21 @@ as well as for demonstration and feedback purposes. .. code:: python + from ouster.sdk import open_source from functools import partial - from ouster.viz import SimpleViz, ScansAccumulator - from ouster.mapping.slam import KissBackend - scans = open_source(pcap_path) - slam = KissBackend(scans.metadata, max_range=75, min_range=1, voxel_size=1.0) + from ouster.sdk.viz import SimpleViz, ScansAccumulator + from ouster.sdk.mapping.slam import KissBackend + data_source = open_source(pcap_path) + slam = KissBackend(data_source.metadata, max_range=75, min_range=1, voxel_size=1.0) - scans_w_poses = map(partial(slam.update), scans) - scans_acc = ScansAccumulator(info, + scans_w_poses = map(partial(slam.update), data_source) + scans_acc = ScansAccumulator(data_source.metadata, accum_max_num=10, accum_min_dist_num=1, map_enabled=True, map_select_ratio=0.01) - SimpleViz(info, scans_accum=scans_acc, rate=0.0).run(scans_w_poses) + SimpleViz(data_source.metadata, scans_accum=scans_acc, rate=1.0).run(scans_w_poses) More details about the visualizer and accumulated scans can be found at the :ref:`Ouster Visualizer ` and :ref:`Scans Accumulator ` diff --git a/docs/python/using-scan-source.rst b/docs/python/using-scan-source.rst deleted file mode 100644 index 459b881b..00000000 --- a/docs/python/using-scan-source.rst +++ /dev/null @@ -1,197 +0,0 @@ -================================== -Using the new ScanSource interface -================================== - -.. contents:: - :local: - :depth: 3 - -.. _scan-source-example: - -In this example we are going to demonstrate the use of the new ScanSource API. - - -Using the open_source method -============================ - -The new API introduces a new method name ``open_source`` which allows users to handle different source types -using the same API. Current supported source types are live sensor, pcap file, or osf file. For example, to -open the same pcap file referenced in the main :ref:`Quick Start ` using the simplified API -can be accomplished as follows: - -.. code:: python - - >>> pcap_path = '' - >>> metadata_path = '' - >>> from ouster.sdk import open_source - >>> source = open_source(pcap_path, meta=[metadata_path]) - - -The ``source`` handle here acts the same as the handle returned by the ``pcap.Pcap`` constructor, with some -extra capabilities that we will cover later. - -Notice here that rather than we try to load and parse the metadata ourselves we only need to pass to metadata -to the method through ``meta`` parameter and the method will take care of loading it and associating it with the -source object. The ``meta`` parameter however is optional and can be omitted. When the ``meta`` parameter is not -set explicity the ``open_source`` method will attempt to locate the metadata automatically for us and we can reduce -the call to: - -.. code:: python - >>> source = open_source(pcap_path) - -However if metadata file is not in the same folder as the pcap and don't have a shared name prefix the method will -fail. - - -.. note:: - Another optional but important parameter for the ``open_source`` method is ``sensor_idx``. This paramter is to zero - by default, which should always be the case unless the pcap file that you are using (or osf or any LidarScan storage) - contains scans from more than one sensor, in this case, users can set the ``sensor_idx`` to a any value between zero - and ``sensors_count -1`` to access and manipulate scans from a specific sensor by the order they appear in the file. - Alternatively, if users set the value of ``sensor_idx`` to ``-1`` then ``open_source`` will return a slightly differnt - interface from ``ScanSource`` which is the ``MultiScanSource`` this interface and as the name suggests allows users to - work with sensor data collected from multiple sensors at the same time. - - The main different between the ``MultiScanSource`` and the ``ScanSource`` is the expected return of some of the object - methods. For example, when creating an iterator for a ``ScanSource`` object, the user would get a single ``LidarScan`` - object per iteration. Iterating over the contents of a ``MultiScanSource`` object always yields a **list** of - ``LidarScan(s)`` per iteration corresponding to the number of sensors stored in the pcap file or whatever source type - is being used. This is true even when the pcap file contains data for a single sensor. - - -On the other hand, if the user wants to open an osf file or access the a live sensor, all that changes is url -of the source. For example, to interact with a live sensor the user can execute the following snippet: - -.. code:: python - - >>> sensor_url = '' - >>> from ouster.sdk import open_source - >>> source = open_source(sensor_url) - - -Obtaining sensor metadata -========================= -Every ScanSource holds a reference to the sensor metadata, which has crucial information that is important when -when processing the invidivual scans. Continuing the example, a user this can access the metadata through the -``metadata`` property of a ``ScanSource`` object: - -.. code:: python - - >>> print(source.metadata) - - -Iterating over Scans -==================== - -Once we have successfully obtain a handle to the ScanSource we can iterate over ``LidarScan`` objects stored in the -pcap file and manipulate each one individually. For example, let's say we want to print the frame id of the first 10 -scans. We can achieve that using: - -.. code:: python - - >>> ctr = 0 - >>> source_iter = iter(source) - >>> for scan in source_iter: - ... print(scan.frame_id) - ... ctr += 1 - ... if ctr == 10: - ... break - -As we noted earlier, if we set ``sensor_idx=-1`` when invoking ``open_source`` method, the method will construct a -``MultiScanSource``, which always addresses a group of sensors. Thus, when iterating over the ``source`` the user -receives a collated set of scans from the addressed sensors per iteration. The ``MultiScanSource`` examines the -timestamp of every scan from every sensor and returns a list of scans that fit within the same time window as single -batch. The size of the batch is fixed corresponding to how many sensors contained in the pcap or osf file. However, -the collation could yield a null value if one or more of the sensors didn't produce a ``LidarScan`` object that fits -within the time frame of current batch or iteration. Thus, depending on the operation at hand it is crticial to check -if we got a valid ``LidarScan`` object when examining the iteration output of a ``MultiScanSource``. If we are to -perform the same example as above when ``source`` is a handle to ``MultiScanSource`` and display the frame_id of -``LidarScan`` objects the belongs to the same batch on the same line the code needs to updated to the following: - -.. code:: python - - >>> ctr = 0 - >>> source_iter = iter(source) - >>> for scans in source_iter: - ... for scan in scans: # source_iter here returns a list of scans - ... if scan: # check if invidiual scan object is valid - ... print(scan.frame_id, end=', ') - ... print() # new line for next batch - ... ctr += 1 - ... if ctr == 10: - ... break - - -Note that when iterating over a ``MultiScanSource`` object, it always a list of scans, even when the underlying scan -source has only a single sensor. In this case, the iterator will yield a list with a single element per iteration. - - -Using indexing and slicing capabilities of a ScanSource -======================================================== - -One of the most prominent new features of the ScanSource API, (besides being able to address multi sensors), is the -ability to use indexing and slicing when accessing the stored scans within the ``LidarScan`` source. Currently, this -capability is only supported for non-live sources. That is to say, the functionality we are discussing can only be used -when accessing a pcap or an osf file. To enable this functionality we need to indicate that we want to manipulate the -source as an indexed one upon opening. Revisitng the previous pcap open example, that would be achieved as follows: -: - - -.. code:: python - - >>> pcap_path = '' - >>> from ouster.sdk import open_source - >>> source = open_source(pcap_path, index=True) - -First note that we omitted the ``meta`` parameter since it can be populated automatically as we explained earlier. -Second you will noticed that we introduced a new parameter ``index`` with its value set to ``True`` (default is false), -The same parameter can be applied to when dealing with an osf file but not a live sensor. - -Depending on the file size and the underlying file format there can be some delay before the file is fully indexed (OSF -file take much less time than pcap file to index). A progress bar will appear to indicate progress of the indexing. - -Once the index is built up, then we can start using utilizing and interact with the ``ScanSource`` object to access scans -in the same manner we are dealing with a python list that holds reference to LidarScan objects. - -For example to access the 10th LidarScan and print its frame id, we can do the following: - -.. code:: python - - >>> print(source[10].frame_id) - -Similarly we can access the last LidarScan object and print its frame_id using: - -.. code:: python - - >>> print(source[-1].frame_id) - - -Alternatively we can instead request a range of scans using the python slice operator. For example, to request the first 10 -scans from a ScanSource and print their frame ids, we can do the following: - -.. code:: python - - >>> for scan in source[0:10]: - ... print(scan.frame_id) - - -Note we don't need to add any break here since the operation `source[0:10]` will only yield the first 10 ``LidarScan(s)``. - -To print frame_id of the last 10 LidarScans we do: - -.. code:: python - - >>> for scan in source[-11:-1]: - ... print(scan.frame_id) - - -Finally, as you would expect from a typical slice operation, you can also using negative step and also use a reversed -iteration as shown in the following example: - -.. code:: python - - >>> for scan in source[0:10:2]: # prints the frame_id of every second scan of the first 10 scans - ... print(scan.frame_id) - - >>> for scan in source[10:0:-1]: # prints the frame_id of every scan of the first 10 scans in reverse - ... print(scan.frame_id) diff --git a/docs/python/viz/index.rst b/docs/python/viz/index.rst index 63f12bb6..e6ea8ec3 100644 --- a/docs/python/viz/index.rst +++ b/docs/python/viz/index.rst @@ -8,7 +8,7 @@ consists of the following: - ``simple-viz`` (:class:`.viz.SimpleViz`): the default Python application visualizer, which can also be used as an entrypoint for more sophisticated custom point cloud visualizations - ``ouster_viz``: the core C++ library -- :mod:`ouster.viz`: the Python module for the bindings +- :mod:``ouster.sdk.viz``: the Python module for the bindings .. todo:: diff --git a/docs/python/viz/viz-run.rst b/docs/python/viz/viz-run.rst index bacc607f..115f4d8f 100644 --- a/docs/python/viz/viz-run.rst +++ b/docs/python/viz/viz-run.rst @@ -33,39 +33,74 @@ keyboard: .. [start-simple-viz-keymap] -Keyboard controls: +Keyboard Controls +----------------- + +**Camera*** ================ =============================================== Key What it does ================ =============================================== - ``o`` Toggle on-screen display - ``p / P`` Increase/decrease point size - ``m / M`` Cycle point cloud coloring mode - ``f / F`` Cycle point cloud color palette - ``b / B`` Cycle top 2D image - ``n / N`` Cycle bottom 2D image - ``R`` Reset camera - ``ctr-r`` Set camera to the birds-eye view - ``u`` Toggle camera mode FOLLOW/FIXED - ``e / E`` Increase/decrease size of displayed 2D images - ``' / "`` Increase/decrease spacing in range markers + ``shift`` Camera Translation with mouse drag ``w`` Camera pitch up ``s`` Camera pitch down ``a`` Camera yaw left ``d`` Camera yaw right - ``1`` Toggle first return point cloud visibility - ``2`` Toggle second return point cloud visibility - ``0`` Toggle orthographic camera + ``R`` Reset camera + ``ctr-r`` Set camera to the birds-eye view + ``u`` Toggle camera mode FOLLOW/FIXED ``= / -`` Dolly in/out - ``?`` Prints key bindings + ``0`` Toggle orthographic camera + ================ =============================================== + +**Playback** + ================ =============================================== + Key What it does + ================ =============================================== ``space`` Toggle pause - ``esc`` Exit visualization ``. / ,`` Step one frame forward/back ``ctrl + . / ,`` Step 10 frames forward/back ``> / <`` Increase/decrease playback rate (during replay) - ``shift`` Camera Translation with mouse drag + ================ =============================================== + +**2D View** + ================ =============================================== + Key What it does + ================ =============================================== + ``b / B`` Cycle top 2D image + ``n / N`` Cycle bottom 2D image + ``e / E`` Increase/decrease size of displayed 2D images + ================ =============================================== + +**3D View** + ================ =============================================== + Key What it does + ================ =============================================== + ``p / P`` Increase/decrease point size + ``m / M`` Cycle point cloud coloring mode + ``f / F`` Cycle point cloud color palette + ``1`` Toggle first return point cloud visibility + ``6`` Toggle scans accumulation view mode (ACCUM) + ``7`` Toggle overall map view mode (MAP) + ``8`` Toggle poses/trajectory view mode (TRACK) + ``2`` Toggle second return point cloud visibility + ``9`` Show axes + ``' / "`` Increase/decrease spacing in range markers + ``ctrl + '`` Increase thickness of range markers + ``c`` Cycle current highlight mode + ``j / J`` Increase/decrease point size of accumulated clouds or map + ``k / K`` Cycle point cloud coloring mode of accumulated clouds or map + ``g / G`` Cycle point cloud color palette of accumulated clouds or map + ================ =============================================== + +**Other** + ================ =============================================== + Key What it does + ================ =============================================== + ``o`` Toggle on-screen display + ``?`` Print keys to standard out ``shift+z`` Save a screenshot of the current view ``shift+x`` Toggle a continuous saving of screenshots - ``?`` Print keys to standard out + ``esc`` Exit ================ =============================================== .. diff --git a/docs/python/viz/viz-scans-accum.rst b/docs/python/viz/viz-scans-accum.rst index 9509168e..108dde55 100644 --- a/docs/python/viz/viz-scans-accum.rst +++ b/docs/python/viz/viz-scans-accum.rst @@ -52,21 +52,18 @@ Keyboard controls available with **ScansAccumulator**: Use in Ouster SDK CLI ^^^^^^^^^^^^^^^^^^^^^^ -**ScansAccumulator** is present in every CLI viz call, no matter the source of data or way of -calling it.It's in live ``sensor viz``, ``pcap record --viz``, ``osf viz``, ``mapping viz`` and -``pcap viz`` as well as all ``source SOURCE`` versions of those commands. Everywhere the same set of -CLI parameters control the behavior of the **ScansAccumulator**. +**ScansAccumulator** is accessible when visualizing data with ``ouster-cli`` using the ``viz`` command. +Here are the ``viz`` command options that affect it: -Works for data sources with/without poses: - - * ``--accum-num N`` - accumulate *N* scans (default: ``0``) - * ``--accum-every K`` - accumulate every *Kth* scan (default: ``1``) - * ``--accum-every-m M`` - accumulate a scan every *Mth* meters traveled (default: ``None``) - * ``--accum-map`` - enable the overall map accumulation, select some percentage of points from - every scan (default: disabled) - * ``--accum-map-ratio R`` - set *R* as a ratio of points to randomly select from every scan - (default: ``0.001`` (*0.1%*)) + * ``--accum-num INTEGER`` - Accumulate up to this number of past scans for visualization. + Use <= 0 for unlimited. Defaults to 100 if ``--accum-every`` or ``--accum-every-m`` is set. + * ``--accum-every INTEGER`` - Add a new scan to the accumulator every this number of scans. + * ``--accum-every-m FLOAT`` - Add a new scan to the accumulator after this many meters of travel. + * ``--map`` - If set, add random points from every scan into an overall map for visualization. + Enabled if either ``--map-ratio`` or ``--map-size`` are set. + * ``--map-ratio R`` - Fraction of random points in every scan to add to overall map (0, 1]. [default: 0.01] + * ``--map-size N`` - Maximum number of points in overall map before discarding. [default: 1500000] Examples of the CLI commands: @@ -93,8 +90,8 @@ Overall map view (with poses) One of the main task that we often needed was a preview of the overall map from the OSF with poses for example:: - ouster-cli osf viz OS-0-128_v3.0.1_1024x10_20230415_152307-000.osf --accum-num 20 \ - --accum-every 0 --accum-every-m 10.5 --accum-map -r 0 -e stop + ouster-cli source OS-0-128_v3.0.1_1024x10_20230415_152307-000.osf viz --accum-num 20 \ + --accum-every 0 --accum-every-m 10.5 --map -e stop And here is the final result when viz is done and stopped (``-e stop``) after playing the whole file: @@ -122,7 +119,7 @@ Programmatic use with (and without) PointViz With ``point_viz: PointViz`` object the ``ScansAccumulator`` can be used as a regular ``LidarScanViz`` and passed directly to ``SimpleViz``:: - from ouster.viz import PointViz, add_default_controls, ScansAccumulator + from ouster.sdk.viz import PointViz, add_default_controls, ScansAccumulator, SimpleViz point_viz = PointViz("SimpleViz usecase") add_default_controls(point_viz) @@ -141,7 +138,7 @@ With ``point_viz: PointViz`` object the ``ScansAccumulator`` can be used as a re Alternatively with a ``PointViz`` it can be used as a canvas to draw the final state only:: - from ouster.viz import ScansAccumulator, add_default_controls, PointViz + from ouster.sdk.viz import ScansAccumulator, add_default_controls, PointViz point_viz = PointViz("Overall map case") add_default_controls(point_viz) @@ -159,7 +156,7 @@ Alternatively with a ``PointViz`` it can be used as a canvas to draw the final s scans_acc.update(scan) scans_acc.draw(update=True) - point_viz.upadte() + point_viz.update() point_viz.run() @@ -167,7 +164,7 @@ Without ``PointViz`` it can be used as in the following snippet to accumulate al data later to draw anywhere (here we still use the ``PointViz`` and ``viz.Cloud()`` as a main graphing tool, but it can be ``matplotlib`` instead):: - from ouster.viz import grey_palette, ScansAccumulator, Cloud, add_default_controls, PointViz + from ouster.sdk.viz import grey_palette, ScansAccumulator, Cloud, add_default_controls, PointViz # ... get scans_w_poses Scans source ... diff --git a/docs/versions.json b/docs/versions.json index 152af2c2..2437b4c2 100644 --- a/docs/versions.json +++ b/docs/versions.json @@ -1,4 +1,11 @@ [ + { + "version": "0.11.1", + "tags": { + "sdkx": "2889b1627b0b78b320861049667e801d04023d05", + "sdk": "d6c6dfe3b33e2c6d5a2ed84b079db5c99e7fd188" + } + }, { "version": "0.11.0", "tags": { diff --git a/examples/async_client_example.cpp b/examples/async_client_example.cpp index 3ca55054..e3faa802 100644 --- a/examples/async_client_example.cpp +++ b/examples/async_client_example.cpp @@ -86,7 +86,7 @@ int main(int argc, char* argv[]) { auto metadata = sensor::get_metadata(*handle); // Raw metadata can be parsed into a `sensor_info` struct - sensor::sensor_info info = sensor::parse_metadata(metadata); + sensor::sensor_info info(metadata); size_t w = info.format.columns_per_frame; size_t h = info.format.pixels_per_column; @@ -120,8 +120,9 @@ int main(int argc, char* argv[]) { * UDP socket. */ - // buffer to store raw packet data - auto packet_buf = std::make_unique(UDP_BUF_SIZE); + // Place to store raw packets as they pass between threads + ouster::sensor::LidarPacket lidar_packet(pf.lidar_packet_size); + ouster::sensor::ImuPacket imu_packet(pf.imu_packet_size); /* In this example we spin two threads one to receive lidar packets while the @@ -159,7 +160,7 @@ int main(int argc, char* argv[]) { std::unique_lock lock(mtx); receiving_cv.wait( lock, [&packet_processed] { return packet_processed; }); - if (!sensor::read_lidar_packet(*handle, packet_buf.get(), pf)) { + if (!sensor::read_lidar_packet(*handle, lidar_packet)) { FATAL("Failed to read a packet of the expected size!"); } packet_processed = false; @@ -171,7 +172,7 @@ int main(int argc, char* argv[]) { std::unique_lock lock(mtx); receiving_cv.wait( lock, [&packet_processed] { return packet_processed; }); - sensor::read_imu_packet(*handle, packet_buf.get(), pf); + sensor::read_imu_packet(*handle, imu_packet); // we are not going to processor imu data // so we will keep packet_processed set to true } @@ -184,7 +185,7 @@ int main(int argc, char* argv[]) { processing_cv.wait( lock, [&packet_processed] { return !packet_processed; }); // batcher will return "true" when the current scan is complete - if (batch_to_scan(packet_buf.get(), scan)) { + if (batch_to_scan(lidar_packet, scan)) { // retry until we receive a full set of valid measurements // (accounting for azimuth_window settings if any) if (scan.complete(info.format.column_window)) { @@ -213,7 +214,8 @@ int main(int argc, char* argv[]) { void display_scan_summary(const LidarScan& scan) { // channel fields can be queried as well - auto n_valid_first_returns = (scan.field(sensor::RANGE) != 0).count(); + auto n_valid_first_returns = + (scan.field(sensor::ChanField::RANGE) != 0).count(); // LidarScan also provides access to header information such as // status and timestamp @@ -247,4 +249,4 @@ void write_cloud(const std::string& file_path, const LidarScan::Points& cloud) { out.close(); std::cerr << " Wrote " << file_path << std::endl; -} \ No newline at end of file +} diff --git a/examples/client_example.cpp b/examples/client_example.cpp index 65284602..0c4e4cf4 100644 --- a/examples/client_example.cpp +++ b/examples/client_example.cpp @@ -65,21 +65,21 @@ int main(int argc, char* argv[]) { * accurate point clouds. */ std::cerr << "Gathering metadata..." << std::endl; - auto metadata = sensor::get_metadata(*handle, 10, false); + auto metadata = sensor::get_metadata(*handle, 10); // Raw metadata can be parsed into a `sensor_info` struct - sensor::sensor_info info = sensor::parse_metadata(metadata); + sensor::sensor_info info(metadata); size_t w = info.format.columns_per_frame; size_t h = info.format.pixels_per_column; ouster::sensor::ColumnWindow column_window = info.format.column_window; - // The dedicated firmware_version_from_metadata API works across firmwares - auto fw_ver = sensor::firmware_version_from_metadata(metadata); + auto fw_ver = info.get_version(); - std::cerr << " Firmware version: " << to_string(fw_ver) - << "\n Serial number: " << info.sn + std::cerr << " Firmware version: " << fw_ver.major << "." << fw_ver.minor + << "." << fw_ver.patch; + std::cerr << "\n Serial number: " << info.sn << "\n Product line: " << info.prod_line << "\n Scan dimensions: " << w << " x " << h << "\n Column window: [" << column_window.first << ", " @@ -153,7 +153,8 @@ int main(int argc, char* argv[]) { clouds.push_back(ouster::cartesian(scan, lut)); // channel fields can be queried as well - auto n_valid_first_returns = (scan.field(sensor::RANGE) != 0).count(); + auto n_valid_first_returns = + (scan.field(sensor::ChanField::RANGE) != 0).count(); // LidarScan also provides access to header information such as // status and timestamp diff --git a/examples/config_example.cpp b/examples/config_example.cpp index 92e5b778..69331c16 100644 --- a/examples/config_example.cpp +++ b/examples/config_example.cpp @@ -47,7 +47,7 @@ int main(int argc, char* argv[]) { //! [doc-stag-cpp-make-config] sensor::sensor_config config; config.azimuth_window = std::make_pair(90000, 270000); - config.ld_mode = sensor::lidar_mode::MODE_512x10; + config.lidar_mode = sensor::lidar_mode::MODE_512x10; // If relevant, use config_flag to set udp dest automatically uint8_t config_flags = 0; @@ -78,7 +78,7 @@ int main(int argc, char* argv[]) { // Confirm that only what we wanted to change changed assert(original_config != new_config); assert(new_config.azimuth_window == config.azimuth_window); - assert(new_config.ld_mode == config.ld_mode); + assert(new_config.lidar_mode == config.lidar_mode); std::cerr << "Updated config: \n" << to_string(new_config) << std::endl; diff --git a/examples/helpers.cpp b/examples/helpers.cpp index e7156922..45fdffde 100644 --- a/examples/helpers.cpp +++ b/examples/helpers.cpp @@ -26,17 +26,17 @@ void get_complete_scan( ouster::ScanBatcher batch_to_scan(info.format.columns_per_frame, pf); // Buffer to store raw packet data - auto packet_buf = std::make_unique(BUF_SIZE); + ouster::sensor::LidarPacket packet(pf.lidar_packet_size); ouster::sensor_utils::packet_info packet_info; while (ouster::sensor_utils::next_packet_info(*handle, packet_info)) { auto packet_size = ouster::sensor_utils::read_packet( - *handle, packet_buf.get(), BUF_SIZE); + *handle, packet.buf.data(), packet.buf.size()); if (packet_size == pf.lidar_packet_size && - packet_info.dst_port == info.udp_port_lidar) { - if (batch_to_scan(packet_buf.get(), scan)) { + packet_info.dst_port == info.config.udp_port_lidar) { + if (batch_to_scan(packet, scan)) { if (first_frame_id == 0) { // end of first frame -- assume it is incomplete and skip first_frame_id = scan.frame_id; diff --git a/examples/lidar_scan_example.cpp b/examples/lidar_scan_example.cpp index c70ed3ff..18287e9c 100644 --- a/examples/lidar_scan_example.cpp +++ b/examples/lidar_scan_example.cpp @@ -60,9 +60,9 @@ int main(int argc, char* argv[]) { //! [doc-stag-lidarscan-reduced-slots] // Finally, you can construct by specifying fields directly - static const std::array, 2> - reduced_slots{{{ChanField::RANGE, ChanFieldType::UINT32}, - {ChanField::NEAR_IR, ChanFieldType::UINT16}}}; + static const std::array reduced_slots{ + {{ChanField::RANGE, ChanFieldType::UINT32}, + {ChanField::NEAR_IR, ChanFieldType::UINT16}}}; auto reduced_fields_scan = ouster::LidarScan(w, h, reduced_slots.begin(), reduced_slots.end()); //! [doc-etag-lidarscan-reduced-slots] @@ -95,12 +95,12 @@ int main(int argc, char* argv[]) { // to access a field: //! [doc-stag-lidarscan-cpp-fields] - auto range = profile_scan.field(ChanField::RANGE); + auto range = profile_scan.field(ChanField::RANGE); //! [doc-etag-lidarscan-cpp-fields] // On dual returns, second returns are often the same field name with 2 // appended: - auto range2 = dual_returns_scan.field(ChanField::RANGE2); + auto range2 = dual_returns_scan.field(ChanField::RANGE2); std::cerr << "\nPrinting first element of received scan headers\n\tframe_id : " @@ -121,7 +121,8 @@ int main(int argc, char* argv[]) { // LidarScan std::cerr << "Accessing field that isn't available..."; try { - auto signal_field = reduced_fields_scan.field(ChanField::SIGNAL); + auto signal_field = + reduced_fields_scan.field(ChanField::SIGNAL); std::cerr << signal_field(0, 0) << std::endl; } catch (const std::out_of_range&) { std::cerr << " ..received expected out of range error. Continuing..." @@ -134,10 +135,10 @@ int main(int argc, char* argv[]) { auto print_el = [](ouster::LidarScan& scan, std::string label) { std::cerr << "Available fields in " << label << "...\n"; //! [doc-stag-cpp-scan-iter] - for (auto it = scan.begin(); it != scan.end(); it++) { - auto field = it->first; - // auto field_type = it->second; - std::cerr << "\t" << to_string(field) << "\n "; + for (const auto& kv : scan.fields()) { + auto field_name = kv.first; + // auto& field = kv.second; + std::cerr << "\t" << field_name << "\n "; } //! [doc-etag-cpp-scan-iter] std::cerr << std::endl; diff --git a/examples/mtp_client_example.cpp b/examples/mtp_client_example.cpp index 90287ace..3018eb0a 100644 --- a/examples/mtp_client_example.cpp +++ b/examples/mtp_client_example.cpp @@ -55,7 +55,7 @@ int main(int argc, char* argv[]) { } auto metadata = sensor::get_metadata(*cli); - sensor::sensor_info info = sensor::parse_metadata(metadata); + sensor::sensor_info info(metadata); sensor::packet_format pf = sensor::get_format(info); auto packet_buf = std::make_unique(UDP_BUF_SIZE); @@ -81,4 +81,4 @@ int main(int argc, char* argv[]) { } } } -} \ No newline at end of file +} diff --git a/examples/representations_example.cpp b/examples/representations_example.cpp index f4189153..571ecce6 100644 --- a/examples/representations_example.cpp +++ b/examples/representations_example.cpp @@ -125,7 +125,8 @@ int main(int argc, char* argv[]) { if (info.format.udp_profile_lidar == sensor::UDPProfileLidar::PROFILE_LIDAR_LEGACY) { - reflectivity = scan.field(sensor::ChanField::REFLECTIVITY); + reflectivity = scan.field(sensor::ChanField::REFLECTIVITY) + .cast(); } else if (info.format.udp_profile_lidar == sensor::UDPProfileLidar::PROFILE_RNG19_RFL8_SIG16_NIR16_DUAL) { reflectivity = scan.field(sensor::ChanField::REFLECTIVITY) diff --git a/ouster_client/CMakeLists.txt b/ouster_client/CMakeLists.txt index 6f21a128..72ac16e6 100644 --- a/ouster_client/CMakeLists.txt +++ b/ouster_client/CMakeLists.txt @@ -9,16 +9,15 @@ include(Coverage) add_library(ouster_client src/client.cpp src/types.cpp src/sensor_info.cpp src/netcompat.cpp src/lidar_scan.cpp src/image_processing.cpp src/udp_packet_source.cpp src/parsing.cpp src/sensor_http.cpp src/sensor_http_imp.cpp src/sensor_tcp_imp.cpp src/logging.cpp - src/profile_extension.cpp - src/util.cpp) + src/field.cpp src/profile_extension.cpp src/util.cpp) target_link_libraries(ouster_client PUBLIC Eigen3::Eigen + jsoncpp_lib $ + spdlog::spdlog PRIVATE - CURL::libcurl - jsoncpp_lib - spdlog::spdlog) + CURL::libcurl) target_compile_definitions(ouster_client PRIVATE EIGEN_MPL2_ONLY) CodeCoverageFunctionality(ouster_client) diff --git a/ouster_client/include/ouster/array_view.h b/ouster_client/include/ouster/array_view.h new file mode 100644 index 00000000..bc8d044f --- /dev/null +++ b/ouster_client/include/ouster/array_view.h @@ -0,0 +1,490 @@ +/** + * Copyright (c) 2024, Ouster, Inc. + * All rights reserved. + */ + +#pragma once + +#include +#include +#include +#include +#include + +#include "ouster/impl/cuda_macros.h" +#include "ouster/impl/idx_range.h" + +namespace ouster { + +template +class ArrayView; + +template +using ConstArrayView = ArrayView; + +template +using ArrayView4 = ArrayView; +template +using ArrayView3 = ArrayView; +template +using ArrayView2 = ArrayView; +template +using ArrayView1 = ArrayView; + +template +using ConstArrayView4 = ConstArrayView; +template +using ConstArrayView3 = ConstArrayView; +template +using ConstArrayView2 = ConstArrayView; +template +using ConstArrayView1 = ConstArrayView; + +namespace impl { + +template +using row_pack = std::integer_sequence; + +template +using index_row = std::make_integer_sequence; + +struct stop_recursion {}; + +template +struct drop_left { + using type = row_pack; +}; + +template +struct drop_left { + using type = stop_recursion; +}; + +template +struct product { + /** + * Host-only version for iterators + */ + template + T operator()(IterT, stop_recursion) { + return T{1}; + } + + template + T operator()(IterT iter, row_pack) { + IterT i = --iter; + return static_cast(*i) * + (*this)(i, typename drop_left::type{}); + } + + /** + * Device/host version for pointers + */ + template + OSDK_FN T operator()(IntT*, stop_recursion) { + return T{1}; + } + + template + OSDK_FN T operator()(IntT* ptr, row_pack) { + IntT* i = --ptr; + return static_cast(*i) * + (*this)(i, typename drop_left::type{}); + } + + /** + * Device/host version for parameter pack + */ + template + OSDK_FN T operator()(Arg arg) { + return static_cast(arg); + } + + template + OSDK_FN T operator()(Arg arg, Args... args) { + return (*this)(arg) * (*this)(args...); + } +}; + +template +struct restrict_if_const_ptr { + using type = T*; +}; + +template +struct restrict_if_const_ptr { + using type = const T* RESTRICT; +}; + +// TODO: switch to fold expressions upon c++17 adoption +template +bool _oob_check(const ShapeT& shape, int i, Arg idx0) { + return impl::range_or_idx(idx0) >= static_cast(shape[i]); +} + +// TODO: switch to fold expressions upon c++17 adoption +template +bool _oob_check(const ShapeT& shape, int i, Arg idx0, Args... idx) { + return _oob_check(shape, i, idx0) + _oob_check(shape, i + 1, idx...); +} + +// TODO: switch to fold expressions upon c++17 adoption +template +bool subview_oob_check(const ShapeT& shape, Args... idx) { + return _oob_check(shape, 0, idx...); +} + +/** + * TODO: this is a cpp14 regression, will not compile into CUDA. + * Fix later -- Tim T. + */ +// TODO: switch to fold expressions upon c++17 adoption +template +OSDK_FN int _strided_index(const StridesT& strides, int i, Arg idx0) { + return idx0 * strides[i]; +} + +// TODO: switch to fold expressions upon c++17 adoption +template +OSDK_FN int _strided_index(const StridesT& strides, int i, Arg idx0, + Args... idx) { + return _strided_index(strides, i, idx0) + + _strided_index(strides, i + 1, idx...); +} + +// TODO: switch to fold expressions upon c++17 adoption +template +OSDK_FN int strided_index(const StridesT& strides, Args... idx) { + return _strided_index(strides, 0, idx...); +} + +} // namespace impl + +template +class ArrayView { + static_assert(Dim > 0, "ArrayView dimensions cannot be less than 1"); + + // order of member declarations is important, most used variables first + typename impl::restrict_if_const_ptr::type data_; + + public: + /** + * C array of stride offsets + */ + const int32_t strides[Dim]; + + /** + * C array of shape dimensions + */ + const int32_t shape[Dim]; + + /** + * Construct from any types with subscript operator, i.e. vector/array + * + * host only due to cuda/stl incompatibility + * + * @param[in] ptr pointer to data + * @param[in] shape arbitrary container of shape dimensions + * @param[in] strides arbitrary container of stride offsets + */ + template + ArrayView(T* ptr, const Indexable1& shape, const Indexable2& strides) + : ArrayView(ptr, shape, strides, impl::index_row{}) {} + + /** + * Construct from any container type, deducing strides + * + * host only due to cuda/stl incompatibility + * + * @param[in] ptr pointer to data + * @param[in] shape arbitrary container of shape dimensions + */ + template + ArrayView(T* ptr, const ContainerT& shape) + : ArrayView(ptr, std::begin(shape), std::end(shape), + impl::index_row{}, impl::index_row{}) {} + + /** + * Construct from C arrays + * + * device, host + * + * @param[in] ptr pointer to data + * @param[in] shape C array of shape dimensions + * @param[in] strides C array of stride offsets + */ + OSDK_FN + ArrayView(T* ptr, const int (&shape)[Dim], const int (&strides)[Dim]) + : ArrayView(ptr, shape, strides, impl::index_row{}) {} + + /** + * Construct from initializer list, deducing strides + * + * device, host + * + * @param[in] ptr pointer to data + * @param[in] shape initializer_list of shape dimensions + */ + template + OSDK_FN ArrayView(T* ptr, std::initializer_list shape) + : ArrayView(ptr, shape.begin(), shape.end(), impl::index_row{}, + impl::index_row{}) {} + + /** + * Construct from C array, deducing strides + * + * device, host + * + * @param[in] ptr pointer to data + * @param[in] shape C array of shape dimensions + */ + OSDK_FN + ArrayView(T* ptr, const int (&shape)[Dim]) + : ArrayView(ptr, shape, shape + Dim, impl::index_row{}, + impl::index_row{}) {} + + /** + * Below allows conversion from ArrayView to ConstArrayView + * but not the other way around + * + * /code + * ArrayView4 a; + * ConstArrayView4 b = a; // compiles + * ArrayView4 c = b; // does not compile + * /endcode + * + * @param[in] other ArrayView + */ + template ::value, void>> + OSDK_FN ArrayView(const ArrayView& other) + : ArrayView(other.data_, other.shape, other.strides) {} + + /** + * Retrieve reference to value with subscript operator + * + * @param[in] idx indices + * + * @return reference to value + */ + template + OSDK_FN const T& operator()(Args&&... idx) const { + static_assert(sizeof...(Args) == Dim, + "ArrayView subscript operator " + "must match the array " + "dimensions"); + + return *(data_ + impl::strided_index(strides, idx...)); + } + + /** + * Retrieve reference to value with subscript operator + * + * @param[in] idx indices + * + * @return reference to value + */ + template + OSDK_FN T& operator()(Args&&... idx) { + return const_cast( + const_cast(*this)(std::forward(idx)...)); + } + + /** + * Templated utility to calculate subview dimensions at compile time + * + * @tparam Args parameter pack of indices or idx_range + * + * @return subview dimension + */ + template + OSDK_FN static constexpr int subview_dim() { + return Dim - sizeof...(Args) + impl::count_n_ranges::value; + } + + /** + * Get a subview + * + * Operates similarly to numpy ndarray bracket operator, returning a sliced, + * potentially sparse subview + * + * The following two snippets are functionally equivalent + * \code + * std::vector data(100*100*100); + * ArrayView3 view{data.data(), {100,100,100}; + * // get a slice of all elements in the first dimension with second + * // and third pinned to 10 and 20 respectively + * ArrayView1 subview = view.subview(keep(), 10, 20); + * \endcode + * + * \code + * import numpy as np + * arr = np.ndarray(shape=(100,100,100), dtype=np.int32) + * subview = arr[:,10,20] + * \endcode + * + * @param[in] idx pack of int indices or idx_range (keep()) + * + * @return subview with different dimensions + */ + template + OSDK_FN ArrayView()> subview(Args... idx) const { + constexpr int N = subview_dim(); + static_assert(N > 0, "Ran out of dimensions to subview"); + +#ifndef __CUDA_ARCH__ + if (impl::subview_oob_check(shape, idx...)) + throw std::invalid_argument( + "ArrayView cannot subview over the " + "shape limits"); +#endif + + T* ptr = + data_ + impl::strided_index(strides, impl::range_or_idx(idx)...); + int32_t new_shape[N]; + int32_t new_strides[N]; + impl::range_args_reshape(shape, new_shape, idx...); + impl::range_args_restride(strides, new_strides, idx...); + return ArrayView(ptr, new_shape, new_strides); + } + + /** + * Reshape array view to a different set of dimensions + * + * @throw std::invalid_argument on trying to reshape a sparse ArrayView + * @throw std::invalid_argument on flattened dimension size not matching + * the original view size + * + * @param[in] dims new dimensions + * + * @return reshaped ArrayView with new dimensions + */ + template + ArrayView reshape(Args... dims) const { + if (sparse()) { + throw std::invalid_argument( + "ArrayView: cannot reshape sparse views"); + } + + if (impl::product{}(dims...) != shape[0] * strides[0]) { + throw std::invalid_argument( + "ArrayView: cannot reshape due to size mismatch"); + } + + return ArrayView(data_, + {static_cast(dims)...}); + } + + /** + * Check if the ArrayView is not contiguous + * + * @return true if sparse + */ + OSDK_FN bool sparse() const { + bool dense = strides[Dim - 1] == 1; + for (int i = 1; i < Dim; ++i) { + dense = dense && (strides[i - 1] == strides[i] * shape[i]); + } + return !dense; + } + + /** + * Iterator functions below return basic pointers to data, which only works + * if the current array is not sparse. + * + * Because we have no way of throwing if the array is sparse, these are + * explicitly host side at the moment, until we get to implementing + * sparse iterators + * + * @return pointer to data + */ + const T* begin() const { + if (sparse()) + throw std::logic_error( + "ArrayView iterators not supported for " + "sparse views"); + return data_; + } + + // @copydoc begin() + const T* end() const { + if (sparse()) + throw std::logic_error( + "ArrayView iterators not supported " + "for sparse views"); + return data_ + shape[0] * strides[0]; + } + + // @copydoc begin() + T* begin() { + return const_cast(const_cast(this)->begin()); + } + + // @copydoc begin() + T* end() { + return const_cast(const_cast(this)->end()); + } + + /** + * Get the underlying data pointer. + * + * @return pointer to data + */ + const T* data() const { return data_; } + + // @copydoc data() + T* data() { return data_; } + + private: + friend class ArrayView; + + /** + * Internal indexable based constructor + * + * host only due to index subscript + */ + template + ArrayView(T* ptr, const Indexable1& _shape, const Indexable2& _strides, + impl::row_pack) + : data_(ptr), + strides{static_cast(_strides[I])...}, + shape{static_cast(_shape[I])...} {} + + /** + * Internal iterator based constructor + * + * host only due to iterators + */ + template + ArrayView(T* ptr, IterT first, IterT last, impl::row_pack, + impl::row_pack) + : data_(ptr), + strides{ + impl::product{}(last, impl::index_row{})..., + 1}, + shape{(void(I), static_cast(*first++))...} {} + + /** + * Internal pointer pair based constructor + */ + template + OSDK_FN ArrayView(T* ptr, IntT* first, IntT* last, impl::row_pack, + impl::row_pack) + : data_(ptr), + strides{ + impl::product{}(last, impl::index_row{})..., + 1}, + shape{(void(I), static_cast(*first++))...} { + (void)last; // silence an unused variable warning due to a compiler bug + } + + /** + * Internal C array based constructor + */ + template + OSDK_FN ArrayView(T* ptr, const int32_t (&shape)[Dim], + const int32_t (&strides)[Dim], impl::row_pack) + : data_(ptr), strides{strides[I]...}, shape{shape[I]...} {} +}; + +} // namespace ouster diff --git a/ouster_client/include/ouster/client.h b/ouster_client/include/ouster/client.h index d4c61cb4..36c26dae 100644 --- a/ouster_client/include/ouster/client.h +++ b/ouster_client/include/ouster/client.h @@ -32,7 +32,7 @@ enum client_state { }; /** Minimum supported version. */ -const util::version min_version = {1, 12, 0}; +const util::version min_version = {1, 12, 0, "", "", "", ""}; /** * Initializes and configures ouster_client logs. This method should be invoked @@ -229,13 +229,11 @@ bool read_imu_packet(const client& cli, ImuPacket& packet); * * @param[in] cli client returned by init_client associated with the connection. * @param[in] timeout_sec how long to wait for the sensor to initialize. - * @param[in] legacy_format whether to use legacy format of metadata output. * * @return a text blob of metadata parseable into a sensor_info struct. */ -std::string get_metadata(client& cli, - int timeout_sec = DEFAULT_HTTP_REQUEST_TIMEOUT_SECONDS, - bool legacy_format = false); +std::string get_metadata( + client& cli, int timeout_sec = DEFAULT_HTTP_REQUEST_TIMEOUT_SECONDS); /** * Get sensor config from the sensor. diff --git a/ouster_client/include/ouster/field.h b/ouster_client/include/ouster/field.h new file mode 100644 index 00000000..e579965c --- /dev/null +++ b/ouster_client/include/ouster/field.h @@ -0,0 +1,747 @@ +/** + * Copyright (c) 2024, Ouster, Inc. + * All rights reserved. + */ + +#pragma once + +#include // for size_t since gcc-12 + +#include +#include +#include +#include +#include +#include +#include + +#include "ouster/array_view.h" +#include "ouster/types.h" + +namespace ouster { + +namespace impl { + +/** + * Calculates vector of stride offsets + * + * @param[in] shape vector of array dimensions + * + * @return vector of stride offsets + */ +std::vector calculate_strides(const std::vector& shape); + +} // namespace impl + +namespace sensor { +namespace impl { + +// clang-format off + +template int type_size() { return sizeof(T); } +template <> int type_size(); + +template size_t type_hash() { return typeid(T).hash_code(); } +template <> size_t type_hash(); + +template +ChanFieldType type_cft() { return ChanFieldType::UNREGISTERED; } +template <> ChanFieldType type_cft(); +template <> ChanFieldType type_cft(); +template <> ChanFieldType type_cft(); +template <> ChanFieldType type_cft(); +template <> ChanFieldType type_cft(); +template <> ChanFieldType type_cft(); +template <> ChanFieldType type_cft(); +template <> ChanFieldType type_cft(); +template <> ChanFieldType type_cft(); +template <> ChanFieldType type_cft(); +template <> ChanFieldType type_cft(); + +// clang-format on + +} // namespace impl +} // namespace sensor + +/** + * Helper struct used by FieldView and Field to describe field contents. + * Unlike FieldType this fully describes a Field's dimensions rather than + * abstract away the lidar width and height or packet count. + */ +struct FieldDescriptor { + /** + * type hash of the described field + */ + size_t type; + + /** + * size in bytes of the described field + */ + size_t bytes; + + // TODO: ideally we need something like llvm::SmallVector here -- Tim T. + + /** + * vector of array dimensions of the described field, if present + */ + std::vector shape; + + /** + * vector of stride offsets of the described field, if present + */ + std::vector strides; + + /** + * Get type hash + * + * warning: different platforms produce different values + * + * @return hash value + */ + template + static size_t type_hash() { + using Type = typename std::remove_cv::type; + return sensor::impl::type_hash(); + } + + /** + * Get array size in elements, or 1 if shape is not present + * + * @return size in elements + */ + size_t size() const; + + /** + * Get size of the underlying type, in bytes + * + * @return type size in bytes + */ + int element_size() const; + + /** + * Get type tag, if can be translated to ChanFieldType, otherwise + * returns ChanFieldType::UNREGISTERED + * + * @return ChanFieldType + */ + sensor::ChanFieldType tag() const; + + bool operator==(const FieldDescriptor& other) const noexcept { + return type == other.type && bytes == other.bytes && + shape == other.shape && strides == other.strides; + } + + /** + * Swaps descriptors + */ + void swap(FieldDescriptor& other); + + /** + * Check if the type is eligible for conversion + * + * @return true if eligible, otherwise false + */ + template + bool eligible_type() const { + // TODO: reinstate upon c++17 -- Tim T. + if /*constexpr*/ ( + std::is_same::type>::value) { + return true; + } + return !type || type_hash() == type; + } + + /** + * Check if descriptor types are compatible + * + * @return true if compatible, otherwise false + */ + bool is_type_compatible(const FieldDescriptor& other) const noexcept; + + /** + * Return number of dimensions of the described field + * + * @return number of dimensions + */ + size_t ndim() const noexcept; + + // Factory functions + + /** + * Get a field descriptor for a chunk of typed memory + * + * useful when storing arbitrary sized structs + * + * @tparam T type of memory to be stored; this gets used by safety checks + * @param[in] bytes number of bytes in memory + * + * @return FieldDescriptor + */ + template + static FieldDescriptor memory(size_t bytes) { + return {type_hash(), bytes, {}, {}}; + } + + /** + * Get a field descriptor for an array + * + * @tparam T array type + * @param[in] shape vector of array dimensions + * + * @return FieldDescriptor + */ + template > + static FieldDescriptor array(const ContainerT& shape) { + static_assert(!std::is_same::value, + "FieldDescriptor::array is disallowed, use " + "FieldDescriptor::memory() instead"); + size_t total = std::accumulate(shape.begin(), shape.end(), size_t{1}, + std::multiplies{}); + size_t bytes = sizeof(T) * total; + + if (!bytes) + throw std::invalid_argument( + "failed creating array descriptor, one " + "of dimensions is zero"); + + return {type_hash(), bytes, shape, impl::calculate_strides(shape)}; + } + + /** + * Get a field descriptor for an array + * + * @param[in] tag tag of array type + * @param[in] shape vector of array dimensions + * + * @return FieldDescriptor + */ + template > + static FieldDescriptor array(sensor::ChanFieldType tag, + const ContainerT& shape) { + switch (tag) { + case sensor::ChanFieldType::UINT8: + return array(shape); + case sensor::ChanFieldType::UINT16: + return array(shape); + case sensor::ChanFieldType::UINT32: + return array(shape); + case sensor::ChanFieldType::UINT64: + return array(shape); + case sensor::ChanFieldType::INT8: + return array(shape); + case sensor::ChanFieldType::INT16: + return array(shape); + case sensor::ChanFieldType::INT32: + return array(shape); + case sensor::ChanFieldType::INT64: + return array(shape); + case sensor::ChanFieldType::FLOAT32: + return array(shape); + case sensor::ChanFieldType::FLOAT64: + return array(shape); + default: + throw std::invalid_argument( + "fd_array: unsupported ChanFieldType"); + } + } +}; + +/** + * Parameter pack shorthand for FieldDescriptor::array + * + * @return FieldDescriptor + */ +template +auto fd_array(Args&&... args) -> FieldDescriptor { + return FieldDescriptor::array({static_cast(args)...}); +} + +// @copydoc fd_array() +template +auto fd_array(sensor::ChanFieldType tag, Args&&... args) -> FieldDescriptor { + return FieldDescriptor::array(tag, {static_cast(args)...}); +} + +/** + * Non-owning wrapper over a memory pointer that allows for type safe + * conversion to typed pointer, eigen array or ArrayView + */ +class FieldView { + protected: + void* ptr_; + FieldDescriptor desc_; + + public: + /** Default constructor for empty FieldView */ + FieldView() noexcept; + + /** + * Initialize FieldView with a pointer and a descriptor + * + * @param[in] ptr memory pointer + * @param[in] desc field descriptor + */ + FieldView(void* ptr, const FieldDescriptor& desc); + + /** + * Returns arbitrary pointer type + * + * @throw std::invalid_argument on type mismatch unless FieldView was + * constructed typeless (with type void) or dereference type is void* + * + * @tparam T pointer type + * @return typed pointer to the memory + */ + template + T* get() { + return const_cast(const_cast(*this).get()); + } + + // @copydoc get() + template + const T* get() const { + if (!desc_.eligible_type()) { + throw std::invalid_argument( + "FieldView: ineligible dereference type"); + } + return reinterpret_cast(ptr_); + } + + /** + * Arbitrary type ptr conversion + + * @throw std::invalid_argument on type mismatch unless FieldView was + * constructed with void ptr or the requested ptr type is void* + */ + template + operator T*() { + return get(); + } + + /** + * Arbitrary type const ptr conversion + + * @throw std::invalid_argument on type mismatch unless FieldView was + * constructed with void ptr or the requested ptr type is void* + */ + template + operator const T*() const { + return get(); + } + + /** + * Arbitrary type ArrayView conversion + + * @throw std::invalid_argument on type mismatch + * @throw std::invalid_argument on dimensional shape mismatch + */ + template + operator ArrayView() { + if (desc_.ndim() != Dim) { + throw std::invalid_argument( + "FieldView: ArrayView conversion failed due to dimension " + "mismatch"); + } + + return ArrayView(get(), desc_.shape, desc_.strides); + } + + /** + * Arbitrary type const ArrayView conversion + + * @throw std::invalid_argument on type mismatch + * @throw std::invalid_argument on dimensional shape mismatch + */ + template + operator ConstArrayView() const { + if (desc_.ndim() != Dim) { + throw std::invalid_argument( + "FieldView: ArrayView conversion failed due to dimension " + "mismatch"); + } + + return ConstArrayView(get(), desc_.shape, desc_.strides); + } + + /** + * Arbitrary type Eigen 2D array conversion + * + * @throw std::invalid_argument on type mismatch + * @throw std::invalid_argument on dimensional shape mismatch + */ + template + operator Eigen::Ref>() { + if (desc_.ndim() != 2) { + throw std::invalid_argument( + "Field: Eigen array conversion failed due to dimension " + "mismatch"); + } + + if (sparse()) { + throw std::invalid_argument( + "Field: Cannot convert sparse view to dense Eigen array"); + } + + Eigen::Index h = shape()[0], w = shape()[1]; + return Eigen::Map>{get(), h, w}; + } + + /** + * Arbitrary type const Eigen 2D array conversion + * + * @throw std::invalid_argument on type mismatch + * @throw std::invalid_argument on dimensional shape mismatch + */ + template + operator Eigen::Ref>() const { + if (desc_.ndim() != 2) { + throw std::invalid_argument( + "Field: Eigen array conversion failed due to dimension " + "mismatch"); + } + + if (sparse()) { + throw std::invalid_argument( + "Field: Cannot convert sparse view to dense Eigen array"); + } + + Eigen::Index h = shape()[0], w = shape()[1]; + return Eigen::Map>{get(), h, w}; + } + + /** + * Arbitrary type const Eigen 1D array conversion + * + * @throw std::invalid_argument on type mismatch + * @throw std::invalid_argument on dimensional shape mismatch + */ + template + operator Eigen::Ref>() { + if (desc_.ndim() != 1) { + throw std::invalid_argument( + "Field: Eigen array conversion failed due to dimension " + "mismatch"); + } + + if (sparse()) { + throw std::invalid_argument( + "Field: Cannot convert sparse view to dense Eigen array"); + } + + Eigen::Index w = shape()[0]; + return Eigen::Map>{get(), w}; + } + + /** + * Arbitrary type const Eigen 1D array conversion + * + * @throw std::invalid_argument on type mismatch + * @throw std::invalid_argument on dimensional shape mismatch + */ + template + operator Eigen::Ref>() const { + if (desc_.ndim() != 1) { + throw std::invalid_argument( + "Field: Eigen array conversion failed due to dimension " + "mismatch"); + } + + if (sparse()) { + throw std::invalid_argument( + "Field: Cannot convert sparse view to dense Eigen array"); + } + + Eigen::Index w = shape()[0]; + return Eigen::Map>{get(), + w}; + } + + /** + * Bool conversion + * + * @return true if FieldView is owning a resource + */ + explicit operator bool() const noexcept; + + /** + * Get a subview + * + * Operates similarly to numpy ndarray bracket operator, returning a sliced, + * potentially sparse subview + * + * The following two snippets are functionally equivalent + * \code + * std::vector data(100*100*100); + * FieldView view(data.data(), fd_array(100, 100, 100)); + * // get a slice of all elements in the first dimension with second + * // and third pinned to 10 and 20 respectively + * FieldView subview = view.subview(keep(), 10, 20); + * \endcode + * + * \code + * import numpy as np + * arr = np.ndarray(shape=(100,100,100), dtype=np.int32) + * subview = arr[:,10,20] + * \endcode + * + * @param[in] idx parameter pack of int indices or idx_range (keep()) + * + * @return FieldView subview + */ + template + FieldView subview(Args... idx) const { + auto dim = desc().ndim(); + auto subview_dim = + dim - sizeof...(Args) + impl::count_n_ranges::value; + + if (subview_dim <= 0) + throw std::invalid_argument( + "FieldView: ran out of dimensions to subview"); + + if (impl::subview_oob_check(shape(), idx...)) { + throw std::invalid_argument( + "FieldView cannot subview over the shape limits"); + } + + char* ptr = + reinterpret_cast(ptr_) + + impl::strided_index(desc().strides, impl::range_or_idx(idx)...) * + desc().element_size(); + + auto new_desc = FieldDescriptor{}; + new_desc.type = desc().type; + new_desc.shape = impl::range_args_reshape(desc().shape, idx...); + new_desc.strides = impl::range_args_restride(desc().strides, idx...); + new_desc.bytes = + std::accumulate(new_desc.shape.begin(), new_desc.shape.end(), 1, + std::multiplies{}) * + desc().element_size(); + + return {reinterpret_cast(ptr), new_desc}; + } + + /** + * Reshape field view to a different set of dimensions + * + * @throw std::invalid_argument on trying to reshape a sparse FieldView + * @throw std::invalid_argument on flattened dimension size not matching + * the original view size + * + * @param[in] dims new dimensions + * + * @return reshaped FieldView with new dimensions + */ + template + FieldView reshape(Args... dims) const { + if (sparse()) { + throw std::invalid_argument( + "FieldView: cannot reshape sparse views"); + } + + if (impl::product{}(dims...) != size()) { + throw std::invalid_argument( + "ArrayView: cannot reshape due to size mismatch"); + } + + auto new_desc = FieldDescriptor{}; + new_desc.type = desc().type; + new_desc.bytes = desc().bytes; + new_desc.shape = std::vector{static_cast(dims)...}; + new_desc.strides = impl::calculate_strides(new_desc.shape); + return {const_cast(ptr_), new_desc}; + } + + /** + * Returns the number of allocated bytes in memory + * + * @return size in bytes + */ + size_t bytes() const noexcept; + + /** + * Returns size in elements, or 1 if field is not an array + * + * @return size in elements + */ + size_t size() const; + + /** + * returns true if FieldView matches descriptor + * + * @param[in] d descriptor to check + * + * @return true if matched, otherwise false + */ + bool matches(const FieldDescriptor& d) const noexcept; + + /** + * Get descriptor of the underlying memory + * + * @return FieldDescriptor + */ + const FieldDescriptor& desc() const; + + /** + * Get shape of the stored array, if present + * + * shorthand for `desc().shape` + * + * @return vector of dimensions + */ + const std::vector& shape() const; + + /** + * Get type tag, if applicable + * + * @return ChanFieldType + */ + sensor::ChanFieldType tag() const; + + /** + * Check if the FieldView is not contiguous + * + * @return true if sparse + */ + bool sparse() const; +}; + +/** + * Classes of LidarScan fields + */ +enum class FieldClass { + /** + * Corresponds to fields of (height, width, ...) dimensions + */ + PIXEL_FIELD = 1, + + /** + * Corresponds to fields that have first dimension equal to width + */ + COLUMN_FIELD = 2, + + /** + * Corresponds to fields that have first dimension equal to number + * of packets necessary to construct a complete LidarScan + */ + PACKET_FIELD = 3, + + /** + * Corresponds to fields of any dimension associated with the scan + * as a whole rather than any pixel, column or packet + */ + SCAN_FIELD = 4, +}; + +/** + * Get string representation of singular FieldClass flag + * + * @param[in] f flag to get the string representation for. + * + * @return string representation of the FieldClass, or "UNKNOWN". + */ +std::string to_string(FieldClass f); + +/** + * RAII memory-owning container for arbitrary typed and sized arrays and POD + * structs with optional type checking + * + * For usage examples, check unit tests + */ +class Field : public FieldView { + protected: + FieldClass class_; + + public: + /** Default constructor, representing invalid Field */ + Field() noexcept; + + /** Field destructor */ + ~Field(); + + /** + * Constructs Field from FieldDescriptor + * + * @param[in] desc FieldDescriptor + * @param[in] field_class FieldClass + */ + Field(const FieldDescriptor& desc, FieldClass field_class = {}); + + /** + * Copy constructor + * + * @param[in] other Field to copy + */ + Field(const Field& other); + + /** + * Copy assignment constructor + * + * @param[in] other Field to copy + */ + Field& operator=(const Field& other); + + /** + * Move constructor + * + * @param[in] other Field to steal resource from + */ + Field(Field&& other) noexcept; + + /** + * Move assignment constructor + * + * @param[in] other Field to steal resource from + */ + Field& operator=(Field&& other) noexcept; + + /** + * Get field class + * + * @return FieldClass + */ + FieldClass field_class() const; + + /** + * Swaps two Fields + * + * @param[in] other Field to swap resources with + */ + void swap(Field& other) noexcept; + + /** + * Equality operator + * + * @return true if type, shape and memory contents are equal to other + */ + bool operator==(const Field& other) const; +}; + +/** + * Acquire a uintXX_t reinterpreted view of the matching type size. + * Useful for memory related operations like parsing and compression. + * + * WARNING: reinterprets the type skipping type safety checks, exercise caution + * + * @throw std::invalid_argument if `other` is not an array view + * @throw std::invalid_argument if `other` is an array view of custom POD types + * that do not match any uintXX_t dimensions + * + * @param[in] other view to interpret + * + * @return reinterpreted view of uint8_t, uint16_t, uint32_t or uint64_t type + */ +FieldView uint_view(const FieldView& other); + +} // namespace ouster + +namespace std { + +/** + * std::swap overload, used by some std algorithms + * + * @param[in] a Field to swap with b + * @param[in] b Field to swap with a + */ +void swap(ouster::Field& a, ouster::Field& b); + +} // namespace std diff --git a/ouster_client/include/ouster/impl/cuda_macros.h b/ouster_client/include/ouster/impl/cuda_macros.h new file mode 100644 index 00000000..7bf93784 --- /dev/null +++ b/ouster_client/include/ouster/impl/cuda_macros.h @@ -0,0 +1,13 @@ +#pragma once + +#ifdef __CUDACC__ +#define OSDK_FN __device__ __host__ +#define OSDK_FN_HOST __host__ +#define OSDK_FN_DEVICE __device__ +#define RESTRICT __restrict__ +#else +#define OSDK_FN +#define OSDK_FN_HOST +#define OSDK_FN_DEVICE +#define RESTRICT +#endif diff --git a/ouster_client/include/ouster/impl/idx_range.h b/ouster_client/include/ouster/impl/idx_range.h new file mode 100644 index 00000000..ad14823d --- /dev/null +++ b/ouster_client/include/ouster/impl/idx_range.h @@ -0,0 +1,155 @@ +/** + * Copyright (c) 2024, Ouster, Inc. + * All rights reserved. + */ + +#pragma once + +#include +#include +#include + +#include "ouster/impl/cuda_macros.h" + +namespace ouster { +namespace impl { + +struct idx_range { + int start = 0; + int end = 0; +}; + +template +struct is_idx_range_t { + static const int v = 0; +}; + +template <> +struct is_idx_range_t { + static const int v = 1; +}; + +template +struct count_n_ranges { + static constexpr int value = + count_n_ranges::value + count_n_ranges::value; +}; + +template +struct count_n_ranges { + static constexpr int value = is_idx_range_t>::v; +}; + +template +OSDK_FN constexpr int is_idx_range(T) { + return is_idx_range_t::v; +} + +template +OSDK_FN int range_or_idx(T idx) { + return idx; +} + +inline OSDK_FN int range_or_idx(idx_range e) { return e.start; } + +template +OSDK_FN std::enable_if_t::v, T> range_replace_dim(T dim, U) { + return dim; +} + +template +OSDK_FN T range_replace_dim(T dim, idx_range e) { + if (e.end == 0) return dim; // default empty idx_range + +#ifndef __CUDA_ARCH__ + if (e.end <= e.start || e.end > static_cast(dim)) { + throw std::runtime_error("misshaped idx_range reshape"); + } +#endif + + return e.end - e.start; +} + +/** + * Shuffles arguments whether arg is an idx_range or not: + * skip array for args that are values, but retain for ranges + */ +template +OSDK_FN void range_args_restride(const int32_t (&strides)[N], + int32_t (&new_strides)[M], Args...) { + static_assert(M == N - sizeof...(Args) + count_n_ranges::value, + "shape mismatch"); + const bool skip[N] = {!static_cast(is_idx_range_t::v)...}; + + int j = 0; + for (int i = 0; i < N; ++i) { + if (!skip[i]) new_strides[j++] = strides[i]; + } +} + +/** + * Same as above, but reworks shapes for shaped ranges + */ +template +OSDK_FN void range_args_reshape(const int32_t (&shape)[N], + int32_t (&new_shape)[M], Args... args) { + int i = 0; + int32_t ranged_shape[N] = {range_replace_dim(shape[i++], args)...}; + for (; i < N; ++i) { + ranged_shape[i] = shape[i]; + } + + range_args_restride(ranged_shape, new_shape, args...); +} + +/** + * Shuffles arguments whether arg is an idx_range or not: + * skip array for args that are values, but retain for ranges + * + * std::vector version + */ +template +OSDK_FN_HOST std::vector range_args_restride(const std::vector& strides, + Args...) { + std::vector out; + + int i = 0; + + for (bool skip : {!is_idx_range_t::v...}) { + if (!skip) out.push_back(strides[i]); + ++i; + } + + for (int end = strides.size(); i < end; ++i) { + out.push_back(strides[i]); + } + + return out; +} + +/** + * Same as above, but reworks shapes for shaped ranges + * + * std::vector version + */ +template +OSDK_FN_HOST std::vector range_args_reshape(const std::vector& shape, + Args... args) { + int i = 0; + + std::vector ranged_shape = {range_replace_dim(shape[i++], args)...}; + for (int end = shape.size(); i < end; ++i) { + ranged_shape.push_back(shape[i]); + } + + return range_args_restride(ranged_shape, args...); +} + +} // namespace impl + +OSDK_FN +inline impl::idx_range keep() { return {0, 0}; } +OSDK_FN +inline impl::idx_range keep(int start, int end) { return {start, end}; } + +} // namespace ouster diff --git a/ouster_client/include/ouster/impl/lidar_scan_impl.h b/ouster_client/include/ouster/impl/lidar_scan_impl.h index 9224b882..08739bce 100644 --- a/ouster_client/include/ouster/impl/lidar_scan_impl.h +++ b/ouster_client/include/ouster/impl/lidar_scan_impl.h @@ -9,6 +9,7 @@ #include #include +#include "ouster/field.h" #include "ouster/impl/packet_writer.h" #include "ouster/lidar_scan.h" #include "ouster/types.h" @@ -41,201 +42,40 @@ struct FieldTag { static constexpr ChanFieldType tag = ChanFieldType::UINT64; }; -/* - * Tagged union for LidarScan fields - */ -struct FieldSlot { - ChanFieldType tag; - union { - img_t f8; - img_t f16; - img_t f32; - img_t f64; - }; - - FieldSlot(ChanFieldType t, size_t w, size_t h) : tag{t} { - switch (t) { - case ChanFieldType::VOID: - break; - case ChanFieldType::UINT8: - new (&f8) img_t{h, w}; - f8.setZero(); - break; - case ChanFieldType::UINT16: - new (&f16) img_t{h, w}; - f16.setZero(); - break; - case ChanFieldType::UINT32: - new (&f32) img_t{h, w}; - f32.setZero(); - break; - case ChanFieldType::UINT64: - new (&f64) img_t{h, w}; - f64.setZero(); - break; - } - } - - FieldSlot() : FieldSlot{ChanFieldType::VOID, 0, 0} {}; - - ~FieldSlot() { clear(); } - - FieldSlot(const FieldSlot& other) { - switch (other.tag) { - case ChanFieldType::VOID: - break; - case ChanFieldType::UINT8: - new (&f8) img_t{other.f8}; - break; - case ChanFieldType::UINT16: - new (&f16) img_t{other.f16}; - break; - case ChanFieldType::UINT32: - new (&f32) img_t{other.f32}; - break; - case ChanFieldType::UINT64: - new (&f64) img_t{other.f64}; - break; - } - tag = other.tag; - } - - FieldSlot(FieldSlot&& other) { set_from(other); } - - FieldSlot& operator=(FieldSlot other) { - clear(); - set_from(other); - return *this; - } - - template - Eigen::Ref> get() { - if (tag == FieldTag::tag) - return get_unsafe(); - else - throw std::invalid_argument("Accessed field at wrong type"); - } - - template - Eigen::Ref> get() const { - if (tag == FieldTag::tag) - return get_unsafe(); - else - throw std::invalid_argument("Accessed field at wrong type"); - } - - friend bool operator==(const FieldSlot& l, const FieldSlot& r) { - if (l.tag != r.tag) return false; - switch (l.tag) { - case ChanFieldType::VOID: - return true; - case ChanFieldType::UINT8: - return (l.f8 == r.f8).all(); - case ChanFieldType::UINT16: - return (l.f16 == r.f16).all(); - case ChanFieldType::UINT32: - return (l.f32 == r.f32).all(); - case ChanFieldType::UINT64: - return (l.f64 == r.f64).all(); - default: - assert(false); - } - // unreachable, appease older gcc - return false; - } - - private: - void set_from(FieldSlot& other) { - switch (other.tag) { - case ChanFieldType::VOID: - break; - case ChanFieldType::UINT8: - new (&f8) img_t{std::move(other.f8)}; - break; - case ChanFieldType::UINT16: - new (&f16) img_t{std::move(other.f16)}; - break; - case ChanFieldType::UINT32: - new (&f32) img_t{std::move(other.f32)}; - break; - case ChanFieldType::UINT64: - new (&f64) img_t{std::move(other.f64)}; - break; - } - tag = other.tag; - other.clear(); - } - - void clear() { - switch (tag) { - case ChanFieldType::VOID: - break; - case ChanFieldType::UINT8: - f8.~img_t(); - break; - case ChanFieldType::UINT16: - f16.~img_t(); - break; - case ChanFieldType::UINT32: - f32.~img_t(); - break; - case ChanFieldType::UINT64: - f64.~img_t(); - break; - } - tag = ChanFieldType::VOID; - } - - template - Eigen::Ref> get_unsafe(); - - template - Eigen::Ref> get_unsafe() const; -}; - -template <> -inline Eigen::Ref> FieldSlot::get_unsafe() { - return f8; -} - template <> -inline Eigen::Ref> FieldSlot::get_unsafe() { - return f16; -} - -template <> -inline Eigen::Ref> FieldSlot::get_unsafe() { - return f32; -} +struct FieldTag { + static constexpr ChanFieldType tag = ChanFieldType::INT8; +}; template <> -inline Eigen::Ref> FieldSlot::get_unsafe() { - return f64; -} +struct FieldTag { + static constexpr ChanFieldType tag = ChanFieldType::INT16; +}; template <> -inline Eigen::Ref> FieldSlot::get_unsafe() const { - return f8; -} +struct FieldTag { + static constexpr ChanFieldType tag = ChanFieldType::INT32; +}; template <> -inline Eigen::Ref> FieldSlot::get_unsafe() const { - return f16; -} +struct FieldTag { + static constexpr ChanFieldType tag = ChanFieldType::INT64; +}; template <> -inline Eigen::Ref> FieldSlot::get_unsafe() const { - return f32; -} +struct FieldTag { + static constexpr ChanFieldType tag = ChanFieldType::FLOAT32; +}; template <> -inline Eigen::Ref> FieldSlot::get_unsafe() const { - return f64; -} +struct FieldTag { + static constexpr ChanFieldType tag = ChanFieldType::FLOAT64; +}; /* * Call a generic operation op(f, Args..) with the type parameter T having - * the correct (dynamic) field type for the LidarScan channel field f + * the correct (dynamic) field type for the Field `field` + * NOTE: requested field must be two dimensional * Example code for the operation: * \code * struct print_field_size { @@ -247,23 +87,96 @@ inline Eigen::Ref> FieldSlot::get_unsafe() const { * }; * \endcode */ -template -void visit_field(SCAN&& ls, sensor::ChanField f, OP&& op, Args&&... args) { - switch (ls.field_type(f)) { +template +void visit_field_2d(FieldView& field, OP&& op, Args&&... args) { + switch (field.tag()) { + case sensor::ChanFieldType::UINT8: + op.template operator()(Eigen::Ref>(field), + std::forward(args)...); + break; + case sensor::ChanFieldType::UINT16: + op.template operator()(Eigen::Ref>(field), + std::forward(args)...); + break; + case sensor::ChanFieldType::UINT32: + op.template operator()(Eigen::Ref>(field), + std::forward(args)...); + break; + case sensor::ChanFieldType::UINT64: + op.template operator()(Eigen::Ref>(field), + std::forward(args)...); + break; + case sensor::ChanFieldType::INT8: + op.template operator()(Eigen::Ref>(field), + std::forward(args)...); + break; + case sensor::ChanFieldType::INT16: + op.template operator()(Eigen::Ref>(field), + std::forward(args)...); + break; + case sensor::ChanFieldType::INT32: + op.template operator()(Eigen::Ref>(field), + std::forward(args)...); + break; + case sensor::ChanFieldType::INT64: + op.template operator()(Eigen::Ref>(field), + std::forward(args)...); + break; + case sensor::ChanFieldType::FLOAT32: + op.template operator()(Eigen::Ref>(field), + std::forward(args)...); + break; + case sensor::ChanFieldType::FLOAT64: + op.template operator()(Eigen::Ref>(field), + std::forward(args)...); + break; + default: + throw std::invalid_argument("Invalid field for LidarScan"); + } +} + +// @copydoc visit_field_2d() +template +void visit_field_2d(const FieldView& field, OP&& op, Args&&... args) { + switch (field.tag()) { case sensor::ChanFieldType::UINT8: - op.template operator()(ls.template field(f), + op.template operator()(Eigen::Ref>(field), std::forward(args)...); break; case sensor::ChanFieldType::UINT16: - op.template operator()(ls.template field(f), + op.template operator()(Eigen::Ref>(field), std::forward(args)...); break; case sensor::ChanFieldType::UINT32: - op.template operator()(ls.template field(f), + op.template operator()(Eigen::Ref>(field), std::forward(args)...); break; case sensor::ChanFieldType::UINT64: - op.template operator()(ls.template field(f), + op.template operator()(Eigen::Ref>(field), + std::forward(args)...); + break; + case sensor::ChanFieldType::INT8: + op.template operator()(Eigen::Ref>(field), + std::forward(args)...); + break; + case sensor::ChanFieldType::INT16: + op.template operator()(Eigen::Ref>(field), + std::forward(args)...); + break; + case sensor::ChanFieldType::INT32: + op.template operator()(Eigen::Ref>(field), + std::forward(args)...); + break; + case sensor::ChanFieldType::INT64: + op.template operator()(Eigen::Ref>(field), + std::forward(args)...); + break; + case sensor::ChanFieldType::FLOAT32: + op.template operator()(Eigen::Ref>(field), + std::forward(args)...); + break; + case sensor::ChanFieldType::FLOAT64: + op.template operator()(Eigen::Ref>(field), std::forward(args)...); break; default: @@ -271,17 +184,58 @@ void visit_field(SCAN&& ls, sensor::ChanField f, OP&& op, Args&&... args) { } } +/* + * Call a generic operation op(f, Args..) with the type parameter T having + * the correct (dynamic) field type for the LidarScan channel field f + * NOTE: requested field must be two dimensional + * Example code for the operation: + * \code + * struct print_field_size { + * template + * void operator()(Eigen::Ref> field) { + * std::cout << "Rows: " + field.rows() << std::endl; + * std::cout << "Cols: " + field.cols() << std::endl; + * } + * }; + * \endcode + */ +template +void visit_field(SCAN&& ls, const std::string& name, OP&& op, Args&&... args) { + // throw early as python downstream expects ValueError + if (!ls.has_field(name)) + throw std::invalid_argument("Invalid field for LidarScan"); + + visit_field_2d(ls.field(name), std::forward(op), + std::forward(args)...); +} + /* * Call a generic operation op(f, Args...) for each field of the lidar scan * with type parameter T having the correct field type */ template -void foreach_field(SCAN&& ls, OP&& op, Args&&... args) { +[[deprecated("Use either ls.fields() or foreach_channel_field instead")]] void +foreach_field(SCAN&& ls, OP&& op, Args&&... args) { for (const auto& ft : ls) visit_field(std::forward(ls), ft.first, std::forward(op), ft.first, std::forward(args)...); } +/* + * Call a generic operation op(f, Args...) for each parsed channel field of + * the lidar scan with type parameter T having the correct field type + */ +template +void foreach_channel_field(SCAN&& ls, const sensor::packet_format& pf, OP&& op, + Args&&... args) { + for (const auto& ft : pf) { + if (ls.has_field(ft.first)) { + visit_field(ls, ft.first, std::forward(op), ft.first, + std::forward(args)...); + } + } +} + // Read LidarScan field and cast to the destination struct read_and_cast { template @@ -307,7 +261,7 @@ struct read_and_cast { struct copy_and_cast { template void operator()(Eigen::Ref> field_dest, const LidarScan& ls_source, - sensor::ChanField ls_source_field) { + const std::string& ls_source_field) { visit_field(ls_source, ls_source_field, read_and_cast(), field_dest); } }; @@ -343,7 +297,7 @@ template void scan_to_packets(const LidarScan& ls, const ouster::sensor::impl::packet_writer& pw, OutputItT iter, uint32_t init_id, uint64_t prod_sn) { - int total_packets = ls.packet_timestamp().size(); + size_t total_packets = ls.packet_timestamp().size(); auto columns_per_packet = pw.columns_per_packet; if (ls.w / columns_per_packet != total_packets) { @@ -353,14 +307,11 @@ void scan_to_packets(const LidarScan& ls, throw std::invalid_argument(err); } - using ouster::sensor::ChanField; using ouster::sensor::LidarPacket; - auto pack_field = [&pw](auto ref_field, ChanField i, LidarPacket& packet) { - // skip over RAW_HEADERS, RAW32_WORD* and CUSTOM* fields - if (i >= ChanField::RAW_HEADERS && i <= ChanField::CHAN_FIELD_MAX) - return; - + // TODO: switch to strings + auto pack_field = [&pw](auto ref_field, const std::string& i, + LidarPacket& packet) { pw.set_block(ref_field, i, packet.buf.data()); }; @@ -371,7 +322,7 @@ void scan_to_packets(const LidarScan& ls, auto frame_id = ls.frame_id; LidarPacket packet(pw.lidar_packet_size); - for (int p_id = 0; p_id < total_packets; ++p_id) { + for (size_t p_id = 0; p_id < total_packets; ++p_id) { uint8_t* lidar_buf = packet.buf.data(); std::memset(packet.buf.data(), 0, packet.buf.size()); packet.host_timestamp = ls.packet_timestamp()[p_id]; @@ -396,10 +347,11 @@ void scan_to_packets(const LidarScan& ls, // do not emit packet if ts == 0 and none of the columns are valid if (!any_valid && !packet.host_timestamp) continue; - foreach_field(ls, pack_field, packet); + foreach_channel_field(ls, pw, pack_field, packet); if (raw_headers_enabled(pw, ls)) { - visit_field(ls, ChanField::RAW_HEADERS, unpack_raw_headers, packet); + visit_field(ls, sensor::ChanField::RAW_HEADERS, unpack_raw_headers, + packet); } else if (pw.udp_profile_lidar != ouster::sensor::UDPProfileLidar::PROFILE_LIDAR_LEGACY) { uint32_t* ptr = reinterpret_cast(packet.buf.data() + diff --git a/ouster_client/src/logging.h b/ouster_client/include/ouster/impl/logging.h similarity index 82% rename from ouster_client/src/logging.h rename to ouster_client/include/ouster/impl/logging.h index 01d6b6b6..4ae9f197 100644 --- a/ouster_client/src/logging.h +++ b/ouster_client/include/ouster/impl/logging.h @@ -12,6 +12,8 @@ class Logger { spdlog::logger& get_logger(); + void configure_generic_sink(spdlog::sink_ptr sink, + const std::string& log_level); bool configure_stdout_sink(const std::string& log_level); bool configure_file_sink(const std::string& log_level, @@ -21,14 +23,9 @@ class Logger { private: Logger(); - void update_sink_and_log_level(spdlog::sink_ptr sink, - const std::string& log_level); - - private: static const std::string logger_name; std::unique_ptr logger_; }; - } // namespace impl spdlog::logger& logger(); diff --git a/ouster_client/include/ouster/impl/packet_writer.h b/ouster_client/include/ouster/impl/packet_writer.h index 63652fd4..af1cd61f 100644 --- a/ouster_client/include/ouster/impl/packet_writer.h +++ b/ouster_client/include/ouster/impl/packet_writer.h @@ -16,7 +16,7 @@ namespace impl { */ class packet_writer : public packet_format { template - void set_block_impl(Eigen::Ref> field, ChanField i, + void set_block_impl(Eigen::Ref> field, const std::string& i, uint8_t* lidar_buf) const; public: @@ -36,10 +36,10 @@ class packet_writer : public packet_format { void set_prod_sn(uint8_t* lidar_buf, uint64_t sn) const; template - void set_px(uint8_t* px_buf, ChanField i, T value) const; + void set_px(uint8_t* px_buf, const std::string& i, T value) const; template - void set_block(Eigen::Ref> field, ChanField i, + void set_block(Eigen::Ref> field, const std::string& i, uint8_t* lidar_buf) const; /** diff --git a/ouster_client/include/ouster/impl/profile_extension.h b/ouster_client/include/ouster/impl/profile_extension.h index 804e88c0..a405788a 100644 --- a/ouster_client/include/ouster/impl/profile_extension.h +++ b/ouster_client/include/ouster/impl/profile_extension.h @@ -26,7 +26,7 @@ struct FieldInfo { void add_custom_profile( int profile_nr, const std::string& name, - const std::vector>& fields, + const std::vector>& fields, size_t chan_data_size); } // namespace sensor diff --git a/ouster_client/include/ouster/lidar_scan.h b/ouster_client/include/ouster/lidar_scan.h index a2c95e11..b2dba26c 100644 --- a/ouster_client/include/ouster/lidar_scan.h +++ b/ouster_client/include/ouster/lidar_scan.h @@ -8,27 +8,93 @@ #include #include #include -#include #include #include +#include #include #include #include "ouster/defaults.h" +#include "ouster/field.h" #include "ouster/types.h" namespace ouster { -// forward declarations -namespace impl { -struct FieldSlot; -} +/* + * Description for a field that we desire in a lidar scan + */ +struct FieldType { + std::string name; ///< Name of the field + sensor::ChanFieldType element_type; ///< Type of field elements + std::vector extra_dims; ///< Additional dimensions of the field + FieldClass field_class = + FieldClass::PIXEL_FIELD; ///< Category of field, determines the + ///< first dimensions of the field + + /** + * Initialize a default FieldType with no name. + */ + FieldType(); + + /** + * Initialize a lidar scan from another with only the indicated fields. + * Casts, zero pads or removes fields from the original scan if necessary. + * + * @param[in] name_ Name of the field described by the type. + * @param[in] element_type_ Primitive type for elements in the field. + * @param[in] extra_dims_ Additional dimensions of the field + * @param[in] class_ Category of field, determins the first dimensions of + the field + */ + FieldType(const std::string& name_, sensor::ChanFieldType element_type_, + const std::vector extra_dims_ = {}, + FieldClass class_ = FieldClass::PIXEL_FIELD); + + /** + * Less than operated needed to allow sorting FieldTypes by name + * + * @param[in] other The FieldType to compare with + * + * @return if the name is "less than" the provided FieldType + */ + inline bool operator<(const FieldType& other) const { + return name < other.name; + } +}; + +/** + * Get string representation of a FieldType. + * + * @param[in] field_type The field type to get the string representation of. + * + * @return string representation of the FieldType. + */ +std::string to_string(const FieldType& field_type); + +/** + * Equality for FieldTypes. + * + * @param[in] a The first type to compare. + * @param[in] b The second type to compare. + * + * @return if a == b. + */ +bool operator==(const FieldType& a, const FieldType& b); + +/** + * Equality for FieldTypes. + * + * @param[in] a The first type to compare. + * @param[in] b The second type to compare. + * + * @return if a != b. + */ +bool operator!=(const FieldType& a, const FieldType& b); /** * Alias for the lidar scan field types */ -using LidarScanFieldTypes = - std::vector>; +using LidarScanFieldTypes = std::vector; /** * Data structure for efficient operations on aggregated lidar data. @@ -50,13 +116,14 @@ class LidarScan { using Points = Eigen::Array; private: - Header timestamp_; - Header packet_timestamp_; - Header measurement_id_; - Header status_; - std::vector pose_; - std::map fields_; - LidarScanFieldTypes field_types_; + std::unordered_map fields_; + + // Required special case "fields" + Field timestamp_; + Field measurement_id_; + Field status_; + Field packet_timestamp_; + Field pose_; LidarScan(size_t w, size_t h, LidarScanFieldTypes field_types, size_t columns_per_packet); @@ -68,7 +135,7 @@ class LidarScan { * @warning Members variables: use with caution, some of these will become * private. */ - std::ptrdiff_t w{0}; + size_t w{0}; /** * Pointer offsets to deal with strides. @@ -76,7 +143,12 @@ class LidarScan { * @warning Members variables: use with caution, some of these will become * private. */ - std::ptrdiff_t h{0}; + size_t h{0}; + + /** + * Number of columns contained in each packet making up the scan. + */ + size_t columns_per_packet_{DEFAULT_COLUMNS_PER_PACKET}; /** * Frame status - information from the packet header which corresponds to a @@ -95,10 +167,6 @@ class LidarScan { */ int64_t frame_id{-1}; - using FieldIter = - decltype(field_types_)::const_iterator; ///< An STL Iterator of the - ///< field types - /** The default constructor creates an invalid 0 x 0 scan. */ LidarScan(); @@ -156,6 +224,8 @@ class LidarScan { * Initialize a lidar scan from another with only the indicated fields. * Casts, zero pads or removes fields from the original scan if necessary. * + * @throw std::invalid_argument if field dimensions are incompatible + * * @param[in] other The other lidar scan to initialize from. * @param[in] fields Fields to have in new lidar scan. */ @@ -211,31 +281,102 @@ class LidarScan { /** * @copydoc ClientLidarScanField */ - template ::value, T>::type = 0> - Eigen::Ref> field(sensor::ChanField f); + template + Eigen::Ref> field(const std::string& f); /** * @copydoc ClientLidarScanField */ - template ::value, T>::type = 0> - Eigen::Ref> field(sensor::ChanField f) const; + template + Eigen::Ref> field(const std::string& f) const; + + /** + * @defgroup ClientLidarScanFieldString Access fields in a lidar scan + * Access a lidar data field. + * + * @param[in] name string key of the field to access + * + * @return Field reference of the requested field + */ + + /** + * @copydoc ClientLidarScanFieldString + */ + Field& field(const std::string& name); + + /** + * @copydoc ClientLidarScanFieldString + */ + const Field& field(const std::string& name) const; + + /** + * Check if a field exists + * + * @param[in] name string key of the field to check + * + * @return true if the lidar scan has the field, else false + */ + bool has_field(const std::string& name) const; + + /** + * Add a new zero-filled field to lidar scan. + * + * @throw std::invalid_argument if key duplicates a preexisting field, or + * if flags dimensional requirements are not met + * + * @param[in] name string key of the field to add + * @param[in] d descriptor of the field to add + * @param[in] field_class class to be assigned to the field, e.g. + * PIXEL_FIELD + * + * @return field + */ + Field& add_field(const std::string& name, FieldDescriptor d, + FieldClass field_class = FieldClass::PIXEL_FIELD); + + /** + * Add a new zero-filled field to lidar scan. + * + * @throw std::invalid_argument if key duplicates a preexisting field + * + * @param[in] type descriptor of the field to add + * + * @return field + */ + Field& add_field(const FieldType& type); + + /** + * Release the field and remove it from lidar scan + * + * @throw std::invalid_argument if field under key does not exist + * + * @param[in] name string key of the field to remove + */ + Field del_field(const std::string& name); /** * Get the type of the specified field. * - * @param[in] f the field to query. + * @param[in] name the string key of the field to query. + * + * @return the type associated with the field. + */ + FieldType field_type(const std::string& name) const; + + /** + * Get the FieldType of all fields in the scan * - * @return the type tag associated with the field. + * @return the type associated with every field in the scan */ - sensor::ChanFieldType field_type(sensor::ChanField f) const; + LidarScanFieldTypes field_types() const; - /** A const forward iterator over field / type pairs. */ - FieldIter begin() const; + /** + * Reference to the internal fields map + */ + std::unordered_map& fields(); - /** @copydoc begin() */ - FieldIter end() const; + /** @copydoc fields() */ + const std::unordered_map& fields() const; /** * Access the measurement timestamp headers. @@ -291,13 +432,14 @@ class LidarScan { Eigen::Ref> status() const; /** - * Access the vector of poses (per each timestamp). - * - * @return a reference to vector with poses (4x4) homogeneous. + * Access the array of poses (per each timestamp). Cast to + * ArrayView3 in order to access as 3d + * @return 3d field of homogenous pose matrices, shaped (w, 4, 4). */ - std::vector& pose(); + Field& pose(); + /** @copydoc pose() */ - const std::vector& pose() const; + const Field& pose() const; /** * Assess completeness of scan. @@ -318,15 +460,6 @@ class LidarScan { */ std::string to_string(const LidarScanFieldTypes& field_types); -/** - * Get the lidar scan field types from a lidar scan - * - * @param[in] ls The lidar scan to get the lidar scan field types from. - * - * @return The lidar scan field types - */ -LidarScanFieldTypes get_field_types(const LidarScan& ls); - /** * Get the lidar scan field types from lidar profile * @@ -517,8 +650,8 @@ inline img_t stagger(const Eigen::Ref>& img, * LidarScan. */ class ScanBatcher { - std::ptrdiff_t w; - std::ptrdiff_t h; + size_t w; + size_t h; uint16_t next_valid_m_id; uint16_t next_headers_m_id; uint16_t next_valid_packet_id; diff --git a/ouster_client/include/ouster/sensor_http.h b/ouster_client/include/ouster/sensor_http.h index 1430f623..06d8131c 100644 --- a/ouster_client/include/ouster/sensor_http.h +++ b/ouster_client/include/ouster/sensor_http.h @@ -19,6 +19,14 @@ namespace ouster { namespace sensor { namespace util { +/** + * Result for get_user_data_and_policy on SensorHttp + */ +struct UserDataAndPolicy { + bool keep_on_config_delete; + std::string data; +}; + /** * An interface to communicate with Ouster sensors via http requests */ @@ -38,25 +46,31 @@ class SensorHttp { /** * Queries the sensor metadata. * + * @param[in] timeout_sec The timeout for the request in seconds. + * * @return returns a Json object of the sensor metadata. */ - virtual Json::Value metadata() const = 0; + virtual Json::Value metadata(int timeout_sec = 1) const = 0; /** * Queries the sensor_info. * + * @param[in] timeout_sec The timeout for the request in seconds. + * * @return returns a Json object representing the sensor_info. */ - virtual Json::Value sensor_info() const = 0; + virtual Json::Value sensor_info(int timeout_sec = 1) const = 0; /** * Queries active/staged configuration on the sensor * * @param[in] active if true retrieve active, otherwise get staged configs. + * @param[in] timeout_sec The timeout for the request in seconds. * * @return a string represnting the active or staged config */ - virtual std::string get_config_params(bool active) const = 0; + virtual std::string get_config_params(bool active, + int timeout_sec = 1) const = 0; /** * Set the value of a specfic configuration on the sensor, the changed @@ -64,66 +78,122 @@ class SensorHttp { * * @param[in] key name of the config to change. * @param[in] value the new value to set for the selected configuration. + * @param[in] timeout_sec The timeout for the request in seconds. */ virtual void set_config_param(const std::string& key, - const std::string& value) const = 0; + const std::string& value, + int timeout_sec = 1) const = 0; /** * Retrieves the active configuration on the sensor + * + * @param[in] timeout_sec The timeout for the request in seconds. */ - virtual Json::Value active_config_params() const = 0; + virtual Json::Value active_config_params(int timeout_sec = 1) const = 0; /** * Retrieves the staged configuration on the sensor + * + * @param[in] timeout_sec The timeout for the request in seconds. */ - virtual Json::Value staged_config_params() const = 0; + virtual Json::Value staged_config_params(int timeout_sec = 1) const = 0; /** * Enables automatic assignment of udp destination ports. + * + * @param[in] timeout_sec The timeout for the request in seconds. */ - virtual void set_udp_dest_auto() const = 0; + virtual void set_udp_dest_auto(int timeout_sec = 1) const = 0; /** * Retrieves beam intrinsics of the sensor. + * + * @param[in] timeout_sec The timeout for the request in seconds. */ - virtual Json::Value beam_intrinsics() const = 0; + virtual Json::Value beam_intrinsics(int timeout_sec = 1) const = 0; /** * Retrieves imu intrinsics of the sensor. + * + * @param[in] timeout_sec The timeout for the request in seconds. */ - virtual Json::Value imu_intrinsics() const = 0; + virtual Json::Value imu_intrinsics(int timeout_sec = 1) const = 0; /** * Retrieves lidar intrinsics of the sensor. + * + * @param[in] timeout_sec The timeout for the request in seconds. */ - virtual Json::Value lidar_intrinsics() const = 0; + virtual Json::Value lidar_intrinsics(int timeout_sec = 1) const = 0; /** * Retrieves lidar data format. + * + * @param[in] timeout_sec The timeout for the request in seconds. */ - virtual Json::Value lidar_data_format() const = 0; + virtual Json::Value lidar_data_format(int timeout_sec = 1) const = 0; /** * Gets the calibaration status of the sensor. + * + * @param[in] timeout_sec The timeout for the request in seconds. */ - virtual Json::Value calibration_status() const = 0; + virtual Json::Value calibration_status(int timeout_sec = 1) const = 0; /** * Restarts the sensor applying all staged configurations. + * + * @param[in] timeout_sec The timeout for the request in seconds. */ - virtual void reinitialize() const = 0; + virtual void reinitialize(int timeout_sec = 1) const = 0; /** * Persist active configuration parameters to the sensor. + * + * @param[in] timeout_sec The timeout for the request in seconds. + */ + virtual void save_config_params(int timeout_sec = 1) const = 0; + + /** + * Gets the user data stored on the sensor. + * + * @param[in] timeout_sec The timeout for the request in seconds. + */ + virtual std::string get_user_data(int timeout_sec = 1) const = 0; + + /** + * Gets the user data stored on the sensor and the retention policy. + * + * @param[in] timeout_sec The timeout for the request in seconds. + */ + virtual UserDataAndPolicy get_user_data_and_policy( + int timeout_sec = 1) const = 0; + + /** + * Sets the user data stored on the sensor. + * + * @param[in] data Value of userdata to set on the sensor. + * @param[in] keep_on_config_delete If true, keep the userdata when + configuration is deleted from the sensor + * @param[in] timeout_sec The timeout for the request in seconds. + */ + virtual void set_user_data(const std::string& data, + bool keep_on_config_delete = true, + int timeout_sec = 1) const = 0; + + /** + * Deletes the user data stored on the sensor. + * + * @param[in] timeout_sec The timeout for the request in seconds. */ - virtual void save_config_params() const = 0; + virtual void delete_user_data(int timeout_sec = 1) const = 0; /** * Retrieves sensor firmware version information as a string. * * @param[in] hostname hostname of the sensor to communicate with. - * @param[in] timeout_sec The timeout to use in seconds, this argument - * is optional. + * @param[in] timeout_sec The timeout to use in seconds for the version + * request, this argument is optional. */ static std::string firmware_version_string( const std::string& hostname, @@ -133,8 +203,8 @@ class SensorHttp { * Retrieves sensor firmware version information. * * @param[in] hostname hostname of the sensor to communicate with. - * @param[in] timeout_sec The timeout to use in seconds, this argument - * is optional. + * @param[in] timeout_sec The timeout to use in seconds for the version + * request, this argument is optional. */ static ouster::util::version firmware_version( const std::string& hostname, @@ -144,8 +214,8 @@ class SensorHttp { * Creates an instance of the SensorHttp interface. * * @param[in] hostname hostname of the sensor to communicate with. - * @param[in] timeout_sec The timeout to use in seconds, this argument - * is optional. + * @param[in] timeout_sec The timeout to use in seconds for the version + * request, this argument is optional. */ static std::unique_ptr create( const std::string& hostname, diff --git a/ouster_client/include/ouster/strings.h b/ouster_client/include/ouster/strings.h new file mode 100644 index 00000000..519e57b2 --- /dev/null +++ b/ouster_client/include/ouster/strings.h @@ -0,0 +1,94 @@ +/** + * Copyright (c) 2024, Ouster, Inc. + * All rights reserved. + */ + +#pragma once + +#include +#include + +namespace ouster { +namespace strings { + +/** + * Channel strings, used as keys for LidarScan + */ +namespace channel { + +const std::string RANGE = "RANGE"; +const std::string RANGE2 = "RANGE2"; +const std::string INTENSITY = "INTENSITY"; +const std::string SIGNAL = "SIGNAL"; +const std::string SIGNAL2 = "SIGNAL2"; +const std::string REFLECTIVITY = "REFLECTIVITY"; +const std::string REFLECTIVITY2 = "REFLECTIVITY2"; +const std::string AMBIENT = "AMBIENT"; +const std::string NEAR_IR = "NEAR_IR"; +const std::string FLAGS = "FLAGS"; +const std::string FLAGS2 = "FLAGS2"; +const std::string RAW_HEADERS = "RAW_HEADERS"; +const std::string RAW32_WORD1 = "RAW32_WORD1"; +const std::string RAW32_WORD2 = "RAW32_WORD2"; +const std::string RAW32_WORD3 = "RAW32_WORD3"; +const std::string RAW32_WORD4 = "RAW32_WORD4"; +const std::string RAW32_WORD5 = "RAW32_WORD5"; +const std::string RAW32_WORD6 = "RAW32_WORD6"; +const std::string RAW32_WORD7 = "RAW32_WORD7"; +const std::string RAW32_WORD8 = "RAW32_WORD8"; +const std::string RAW32_WORD9 = "RAW32_WORD9"; + +} // namespace channel + +/** + * Header strings, used as keys for LidarScan + */ +namespace header { + +const std::string timestamp = "timestamp"; +const std::string measurement_id = "measurement_id"; +const std::string status = "status"; +const std::string packet_timestamp = "packet_timestamp"; +const std::string pose = "pose"; + +const std::array all = {header::timestamp, + header::measurement_id, header::status, + header::packet_timestamp, header::pose}; + +} // namespace header + +// clang-format off +const std::array all = { + /* channels */ + channel::RANGE, + channel::RANGE2, + channel::INTENSITY, + channel::SIGNAL, + channel::SIGNAL2, + channel::REFLECTIVITY, + channel::REFLECTIVITY2, + channel::AMBIENT, + channel::NEAR_IR, + channel::FLAGS, + channel::FLAGS2, + channel::RAW_HEADERS, + channel::RAW32_WORD1, + channel::RAW32_WORD2, + channel::RAW32_WORD3, + channel::RAW32_WORD4, + channel::RAW32_WORD5, + channel::RAW32_WORD6, + channel::RAW32_WORD7, + channel::RAW32_WORD8, + channel::RAW32_WORD9, + /* headers */ + header::timestamp, + header::measurement_id, + header::status, + header::packet_timestamp, + header::pose +}; +// clang-format on + +} // namespace strings +} // namespace ouster diff --git a/ouster_client/include/ouster/types.h b/ouster_client/include/ouster/types.h index a9bf6065..e6acf137 100644 --- a/ouster_client/include/ouster/types.h +++ b/ouster_client/include/ouster/types.h @@ -62,11 +62,10 @@ enum lidar_mode { MODE_512x10, ///< lidar mode: 10 scans of 512 columns per second MODE_512x20, ///< lidar mode: 20 scans of 512 columns per second MODE_1024x10, ///< lidar mode: 10 scans of 1024 columns per second - MODE_1024x20, ///< lidar mode: 20 scans of 1024 columsn per second + MODE_1024x20, ///< lidar mode: 20 scans of 1024 columns per second MODE_2048x10, ///< lidar mode: 10 scans of 2048 columns per second MODE_4096x5 ///< lidar mode: 5 scans of 4096 columns per second. Only ///< available on select sensors - }; /** @@ -229,7 +228,7 @@ enum ShotLimitingStatus { SHOT_LIMITING_NORMAL = 0x00, ///< Normal operation SHOT_LIMITING_IMMINENT = 0x01, ///< Shot limiting imminent SHOT_LIMITING_REDUCTION_0_10 = - 0x02, //< Shot limiting reduction by 0 to 10% + 0x02, ///< Shot limiting reduction by 0 to 10% SHOT_LIMITING_REDUCTION_10_20 = 0x03, ///< Shot limiting reduction by 10 to 20% SHOT_LIMITING_REDUCTION_20_30 = @@ -261,12 +260,12 @@ using ColumnWindow = std::pair; * Struct for sensor configuration parameters. */ struct sensor_config { - optional udp_dest; ///< The destination address for the - ///< lidar/imu data to be sent to - optional udp_port_lidar; ///< The destination port for the lidar - ///< data to be sent to - optional udp_port_imu; ///< The destination port for the imu data - ///< to be sent to + optional udp_dest; ///< The destination address for the + ///< lidar/imu data to be sent to + optional udp_port_lidar; ///< The destination port for the lidar + ///< data to be sent to + optional udp_port_imu; ///< The destination port for the imu + ///< data to be sent to // TODO: replace ts_mode and ld_mode when timestamp_mode and // lidar_mode get changed to CapsCase @@ -274,13 +273,13 @@ struct sensor_config { * The timestamp mode for the sensor to use. * Refer to timestamp_mode for more details. */ - optional ts_mode; + optional timestamp_mode; /** * The lidar mode for the sensor to use. * Refer to lidar_mode for more details. */ - optional ld_mode; + optional lidar_mode; /** * The operating mode for the sensor to use. @@ -434,18 +433,106 @@ struct calibration_status { optional reflectivity_timestamp; }; +/** Stores parsed information about the prod line */ +class product_info { + public: + /** + * The original full product line string. + */ + const std::string full_product_info; + + /** + * The form factor of the sensor. This will + * look like: "OS1" or "OS2" and such. + */ + const std::string form_factor; + + /** + * Is the sensor a short range build or not. + */ + const bool short_range; + + /** + * The beam configuration of the sensor. + */ + const std::string beam_config; + + /** + * The number of beams on the sensor. + */ + const int beam_count; + + /** + * Static method used to create product_info classes. + * + * @throws std::runtime_error on a bad product info line. + * + * @param[in] product_info_string The product info string to create + * the product_info class from. + * @return The new product_info class. + */ + static product_info create_product_info(std::string product_info_string); + + /** + * Default constructor for product_info that + * sets everything to blank. + */ + product_info(); + + protected: + /** + * Constructor to initialize each of the members off of. + * + * @internal + * + * @param[in] product_info_string The full product line string. + * @param[in] form_factor The sensor form factor. + * @param[in] short_range If the sensor is short range or not. + * @param[in] beam_config The beam configuration for the sensor. + * @param[in] beam_count The number of beams for a sensor. + */ + product_info(std::string product_info_string, std::string form_factor, + bool short_range, std::string beam_config, int beam_count); +}; + +/** + * Equality for product_info. + * + * @param[in] lhs The first object to compare. + * @param[in] rhs The second object to compare. + * + * @return lhs == rhs + */ +bool operator==(const product_info& lhs, const product_info& rhs); + +/** + * In-Equality for product_info. + * + * @param[in] lhs The first object to compare. + * @param[in] rhs The second object to compare. + * + * @return lhs != rhs + */ +bool operator!=(const product_info& lhs, const product_info& rhs); + +/** + * Get string representation of a product info. + * + * @param[in] info Product Info to get the string representation from. + * + * @return string representation of the product info. + */ +std::string to_string(const product_info& info); + /** Stores parsed information from metadata and */ struct sensor_info { // clang-format off - std::string - name{}; ///< user-convenience client-side assignable name, corresponds - ///< to hostname in metadata.json if present std::string sn{}; ///< sensor serial number corresponding to prod_sn in ///< metadata.json std::string fw_rev{}; ///< fw revision corresponding to build_rev in metadata.json - lidar_mode mode{}; ///< lidar mode of sensor std::string prod_line{}; ///< prod line + data_format format{}; ///< data format of sensor std::vector beam_azimuth_angles{}; ///< beam azimuth angles for 3D projection @@ -461,43 +548,44 @@ struct sensor_info { mat4d lidar_to_sensor_transform = mat4d::Zero(); ///< transform between lidar and sensor ///< coordinate frames - // TODO read extrinsic from metadata.json in the future mat4d extrinsic = mat4d::Zero(); ///< user-convenience client-side assignable extrinsic ///< matrix, currently is not read from metadata.json uint32_t init_id{}; ///< initialization ID updated every reinit - uint16_t udp_port_lidar{}; ///< the lidar destination port - uint16_t udp_port_imu{}; ///< the imu destination port std::string build_date{}; ///< build date from FW sensor_info std::string image_rev{}; ///< image rev from FW sensor_info std::string prod_pn{}; ///< prod pn std::string status{}; ///< sensor status at time of pulling metadata - calibration_status cal{}; ///< sensor calibration - sensor_config config{}; ///< parsed sensor config if available from metadata + calibration_status cal{}; ///< sensor calibration + sensor_config config{}; ///< parsed sensor config if available from metadata + std::string user_data{}; ///< userdata from sensor if available /* Constructor from metadata */ - sensor_info(const std::string& metadata, bool skip_beam_validation = false); + explicit sensor_info(const std::string& metadata, bool skip_beam_validation = false); /* Empty constructor -- keep for */ sensor_info(); - /* Return original metadata string should sensor_info have been - * constructed from one -- this string will be **unchanged** and - * will not reflect the changes to fields made to sensor_info*/ - std::string original_string() const; - /* Return an updated version of the metadata string reflecting any * changes to the sensor_info. * Errors out if changes are incompatible but does not check for validity */ - std::string updated_metadata_string() const; + std::string to_json_string() const; + + /** + * Parse and return version info about this sensor. + * + * @return sensor version info + */ + ouster::util::version get_version() const; + + product_info get_product_info() const; bool has_fields_equal(const sensor_info& other) const; private: - std::string - original_metadata_string{}; ///< string from which values were parsed + bool was_legacy_ = false; // clang-format on }; @@ -824,24 +912,6 @@ std::string to_string(ThermalShutdownStatus thermal_shutdown_status); */ void check_signal_multiplier(const double signal_multiplier); -/** - * Parse metadata text blob from the sensor into a sensor_info struct. - * - * String and vector fields will have size 0 if the parameter cannot - * be found or parsed, while lidar_mode will be set to 0 (invalid). - * - * @throw runtime_error if the text is not valid json - * - * @param[in] metadata a text blob returned by get_metadata from client.h. - * @param[in] skip_beam_validation whether to skip validation on metdata - not - * for use on recorded data or metadata - * from sensors - * - * @return a sensor_info struct populated with a subset of the metadata. - */ -sensor_info parse_metadata(const std::string& metadata, - bool skip_beam_validation = false); - /** * Parse metadata given path to a json file. * @@ -895,15 +965,6 @@ sensor_config parse_config(const std::string& config); */ std::string to_string(const sensor_config& config); -/** - * Convert non-legacy string representation of metadata to legacy. - * - * @param[in] metadata non-legacy string representation of metadata. - * - * @return legacy string representation of metadata. - */ -std::string convert_to_legacy(const std::string& metadata); - /** * Get a string representation of sensor calibration. Only set fields will be * represented. @@ -929,63 +990,56 @@ std::string client_version(); * * @return version corresponding to the string, or invalid_version on error. */ -ouster::util::version firmware_version_from_metadata( - const std::string& metadata); +[[deprecated("Use sensor_info::get_version() instead")]] ouster::util::version +firmware_version_from_metadata(const std::string& metadata); // clang-format off +typedef const char* cf_type; /** Tag to identitify a paricular value reported in the sensor channel data * block. */ -enum ChanField { - RANGE = 1, ///< 1st return range in mm - RANGE2 = 2, ///< 2nd return range in mm - INTENSITY = 3, ///< @deprecated Use SIGNAL instead - SIGNAL = 3, ///< 1st return signal in photons - SIGNAL2 = 4, ///< 2nd return signal in photons - REFLECTIVITY = 5, ///< 1st return reflectivity, calibrated by range and sensor +namespace ChanField { + static constexpr cf_type RANGE = "RANGE"; ///< 1st return range in mm + static constexpr cf_type RANGE2 = "RANGE2"; ///< 2nd return range in mm + static constexpr cf_type SIGNAL = "SIGNAL"; ///< 1st return signal in photons + static constexpr cf_type SIGNAL2 = "SIGNAL2"; ///< 2nd return signal in photons + static constexpr cf_type REFLECTIVITY = "REFLECTIVITY"; ///< 1st return reflectivity, calibrated by range and sensor ///< sensitivity in FW 2.1+. See sensor docs for more details - REFLECTIVITY2 = 6, ///< 2nd return reflectivity, calibrated by range and sensor + static constexpr cf_type REFLECTIVITY2 = "REFLECTIVITY2"; ///< 2nd return reflectivity, calibrated by range and sensor ///< sensitivity in FW 2.1+. See sensor docs for more details - AMBIENT = 7, ///< @deprecated Use NEAR_IR instead - NEAR_IR = 7, ///< near_ir in photons - FLAGS = 8, ///< 1st return flags - FLAGS2 = 9, ///< 2nd return flags - RAW_HEADERS = 40, ///< raw headers for packet/footer/column for dev use - RAW32_WORD5 = 45, ///< raw word access to packet for dev use - RAW32_WORD6 = 46, ///< raw word access to packet for dev use - RAW32_WORD7 = 47, ///< raw word access to packet for dev use - RAW32_WORD8 = 48, ///< raw word access to packet for dev use - RAW32_WORD9 = 49, ///< raw word access to packet for dev use - CUSTOM0 = 50, ///< custom user field - CUSTOM1 = 51, ///< custom user field - CUSTOM2 = 52, ///< custom user field - CUSTOM3 = 53, ///< custom user field - CUSTOM4 = 54, ///< custom user field - CUSTOM5 = 55, ///< custom user field - CUSTOM6 = 56, ///< custom user field - CUSTOM7 = 57, ///< custom user field - CUSTOM8 = 58, ///< custom user field - CUSTOM9 = 59, ///< custom user field - RAW32_WORD1 = 60, ///< raw word access to packet for dev use - RAW32_WORD2 = 61, ///< raw word access to packet for dev use - RAW32_WORD3 = 62, ///< raw word access to packet for dev use - RAW32_WORD4 = 63, ///< raw word access to packet for dev use - CHAN_FIELD_MAX = 64, ///< max which allows us to introduce future fields + + static constexpr cf_type NEAR_IR = "NEAR_IR"; ///< near_ir in photons + static constexpr cf_type FLAGS = "FLAGS"; ///< 1st return flags + static constexpr cf_type FLAGS2 = "FLAGS2"; ///< 2nd return flags + static constexpr cf_type RAW_HEADERS = "RAW_HEADERS"; ///< raw headers for packet/footer/column for dev use + static constexpr cf_type RAW32_WORD5 = "RAW32_WORD5"; ///< raw word access to packet for dev use + static constexpr cf_type RAW32_WORD6 = "RAW32_WORD6"; ///< raw word access to packet for dev use + static constexpr cf_type RAW32_WORD7 = "RAW32_WORD7"; ///< raw word access to packet for dev use + static constexpr cf_type RAW32_WORD8 = "RAW32_WORD8"; ///< raw word access to packet for dev use + static constexpr cf_type RAW32_WORD9 = "RAW32_WORD9"; ///< raw word access to packet for dev use + static constexpr cf_type RAW32_WORD1 = "RAW32_WORD1"; ///< raw word access to packet for dev use + static constexpr cf_type RAW32_WORD2 = "RAW32_WORD2"; ///< raw word access to packet for dev use + static constexpr cf_type RAW32_WORD3 = "RAW32_WORD3"; ///< raw word access to packet for dev use + static constexpr cf_type RAW32_WORD4 = "RAW32_WORD4"; ///< raw word access to packet for dev use }; // clang-format on -/** - * Get string representation of a channel field. - * - * @param[in] field The field to get the string representation of. - * - * @return string representation of the channel field. - */ -std::string to_string(ChanField field); - /** * Types of channel fields. */ -enum ChanFieldType { VOID = 0, UINT8, UINT16, UINT32, UINT64 }; +enum ChanFieldType { + VOID = 0, + UINT8 = 1, + UINT16 = 2, + UINT32 = 3, + UINT64 = 4, + INT8 = 5, + INT16 = 6, + INT32 = 7, + INT64 = 8, + FLOAT32 = 9, + FLOAT64 = 10, + UNREGISTERED = 100 +}; /** * Get the size of the ChanFieldType in bytes. @@ -1021,17 +1075,16 @@ std::string to_string(ChanFieldType ft); class packet_format { protected: template - T px_field(const uint8_t* px_buf, ChanField i) const; + T px_field(const uint8_t* px_buf, const std::string& i) const; template - void block_field_impl(Eigen::Ref> field, ChanField i, + void block_field_impl(Eigen::Ref> field, const std::string& i, const uint8_t* packet_buf) const; struct Impl; std::shared_ptr impl_; - std::vector> - field_types_; + std::vector> field_types_; public: packet_format(UDPProfileLidar udp_profile_lidar, size_t pixels_per_column, @@ -1139,7 +1192,7 @@ class packet_format { * @return a type tag specifying the bitwidth of the requested field or * ChannelFieldType::VOID if it is not supported by the packet format. */ - ChanFieldType field_type(ChanField f) const; + ChanFieldType field_type(const std::string& f) const; /** * A const forward iterator over field / type pairs. @@ -1210,7 +1263,7 @@ class packet_format { /** * Copy the specified channel field out of a packet measurement block. * - * @tparam T T should be an unsigned integer type large enough to store + * @tparam T T should be a numeric type large enough to store * values of the specified field. Otherwise, data will be truncated. * * @param[in] col_buf a measurement block pointer returned by `nth_col()`. @@ -1218,9 +1271,8 @@ class packet_format { * @param[out] dst destination array of size pixels_per_column * dst_stride. * @param[in] dst_stride stride for writing to the destination array. */ - template ::value, T>::type = 0> - void col_field(const uint8_t* col_buf, ChanField f, T* dst, + template + void col_field(const uint8_t* col_buf, const std::string& f, T* dst, int dst_stride = 1) const; /** @@ -1235,16 +1287,15 @@ class packet_format { * Faster traversal than col_field, but has to copy the entire packet all at * once. * - * @tparam T T should be an unsigned integer type large enough to store + * @tparam T T should be a numeric type large enough to store * values of the specified field. Otherwise, data will be truncated. * * @param[out] field destination eigen array * @param[in] f the channel field to copy. * @param[in] lidar_buf the lidar buffer. */ - template ::value, T>::type = 0> - void block_field(Eigen::Ref> field, ChanField f, + template + void block_field(Eigen::Ref> field, const std::string& f, const uint8_t* lidar_buf) const; // Per-pixel channel data block accessors @@ -1346,7 +1397,7 @@ class packet_format { * * @return mask of possible values */ - uint64_t field_value_mask(ChanField f) const; + uint64_t field_value_mask(const std::string& f) const; /** * Get number of bits in the channel field @@ -1355,7 +1406,7 @@ class packet_format { * * @return number of bits */ - int field_bitness(ChanField f) const; + int field_bitness(const std::string& f) const; }; /** @defgroup OusterClientTypeGetFormat Get Packet Format functions */ @@ -1406,11 +1457,31 @@ struct Packet { } }; +/* + * Reasons for failure of packet validation. + */ +enum class PacketValidationFailure { + NONE = 0, ///< No validation errors were found + PACKET_SIZE = 1, ///< The packet size does not match the expected size + ID = 2 ///< The prod_sn or init_id does not match the metadata +}; + /** * Encapsulate a lidar packet buffer and attributes associated with it. */ struct LidarPacket : public Packet { using Packet::Packet; + + /** + * Validates that the packet matches the expected format and metadata. + * + * @param[in] info expected sensor metadata + * @param[in] format expected packet_format + * + * @return a PacketValdationFailure with either NONE or a failure reason. + */ + PacketValidationFailure validate(const sensor_info& info, + const packet_format& format); }; /** @@ -1418,6 +1489,17 @@ struct LidarPacket : public Packet { */ struct ImuPacket : public Packet { using Packet::Packet; + + /** + * Validates that the packet matches the expected format and metadata. + * + * @param[in] info expected sensor metadata + * @param[in] format expected packet_format + * + * @return a PacketValdationFailure with either NONE or a failure reason. + */ + PacketValidationFailure validate(const sensor_info& info, + const packet_format& format); }; namespace impl { diff --git a/ouster_client/include/ouster/version.h b/ouster_client/include/ouster/version.h index fe162cab..d7bc62e6 100644 --- a/ouster_client/include/ouster/version.h +++ b/ouster_client/include/ouster/version.h @@ -15,12 +15,16 @@ namespace ouster { namespace util { struct version { - uint16_t major; ///< Major version number - uint16_t minor; ///< Minor version number - uint16_t patch; ///< Patch(or revision) version number + uint16_t major; ///< Major version number + uint16_t minor; ///< Minor version number + uint16_t patch; ///< Patch(or revision) version number + std::string stage; ///< Release stage name, if present. + std::string machine; ///< Machine name, if present. + std::string prerelease; ///< Prerelease name (e.g. rc1), if present. + std::string build; ///< Build info, if present. Often a date string. }; -const version invalid_version = {0, 0, 0}; +const version invalid_version = {0, 0, 0, "", "", "", ""}; /** \defgroup ouster_client_version_operators Ouster Client version.h Operators * @{ @@ -34,7 +38,9 @@ const version invalid_version = {0, 0, 0}; * @return If the versions are the same. */ inline bool operator==(const version& u, const version& v) { - return u.major == v.major && u.minor == v.minor && u.patch == v.patch; + return u.major == v.major && u.minor == v.minor && u.patch == v.patch && + u.stage == v.stage && u.machine == v.machine && u.build == v.build && + u.prerelease == v.prerelease; } /** @@ -94,26 +100,9 @@ inline bool operator>(const version& u, const version& v) { return !(u <= v); } /** @}*/ /** - * Get string representation of a version. - * - * @param[in] v version. - * - * @return string representation of the version. - */ -std::string to_string(const version& v); - -/** - * Get version from string. - * - * @param[in] s string. - * - * @return version corresponding to the string, or invalid_version on error. - */ -[[deprecated("Use version_from_string instead")]] version version_of_string( - const std::string& s); - -/** - * Get version from string. + * Get version from string. Parses strings of the format: + * STAGE-MACHINE-vMAJOR.MINOR.PATCH-PRERELEASE+BUILD + * Requires at least major.minor.patch to return a valid version. * * @param[in] ver string. * diff --git a/ouster_client/src/client.cpp b/ouster_client/src/client.cpp index af3d2cd2..9e67195d 100644 --- a/ouster_client/src/client.cpp +++ b/ouster_client/src/client.cpp @@ -23,8 +23,8 @@ #include #include -#include "logging.h" #include "ouster/impl/client_poller.h" +#include "ouster/impl/logging.h" #include "ouster/impl/netcompat.h" #include "ouster/sensor_http.h" #include "ouster/types.h" @@ -261,18 +261,29 @@ Json::Value collect_metadata(const std::string& hostname, int timeout_sec) { "A timeout occurred while waiting for the sensor to " "initialize."); } - status = sensor_http->sensor_info()["status"].asString(); + status = sensor_http->sensor_info(timeout_sec)["status"].asString(); if (status != "INITIALIZING") { break; } std::this_thread::sleep_for(1s); } + std::string user_data = ""; try { - auto metadata = sensor_http->metadata(); + user_data = sensor_http->get_user_data(timeout_sec); + } catch (const std::runtime_error& e) { + if (strcmp(e.what(), + "user data API not supported on this FW version") != 0) { + throw e; + } + } + + try { + auto metadata = sensor_http->metadata(timeout_sec); metadata["ouster-sdk"]["client_version"] = client_version(); metadata["ouster-sdk"]["output_source"] = "collect_metadata"; + metadata["user_data"] = user_data; return metadata; } catch (const std::runtime_error& e) { @@ -288,7 +299,7 @@ Json::Value collect_metadata(const std::string& hostname, int timeout_sec) { bool get_config(const std::string& hostname, sensor_config& config, bool active, int timeout_sec) { auto sensor_http = SensorHttp::create(hostname, timeout_sec); - auto res = sensor_http->get_config_params(active); + auto res = sensor_http->get_config_params(active, timeout_sec); config = parse_config(res); return true; } @@ -298,7 +309,7 @@ bool set_config(const std::string& hostname, const sensor_config& config, auto sensor_http = SensorHttp::create(hostname, timeout_sec); // reset staged config to avoid spurious errors - auto config_params = sensor_http->active_config_params(); + auto config_params = sensor_http->active_config_params(timeout_sec); Json::Value config_params_copy = config_params; // set all desired config parameters @@ -332,9 +343,9 @@ bool set_config(const std::string& hostname, const sensor_config& config, if (config.udp_dest) throw std::invalid_argument( "UDP_DEST_AUTO flag set but provided config has udp_dest"); - sensor_http->set_udp_dest_auto(); + sensor_http->set_udp_dest_auto(timeout_sec); - auto staged = sensor_http->staged_config_params(); + auto staged = sensor_http->staged_config_params(timeout_sec); // now we set config_params according to the staged udp_dest from the // sensor @@ -356,20 +367,20 @@ bool set_config(const std::string& hostname, const sensor_config& config, // send full string -- depends on older FWs not rejecting a blob even // when it contains unknown keys auto config_params_str = Json::writeString(builder, config_params); - sensor_http->set_config_param(".", config_params_str); + sensor_http->set_config_param(".", config_params_str, timeout_sec); // reinitialize to make all staged parameters effective - sensor_http->reinitialize(); + sensor_http->reinitialize(timeout_sec); } // save if indicated if (config_flags & CONFIG_PERSIST) { - sensor_http->save_config_params(); + sensor_http->save_config_params(timeout_sec); } return true; } -std::string get_metadata(client& cli, int timeout_sec, bool legacy_format) { +std::string get_metadata(client& cli, int timeout_sec) { // Note, this function calls functions that throw std::runtime_error // on timeout. try { @@ -385,18 +396,9 @@ std::string get_metadata(client& cli, int timeout_sec, bool legacy_format) { builder["precision"] = 6; builder["indentation"] = " "; auto metadata_string = Json::writeString(builder, cli.meta); - if (legacy_format) { - logger().warn( - "The SDK will soon output the non-legacy metadata format by " - "default. If you parse the metadata directly instead of using the " - "SDK (which will continue to read both legacy and non-legacy " - "formats), please be advised that on the next release you will " - "either have to update your parsing or specify legacy_format = " - "true to the get_metadata function."); - } // We can't insert this logic into the light init_client since its advantage - // is that it doesn't make netowrk calls but we need it to run every time + // is that it doesn't make network calls but we need it to run every time // there is a valid connection to the sensor So we insert it here // TODO: remove after release of FW 3.2/3.3 (sufficient warning) sensor_config config; @@ -413,7 +415,7 @@ std::string get_metadata(client& cli, int timeout_sec, bool legacy_format) { "sticking with older FWs, the Ouster SDK will continue to parse " "the legacy lidar profile."); } - return legacy_format ? convert_to_legacy(metadata_string) : metadata_string; + return metadata_string; } bool init_logger(const std::string& log_level, const std::string& log_file_path, @@ -466,8 +468,8 @@ std::shared_ptr init_client(const std::string& hostname, config_flags |= CONFIG_UDP_DEST_AUTO; else config.udp_dest = udp_dest_host; - if (ld_mode) config.ld_mode = ld_mode; - if (ts_mode) config.ts_mode = ts_mode; + if (ld_mode) config.lidar_mode = ld_mode; + if (ts_mode) config.timestamp_mode = ts_mode; if (lidar_port) config.udp_port_lidar = lidar_port; if (imu_port) config.udp_port_imu = imu_port; if (persist_config) config_flags |= CONFIG_PERSIST; diff --git a/ouster_client/src/curl_client.h b/ouster_client/src/curl_client.h index 83e196d3..7114548d 100644 --- a/ouster_client/src/curl_client.h +++ b/ouster_client/src/curl_client.h @@ -2,21 +2,29 @@ #include #include +#include #include #include "http_client.h" -#include "logging.h" +#include "ouster/impl/logging.h" class CurlClient : public ouster::util::HttpClient { + enum class RequestType { + TYPE_GET = 0, + TYPE_DELETE = 1, + TYPE_POST = 2, + TYPE_PUT = 3 + }; + + mutable std::mutex mutex_; + public: - CurlClient(const std::string& base_url_, int timeout_seconds) - : HttpClient(base_url_) { + CurlClient(const std::string& base_url_) : HttpClient(base_url_) { curl_global_init(CURL_GLOBAL_ALL); curl_handle = curl_easy_init(); curl_easy_setopt(curl_handle, CURLOPT_WRITEFUNCTION, &CurlClient::write_memory_callback); curl_easy_setopt(curl_handle, CURLOPT_WRITEDATA, this); - curl_easy_setopt(curl_handle, CURLOPT_TIMEOUT, timeout_seconds); } virtual ~CurlClient() override { @@ -25,9 +33,32 @@ class CurlClient : public ouster::util::HttpClient { } public: - std::string get(const std::string& url) const override { + std::string get(const std::string& url, + int timeout_seconds) const override { + auto full_url = url_combine(base_url, url); + return execute_request(RequestType::TYPE_GET, full_url, + timeout_seconds); + } + + std::string del(const std::string& url, + int timeout_seconds) const override { + auto full_url = url_combine(base_url, url); + return execute_request(RequestType::TYPE_DELETE, full_url, + timeout_seconds); + } + + std::string post(const std::string& url, const std::string& data, + int timeout_seconds) const override { auto full_url = url_combine(base_url, url); - return execute_get(full_url); + return execute_request(RequestType::TYPE_POST, full_url, + timeout_seconds, data.c_str()); + } + + std::string put(const std::string& url, const std::string& data, + int timeout_seconds) const override { + auto full_url = url_combine(base_url, url); + return execute_request(RequestType::TYPE_PUT, full_url, timeout_seconds, + data.c_str()); } std::string encode(const std::string& str) const override { @@ -53,19 +84,41 @@ class CurlClient : public ouster::util::HttpClient { return url1 + url2; } - std::string execute_get(const std::string& url, int attempts = 3, - int retry_delay_ms = 500) const { + std::string execute_request(RequestType type, const std::string& url, + int timeout_seconds, const char* data = 0, + int attempts = 3, + int retry_delay_ms = 500) const { long http_code = 0; + struct curl_slist* hs = NULL; if (attempts < 1) { throw std::invalid_argument( - "CurlClient::execute_get: invalid number of retries"); + "CurlClient::execute_request: invalid number of retries"); } if (retry_delay_ms < 0) { throw std::invalid_argument( - "CurlClient::execute_get: invalid retry delay"); + "CurlClient::execute_request: invalid retry delay"); } + std::lock_guard guard(mutex_); curl_easy_setopt(curl_handle, CURLOPT_URL, url.c_str()); - curl_easy_setopt(curl_handle, CURLOPT_HTTPGET, 1L); + curl_easy_setopt(curl_handle, CURLOPT_TIMEOUT, timeout_seconds); + if (type == RequestType::TYPE_GET) { + curl_easy_setopt(curl_handle, CURLOPT_CUSTOMREQUEST, 0); + curl_easy_setopt(curl_handle, CURLOPT_HTTPGET, 1L); + curl_easy_setopt(curl_handle, CURLOPT_HTTPHEADER, 0); + } else if (type == RequestType::TYPE_DELETE) { + curl_easy_setopt(curl_handle, CURLOPT_CUSTOMREQUEST, "DELETE"); + curl_easy_setopt(curl_handle, CURLOPT_HTTPHEADER, 0); + } else if (type == RequestType::TYPE_POST) { + curl_easy_setopt(curl_handle, CURLOPT_CUSTOMREQUEST, 0); + hs = curl_slist_append(hs, "Content-Type: application/json"); + curl_easy_setopt(curl_handle, CURLOPT_HTTPHEADER, hs); + curl_easy_setopt(curl_handle, CURLOPT_POSTFIELDS, data); + } else if (type == RequestType::TYPE_PUT) { + curl_easy_setopt(curl_handle, CURLOPT_CUSTOMREQUEST, "PUT"); + hs = curl_slist_append(hs, "Content-Type: application/json"); + curl_easy_setopt(curl_handle, CURLOPT_HTTPHEADER, hs); + curl_easy_setopt(curl_handle, CURLOPT_POSTFIELDS, data); + } while (attempts--) { buffer.clear(); auto res = curl_easy_perform(curl_handle); @@ -76,30 +129,34 @@ class CurlClient : public ouster::util::HttpClient { res = curl_easy_perform(curl_handle); } if (res != CURLE_OK) { + curl_slist_free_all(hs); throw std::runtime_error( - "CurlClient::execute_get failed for the url: [" + url + + "CurlClient::execute_request failed for the url: [" + url + "] with the error message: " + curl_easy_strerror(res)); } curl_easy_getinfo(curl_handle, CURLINFO_RESPONSE_CODE, &http_code); if (200 <= http_code && http_code < 300) { // HTTP 2XX means a successful response + curl_slist_free_all(hs); return buffer; } if (attempts && 500 <= http_code && http_code < 600) { // HTTP 5XX means a server error, so we should re-attempt. // log a warning and sleep before re-attempting - ouster::sensor::logger().warn( - "Re-attempting CurlClient::execute_get after failure for url: [{}] with the code: [{}] - and return: {}", - url, http_code, buffer); - std::this_thread::sleep_for( + ouster::sensor::logger().warn( + "Re-attempting CurlClient::execute_get after failure for " + "url: [{}] with the code: [{}] - and return: {}", + url, http_code, buffer); + std::this_thread::sleep_for( std::chrono::milliseconds(retry_delay_ms)); } } // the request failed after repeated attempts with a response code other // than HTTP 2XX + curl_slist_free_all(hs); throw std::runtime_error( - std::string("CurlClient::execute_get failed for url: [" + url + + std::string("CurlClient::execute_request failed for url: [" + url + "] with the code: [" + std::to_string(http_code) + std::string("] - and return: ") + buffer)); } diff --git a/ouster_client/src/field.cpp b/ouster_client/src/field.cpp new file mode 100644 index 00000000..a99ec555 --- /dev/null +++ b/ouster_client/src/field.cpp @@ -0,0 +1,236 @@ +/** + * Copyright (c) 2024, Ouster, Inc. + * All rights reserved. + */ + +#include "ouster/field.h" + +#include + +namespace ouster { + +namespace impl { + +std::vector calculate_strides(const std::vector& shape) { + auto total = std::accumulate(shape.begin(), shape.end(), size_t{1}, + std::multiplies{}); + auto strides = std::vector{}; + strides.reserve(shape.size()); + for (auto dim : shape) { + total /= dim; + strides.push_back(total); + } + return strides; +} + +} // namespace impl + +namespace sensor { +namespace impl { + +// clang-format off + +template <> int type_size() { return 1; } + +template <> size_t type_hash() { return 0; } + +template <> ChanFieldType type_cft() { return ChanFieldType::VOID; } +template <> ChanFieldType type_cft() { return ChanFieldType::UINT8; } +template <> ChanFieldType type_cft() { return ChanFieldType::UINT16; } +template <> ChanFieldType type_cft() { return ChanFieldType::UINT32; } +template <> ChanFieldType type_cft() { return ChanFieldType::UINT64; } +template <> ChanFieldType type_cft() { return ChanFieldType::INT8; } +template <> ChanFieldType type_cft() { return ChanFieldType::INT16; } +template <> ChanFieldType type_cft() { return ChanFieldType::INT32; } +template <> ChanFieldType type_cft() { return ChanFieldType::INT64; } +template <> ChanFieldType type_cft() { return ChanFieldType::FLOAT32; } +template <> ChanFieldType type_cft() { return ChanFieldType::FLOAT64; } + +// clang-format on + +template +using Table = std::array, N>; + +extern const Table field_class_strings{ + {{FieldClass::PIXEL_FIELD, "PIXEL_FIELD"}, + {FieldClass::COLUMN_FIELD, "COLUMN_FIELD"}, + {FieldClass::PACKET_FIELD, "PACKET_FIELD"}, + {FieldClass::SCAN_FIELD, "SCAN_FIELD"}}}; + +} // namespace impl +} // namespace sensor + +std::string to_string(FieldClass flag) { + auto end = sensor::impl::field_class_strings.end(); + auto res = std::find_if(sensor::impl::field_class_strings.begin(), end, + [flag](const auto& p) { return p.first == flag; }); + + return res == end ? "UNKNOWN" : res->second; +} + +size_t FieldDescriptor::size() const { + return std::accumulate(shape.begin(), shape.end(), size_t{1}, + std::multiplies{}); +} + +int FieldDescriptor::element_size() const { return bytes / size(); } + +sensor::ChanFieldType FieldDescriptor::tag() const { + using sensor::impl::type_cft; + + if (type == type_hash()) { + return type_cft(); + } else if (type == type_hash()) { + return type_cft(); + } else if (type == type_hash()) { + return type_cft(); + } else if (type == type_hash()) { + return type_cft(); + } else if (type == type_hash()) { + return type_cft(); + } else if (type == type_hash()) { + return type_cft(); + } else if (type == type_hash()) { + return type_cft(); + } else if (type == type_hash()) { + return type_cft(); + } else if (type == type_hash()) { + return type_cft(); + } else if (type == type_hash()) { + return type_cft(); + } else if (type == type_hash()) { + return type_cft(); + } else { + return sensor::ChanFieldType::UNREGISTERED; + } +} + +void FieldDescriptor::swap(FieldDescriptor& other) { + std::swap(type, other.type); + std::swap(bytes, other.bytes); + std::swap(shape, other.shape); + std::swap(strides, other.strides); +} + +bool FieldDescriptor::is_type_compatible( + const FieldDescriptor& other) const noexcept { + return !type || !other.type || type == other.type; +} + +size_t FieldDescriptor::ndim() const noexcept { return shape.size(); } + +FieldView::FieldView() noexcept : ptr_(nullptr), desc_() {} + +FieldView::FieldView(void* ptr, const FieldDescriptor& desc) + : ptr_(ptr), desc_(desc) {} + +FieldView::operator bool() const noexcept { return !!get(); } + +size_t FieldView::bytes() const noexcept { return desc_.bytes; } + +size_t FieldView::size() const { return desc_.size(); } + +bool FieldView::matches(const FieldDescriptor& d) const noexcept { + return desc_ == d; +} + +const FieldDescriptor& FieldView::desc() const { return desc_; } + +const std::vector& FieldView::shape() const { return desc_.shape; } + +sensor::ChanFieldType FieldView::tag() const { return desc_.tag(); } + +bool FieldView::sparse() const { + const auto& strides = desc_.strides; + const auto& shape = desc_.shape; + + if (shape.size() == 0) return false; + + bool dense = strides.back() == 1; + for (int i = 1, end = shape.size(); i < end; ++i) { + dense = dense && (strides[i - 1] == strides[i] * shape[i]); + } + return !dense; +} + +Field::Field() noexcept : FieldView(), class_{FieldClass::SCAN_FIELD} {} +Field::~Field() { free(ptr_); } + +Field::Field(const FieldDescriptor& desc, FieldClass field_class) + : FieldView(nullptr, desc), class_{field_class} { + ptr_ = calloc(desc.bytes, sizeof(uint8_t)); + if (!ptr_) { + throw std::runtime_error("Field: host allocation failed"); + } +} + +Field::Field(Field&& other) noexcept : Field() { swap(other); }; + +Field& Field::operator=(Field&& other) noexcept { + swap(other); + return (*this); +} + +Field::Field(const Field& other) + : FieldView(nullptr, other.desc()), class_{other.class_} { + ptr_ = malloc(desc().bytes); + if (!ptr_) { + throw std::runtime_error("Field: host allocation failed"); + } + std::memcpy(ptr_, other.ptr_, other.bytes()); +} + +Field& Field::operator=(const Field& other) { + Field new_fp(other); + swap(new_fp); + return (*this); +} + +FieldClass Field::field_class() const { return class_; } + +void Field::swap(Field& other) noexcept { + std::swap(ptr_, other.ptr_); + desc_.swap(other.desc_); + std::swap(class_, other.class_); +} + +bool Field::operator==(const Field& other) const { + return matches(other.desc()) && + (std::memcmp(ptr_, other.ptr_, bytes()) == 0) && + class_ == other.class_; +} + +FieldView uint_view(const FieldView& other) { + if (other.shape().size() == 0) { + throw std::invalid_argument( + "uint_view: attempted converting a non-array FieldView"); + } + + FieldDescriptor desc; + switch (other.desc().element_size()) { + case 1: + desc = FieldDescriptor::array(other.shape()); + break; + case 2: + desc = FieldDescriptor::array(other.shape()); + break; + case 4: + desc = FieldDescriptor::array(other.shape()); + break; + case 8: + desc = FieldDescriptor::array(other.shape()); + break; + default: + // shape size check should usually suffice, but this may trigger + // on strange cases like views bound to arrays of custom structs + throw std::invalid_argument( + "uint_view: got wrong element size, are you using an array " + "of primitives?"); + } + + return {const_cast(other.get()), desc}; +} + +} // namespace ouster + +void std::swap(ouster::Field& a, ouster::Field& b) { a.swap(b); } diff --git a/ouster_client/src/http_client.h b/ouster_client/src/http_client.h index 7f900711..df579e41 100644 --- a/ouster_client/src/http_client.h +++ b/ouster_client/src/http_client.h @@ -33,11 +33,49 @@ class HttpClient { * Executes a GET request towards the provided url. * * @param[in] url http request url. + * @param[in] timeout_sec The timeout for the request in seconds. * * @return the result of the execution. If request fails it returns an empty * string. */ - virtual std::string get(const std::string& url) const = 0; + virtual std::string get(const std::string& url, int timeout_sec) const = 0; + + /** + * Executes a Delete request towards the provided url. + * + * @param[in] url http request url. + * @param[in] timeout_sec The timeout for the request in seconds. + * + * @return the result of the execution. If request fails it returns an empty + * string. + */ + virtual std::string del(const std::string& url, int timeout_sec) const = 0; + + /** + * Executes a Delete request towards the provided url. + * + * @param[in] url http request url. + * @param[in] data http post data. + * @param[in] timeout_sec The timeout for the request in seconds. + * + * @return the result of the execution. If request fails it returns an empty + * string. + */ + virtual std::string post(const std::string& url, const std::string& data, + int timeout_sec) const = 0; + + /** + * Executes a Delete request towards the provided url. + * + * @param[in] url http request url. + * @param[in] data http post data. + * @param[in] timeout_sec The timeout for the request in seconds. + * + * @return the result of the execution. If request fails it returns an empty + * string. + */ + virtual std::string put(const std::string& url, const std::string& data, + int timeout_sec) const = 0; /** * Encodes the given string as a url. diff --git a/ouster_client/src/lidar_scan.cpp b/ouster_client/src/lidar_scan.cpp index bf66eae4..7675b131 100644 --- a/ouster_client/src/lidar_scan.cpp +++ b/ouster_client/src/lidar_scan.cpp @@ -6,16 +6,20 @@ #include "ouster/lidar_scan.h" #include +#include #include #include #include #include #include -#include "logging.h" #include "ouster/impl/lidar_scan_impl.h" +#include "ouster/impl/logging.h" +#include "ouster/strings.h" #include "ouster/types.h" +using namespace ouster::strings; + namespace ouster { // clang-format off @@ -29,13 +33,12 @@ enum frame_status_masks : uint64_t { //! @cond Doxygen_Suppress enum frame_status_shifts: uint64_t { - FRAME_STATUS_THERMAL_SHUTDOWN_SHIFT = 0, ///< No shift for thermal shutdown + FRAME_STATUS_THERMAL_SHUTDOWN_SHIFT = 0, ///< No shift for thermal shutdown FRAME_STATUS_SHOT_LIMITING_SHIFT = 4 /// shift 4 for shot limiting }; //! @endcond // clang-format on -using sensor::ChanField; using sensor::ChanFieldType; using sensor::UDPProfileLidar; @@ -50,53 +53,53 @@ namespace impl { template using Table = std::array, N>; -static const Table legacy_field_slots{ - {{ChanField::RANGE, ChanFieldType::UINT32}, - {ChanField::SIGNAL, ChanFieldType::UINT32}, - {ChanField::NEAR_IR, ChanFieldType::UINT32}, - {ChanField::REFLECTIVITY, ChanFieldType::UINT32}}}; - -static const Table dual_field_slots{{ - {ChanField::RANGE, ChanFieldType::UINT32}, - {ChanField::RANGE2, ChanFieldType::UINT32}, - {ChanField::SIGNAL, ChanFieldType::UINT16}, - {ChanField::SIGNAL2, ChanFieldType::UINT16}, - {ChanField::REFLECTIVITY, ChanFieldType::UINT8}, - {ChanField::REFLECTIVITY2, ChanFieldType::UINT8}, - {ChanField::NEAR_IR, ChanFieldType::UINT16}, +static const Table legacy_field_slots{ + {{sensor::ChanField::RANGE, ChanFieldType::UINT32}, + {sensor::ChanField::SIGNAL, ChanFieldType::UINT32}, + {sensor::ChanField::NEAR_IR, ChanFieldType::UINT32}, + {sensor::ChanField::REFLECTIVITY, ChanFieldType::UINT32}}}; + +static const Table dual_field_slots{{ + {sensor::ChanField::RANGE, ChanFieldType::UINT32}, + {sensor::ChanField::RANGE2, ChanFieldType::UINT32}, + {sensor::ChanField::SIGNAL, ChanFieldType::UINT16}, + {sensor::ChanField::SIGNAL2, ChanFieldType::UINT16}, + {sensor::ChanField::REFLECTIVITY, ChanFieldType::UINT8}, + {sensor::ChanField::REFLECTIVITY2, ChanFieldType::UINT8}, + {sensor::ChanField::NEAR_IR, ChanFieldType::UINT16}, }}; -static const Table single_field_slots{{ - {ChanField::RANGE, ChanFieldType::UINT32}, - {ChanField::SIGNAL, ChanFieldType::UINT16}, - {ChanField::REFLECTIVITY, ChanFieldType::UINT16}, - {ChanField::NEAR_IR, ChanFieldType::UINT16}, +static const Table single_field_slots{{ + {sensor::ChanField::RANGE, ChanFieldType::UINT32}, + {sensor::ChanField::SIGNAL, ChanFieldType::UINT16}, + {sensor::ChanField::REFLECTIVITY, ChanFieldType::UINT16}, + {sensor::ChanField::NEAR_IR, ChanFieldType::UINT16}, }}; -static const Table lb_field_slots{{ - {ChanField::RANGE, ChanFieldType::UINT32}, - {ChanField::REFLECTIVITY, ChanFieldType::UINT16}, - {ChanField::NEAR_IR, ChanFieldType::UINT16}, +static const Table lb_field_slots{{ + {sensor::ChanField::RANGE, ChanFieldType::UINT32}, + {sensor::ChanField::REFLECTIVITY, ChanFieldType::UINT16}, + {sensor::ChanField::NEAR_IR, ChanFieldType::UINT16}, }}; -static const Table five_word_slots{{ - {ChanField::RAW32_WORD1, ChanFieldType::UINT32}, - {ChanField::RAW32_WORD2, ChanFieldType::UINT32}, - {ChanField::RAW32_WORD3, ChanFieldType::UINT32}, - {ChanField::RAW32_WORD4, ChanFieldType::UINT32}, - {ChanField::RAW32_WORD5, ChanFieldType::UINT32}, +static const Table five_word_slots{{ + {sensor::ChanField::RAW32_WORD1, ChanFieldType::UINT32}, + {sensor::ChanField::RAW32_WORD2, ChanFieldType::UINT32}, + {sensor::ChanField::RAW32_WORD3, ChanFieldType::UINT32}, + {sensor::ChanField::RAW32_WORD4, ChanFieldType::UINT32}, + {sensor::ChanField::RAW32_WORD5, ChanFieldType::UINT32}, }}; -static const Table fusa_two_word_slots{{ - {ChanField::RANGE, ChanFieldType::UINT32}, - {ChanField::REFLECTIVITY, ChanFieldType::UINT8}, - {ChanField::NEAR_IR, ChanFieldType::UINT16}, - {ChanField::RANGE2, ChanFieldType::UINT32}, - {ChanField::REFLECTIVITY2, ChanFieldType::UINT8}, +static const Table fusa_two_word_slots{{ + {sensor::ChanField::RANGE, ChanFieldType::UINT32}, + {sensor::ChanField::REFLECTIVITY, ChanFieldType::UINT8}, + {sensor::ChanField::NEAR_IR, ChanFieldType::UINT16}, + {sensor::ChanField::RANGE2, ChanFieldType::UINT32}, + {sensor::ChanField::REFLECTIVITY2, ChanFieldType::UINT8}, }}; struct DefaultFieldsEntry { - const std::pair* fields; + const std::pair* fields; size_t n_fields; }; @@ -118,8 +121,7 @@ Table default_scan_fields }}; // clang-format on -static std::vector> lookup_scan_fields( - UDPProfileLidar profile) { +static LidarScanFieldTypes lookup_scan_fields(UDPProfileLidar profile) { auto end = impl::default_scan_fields.end(); auto it = std::find_if(impl::default_scan_fields.begin(), end, @@ -129,15 +131,23 @@ static std::vector> lookup_scan_fields( throw std::invalid_argument("Unknown lidar udp profile"); auto entry = it->second; - return {entry.fields, entry.fields + entry.n_fields}; + LidarScanFieldTypes field_types; + for (size_t i = 0; i < entry.n_fields; i++) { + // everything is pixel field for now + field_types.emplace_back(entry.fields[i].first, entry.fields[i].second, + std::vector(), + FieldClass::PIXEL_FIELD); + } + return field_types; } bool raw_headers_enabled(const sensor::packet_format& pf, const LidarScan& ls) { using ouster::sensor::logger; - ChanFieldType raw_headers_ft = ls.field_type(ChanField::RAW_HEADERS); - if (!raw_headers_ft) { + if (!ls.has_field(sensor::ChanField::RAW_HEADERS)) { return false; } + + auto raw_headers_ft = ls.field(sensor::ChanField::RAW_HEADERS).tag(); // ensure that we can pack headers into the size of a single RAW_HEADERS // column if (pf.pixels_per_column * sensor::field_type_size(raw_headers_ft) < @@ -154,55 +164,99 @@ bool raw_headers_enabled(const sensor::packet_format& pf, const LidarScan& ls) { } // namespace impl +static FieldDescriptor get_field_type_descriptor(const LidarScan& scan, + const FieldType& ft) { + if (ft.field_class == FieldClass::PIXEL_FIELD) { + std::vector dims; + dims.push_back(scan.h); + dims.push_back(scan.w); + dims.insert(dims.end(), ft.extra_dims.begin(), ft.extra_dims.end()); + return FieldDescriptor::array(ft.element_type, dims); + } else if (ft.field_class == FieldClass::COLUMN_FIELD) { + std::vector dims; + dims.push_back(scan.w); + dims.insert(dims.end(), ft.extra_dims.begin(), ft.extra_dims.end()); + return FieldDescriptor::array(ft.element_type, dims); + } else if (ft.field_class == FieldClass::PACKET_FIELD) { + std::vector dims; + dims.push_back(scan.w / scan.columns_per_packet_ + + (scan.w % scan.columns_per_packet_ ? 1 : 0)); + dims.insert(dims.end(), ft.extra_dims.begin(), ft.extra_dims.end()); + return FieldDescriptor::array(ft.element_type, dims); + } else { // FieldClass::SCAN_FIELD + return FieldDescriptor::array(ft.element_type, ft.extra_dims); + } +} + // specify sensor:: namespace for doxygen matching LidarScan::LidarScan(size_t w, size_t h, LidarScanFieldTypes field_types, size_t columns_per_packet) - : timestamp_{Header::Zero(w)}, - packet_timestamp_{Header::Zero(w / columns_per_packet)}, - measurement_id_{Header::Zero(w)}, - status_{Header::Zero(w)}, - pose_(w, mat4d::Identity()), - field_types_{std::move(field_types)}, - w{static_cast(w)}, - h{static_cast(h)} { - for (const auto& ft : field_types_) { - if (fields_.count(ft.first) > 0) - throw std::invalid_argument("Duplicated fields found"); - fields_[ft.first] = impl::FieldSlot{ft.second, w, h}; + : w{w}, h{h}, columns_per_packet_(columns_per_packet) { + if (w * h == 0) { + throw std::invalid_argument( + "Cannot construct non-empty LidarScan with " + "zero width or height"); + } + + for (const auto& ft : field_types) { + add_field(ft); + } + + timestamp_ = Field{fd_array(w), FieldClass::COLUMN_FIELD}; + measurement_id_ = Field{fd_array(w), FieldClass::COLUMN_FIELD}; + status_ = Field{fd_array(w), FieldClass::COLUMN_FIELD}; + packet_timestamp_ = + Field{fd_array(w / columns_per_packet + + (w % columns_per_packet ? 1 : 0)), + FieldClass::PACKET_FIELD}; + pose_ = Field{fd_array(w, 4, 4), {}}; + /** + * These may be unnecessary to set to identity + */ + for (size_t i = 0; i < w; ++i) { + Eigen::Ref> pose = pose_.subview(i); + pose = mat4d::Identity(); } } LidarScan::LidarScan(const LidarScan& ls_src, const LidarScanFieldTypes& field_types) - : timestamp_(ls_src.timestamp_), - packet_timestamp_(ls_src.packet_timestamp_), - measurement_id_(ls_src.measurement_id_), - status_(ls_src.status_), - pose_(ls_src.pose_), - field_types_(field_types), - w(ls_src.w), + : w(ls_src.w), h(ls_src.h), + columns_per_packet_(ls_src.columns_per_packet_), frame_status(ls_src.frame_status), frame_id(ls_src.frame_id) { - // Initialize fields - for (const auto& ft : field_types_) { - if (fields_.count(ft.first) > 0) - throw std::invalid_argument("Duplicated fields found"); - fields_[ft.first] = impl::FieldSlot{ft.second, static_cast(w), - static_cast(h)}; - } - - // Copy fields for (const auto& ft : field_types) { - if (ls_src.field_type(ft.first)) { - ouster::impl::visit_field(*this, ft.first, - ouster::impl::copy_and_cast(), ls_src, - ft.first); + const std::string& name = ft.name; + FieldDescriptor dst_desc = get_field_type_descriptor(*this, ft); + if (ls_src.has_field(name)) { + // either copy cast or error + const auto& src_field = ls_src.field(name); + const auto& src_desc = src_field.desc(); + if (src_desc == dst_desc) { + fields()[name] = ls_src.field(name); + } else { + // cast if the dimensions match + if (dst_desc.shape != src_desc.shape) { + throw std::invalid_argument( + "Field '" + name + + "' from source scan has dimensions that don't match " + "desired."); + } + add_field(name, dst_desc, ft.field_class); + ouster::impl::visit_field( + *this, name, ouster::impl::copy_and_cast(), ls_src, name); + } } else { - ouster::impl::visit_field(*this, ft.first, - ouster::impl::zero_field()); + add_field(name, dst_desc, ft.field_class); } } + + timestamp_ = ls_src.timestamp_; + measurement_id_ = ls_src.measurement_id_; + status_ = ls_src.status_; + packet_timestamp_ = ls_src.packet_timestamp_; + pose_ = ls_src.pose_; } LidarScan::LidarScan(size_t w, size_t h, sensor::UDPProfileLidar profile, @@ -226,35 +280,142 @@ sensor::ThermalShutdownStatus LidarScan::thermal_shutdown() const { frame_status_shifts::FRAME_STATUS_THERMAL_SHUTDOWN_SHIFT); } -template ::value, T>::type> -Eigen::Ref> LidarScan::field(ChanField f) { - return fields_.at(f).get(); +Field& LidarScan::field(const std::string& name) { return fields().at(name); } + +const Field& LidarScan::field(const std::string& name) const { + return fields().at(name); +} + +bool LidarScan::has_field(const std::string& name) const { + return fields().count(name) > 0; +} + +Field& LidarScan::add_field(const FieldType& type) { + if (has_field(type.name) > 0) { + throw std::invalid_argument("Duplicated field '" + type.name + "'"); + } + + // no other checking is necessary since the user isnt providing dimensions + // that need validation + fields()[type.name] = + Field(get_field_type_descriptor(*this, type), type.field_class); + + return fields()[type.name]; +} + +Field& LidarScan::add_field(const std::string& name, FieldDescriptor desc, + FieldClass field_class) { + if (has_field(name) > 0) + throw std::invalid_argument("Duplicated field '" + name + "'"); + + if (field_class == FieldClass::PIXEL_FIELD) { + if (desc.shape.size() < 2) + throw std::invalid_argument( + "Pixel fields must have at least 2 dimensions"); + if (desc.shape[0] != h || desc.shape[1] != w) + throw std::invalid_argument( + "Pixel field shape must match " + "LidarScan's width and height. Was " + + std::to_string(desc.shape[0]) + "x" + + std::to_string(desc.shape[1]) + " vs " + std::to_string(h) + + "x" + std::to_string(w)); + } + + if (field_class == FieldClass::COLUMN_FIELD) { + if (desc.shape[0] != w) + throw std::invalid_argument( + "Column field shape must match " + "LidarScan's height. Width was " + + std::to_string(desc.shape[0]) + " vs required width of " + + std::to_string(w)); + } + + if (field_class == FieldClass::PACKET_FIELD) { + const size_t desired_w = + w / columns_per_packet_ + (w % columns_per_packet_ ? 1 : 0); + if (desc.shape[0] != desired_w) + throw std::invalid_argument( + "Packet field shape must match " + "number of packets. Width was " + + std::to_string(desc.shape[0]) + " vs required width of " + + std::to_string(desired_w)); + } + + fields()[name] = Field{desc, field_class}; + + return fields()[name]; +} + +// TODO: verify this is sane with python bindings, might be hard to keep alive +Field LidarScan::del_field(const std::string& name) { + if (!has_field(name)) + throw std::invalid_argument("Attempted deleting non existing field '" + + name + "'"); + + Field ptr; + field(name).swap(ptr); + fields().erase(name); + return ptr; +} + +template +Eigen::Ref> LidarScan::field(const std::string& name) { + return field(name); } -template ::value, T>::type> -Eigen::Ref> LidarScan::field(ChanField f) const { - return fields_.at(f).get(); +template +Eigen::Ref> LidarScan::field(const std::string& name) const { + return field(name); } // explicitly instantiate for each supported field type -template Eigen::Ref> LidarScan::field(ChanField f); -template Eigen::Ref> LidarScan::field(ChanField f); -template Eigen::Ref> LidarScan::field(ChanField f); -template Eigen::Ref> LidarScan::field(ChanField f); -template Eigen::Ref> LidarScan::field(ChanField f) const; -template Eigen::Ref> LidarScan::field(ChanField f) const; -template Eigen::Ref> LidarScan::field(ChanField f) const; -template Eigen::Ref> LidarScan::field(ChanField f) const; -ChanFieldType LidarScan::field_type(ChanField f) const { - return fields_.count(f) ? fields_.at(f).tag : ChanFieldType::VOID; +// clang-format off +template Eigen::Ref> LidarScan::field(const std::string& f); +template Eigen::Ref> LidarScan::field(const std::string& f); +template Eigen::Ref> LidarScan::field(const std::string& f); +template Eigen::Ref> LidarScan::field(const std::string& f); +template Eigen::Ref> LidarScan::field(const std::string& f); +template Eigen::Ref> LidarScan::field(const std::string& f); +template Eigen::Ref> LidarScan::field(const std::string& f); +template Eigen::Ref> LidarScan::field(const std::string& f); +template Eigen::Ref> LidarScan::field(const std::string& f); +template Eigen::Ref> LidarScan::field(const std::string& f); +template Eigen::Ref> LidarScan::field(const std::string& f) const; +template Eigen::Ref> LidarScan::field(const std::string& f) const; +template Eigen::Ref> LidarScan::field(const std::string& f) const; +template Eigen::Ref> LidarScan::field(const std::string& f) const; +template Eigen::Ref> LidarScan::field(const std::string& f) const; +template Eigen::Ref> LidarScan::field(const std::string& f) const; +template Eigen::Ref> LidarScan::field(const std::string& f) const; +template Eigen::Ref> LidarScan::field(const std::string& f) const; +template Eigen::Ref> LidarScan::field(const std::string& f) const; +template Eigen::Ref> LidarScan::field(const std::string& f) const; +// clang-format on + +static FieldType get_field_type(const std::string& name, const Field& field) { + int offset = 0; + if (field.field_class() == FieldClass::PIXEL_FIELD) { + offset = 2; + } else if ((field.field_class() == FieldClass::COLUMN_FIELD) || + (field.field_class() == FieldClass::PACKET_FIELD)) { + offset = 1; + } + std::vector extra_dims; + extra_dims.insert(extra_dims.begin(), field.shape().begin() + offset, + field.shape().end()); + return FieldType(name, field.tag(), extra_dims, field.field_class()); } -LidarScan::FieldIter LidarScan::begin() const { return field_types_.cbegin(); } +FieldType LidarScan::field_type(const std::string& name) const { + return get_field_type(name, field(name)); +} + +std::unordered_map& LidarScan::fields() { return fields_; } -LidarScan::FieldIter LidarScan::end() const { return field_types_.cend(); } +const std::unordered_map& LidarScan::fields() const { + return fields_; +} Eigen::Ref> LidarScan::timestamp() { return timestamp_; @@ -267,6 +428,7 @@ Eigen::Ref> LidarScan::timestamp() const { Eigen::Ref> LidarScan::packet_timestamp() { return packet_timestamp_; } + Eigen::Ref> LidarScan::packet_timestamp() const { return packet_timestamp_; @@ -274,9 +436,6 @@ Eigen::Ref> LidarScan::packet_timestamp() uint64_t LidarScan::get_first_valid_packet_timestamp() const { int total_packets = packet_timestamp().size(); - if (total_packets == 0) { - return 0; // prevent a divide by zero - } int columns_per_packet = w / total_packets; for (int i = 0; i < total_packets; ++i) { @@ -293,19 +452,21 @@ uint64_t LidarScan::get_first_valid_packet_timestamp() const { Eigen::Ref> LidarScan::measurement_id() { return measurement_id_; } + Eigen::Ref> LidarScan::measurement_id() const { return measurement_id_; } Eigen::Ref> LidarScan::status() { return status_; } + Eigen::Ref> LidarScan::status() const { return status_; } -std::vector& LidarScan::pose() { return pose_; } +Field& LidarScan::pose() { return pose_; } -const std::vector& LidarScan::pose() const { return pose_; } +const Field& LidarScan::pose() const { return pose_; } bool LidarScan::complete(sensor::ColumnWindow window) const { const auto& status = this->status(); @@ -328,15 +489,21 @@ bool LidarScan::complete(sensor::ColumnWindow window) const { bool operator==(const LidarScan& a, const LidarScan& b) { return a.frame_id == b.frame_id && a.w == b.w && a.h == b.h && - a.frame_status == b.frame_status && a.fields_ == b.fields_ && - a.field_types_ == b.field_types_ && - (a.timestamp() == b.timestamp()).all() && - (a.measurement_id() == b.measurement_id()).all() && - (a.status() == b.status()).all() && a.pose() == b.pose(); + a.frame_status == b.frame_status && + a.measurement_id_ == b.measurement_id_ && + a.timestamp_ == b.timestamp_ && + a.packet_timestamp_ == b.packet_timestamp_ && a.pose() == b.pose() && + a.fields() == b.fields(); } -LidarScanFieldTypes get_field_types(const LidarScan& ls) { - return {ls.begin(), ls.end()}; +LidarScanFieldTypes LidarScan::field_types() const { + LidarScanFieldTypes ft; + for (const auto& kv : fields()) { + ft.push_back(get_field_type(kv.first, kv.second)); + } + + std::sort(ft.begin(), ft.end()); + return ft; } LidarScanFieldTypes get_field_types(UDPProfileLidar udp_profile_lidar) { @@ -349,13 +516,27 @@ LidarScanFieldTypes get_field_types(const sensor::sensor_info& info) { return get_field_types(info.format.udp_profile_lidar); } +std::string to_string(const FieldType& field_type) { + std::string out = field_type.name; + out += ": "; + out += to_string(field_type.element_type); + out += " ("; + int j = 0; + for (const auto& d : field_type.extra_dims) { + if (j++ > 0) out += ", "; + out += std::to_string(d); + } + out += ") "; + out += ouster::to_string(field_type.field_class); + return out; +} + std::string to_string(const LidarScanFieldTypes& field_types) { std::stringstream ss; ss << "("; for (size_t i = 0; i < field_types.size(); ++i) { if (i > 0) ss << ", "; - ss << sensor::to_string(field_types[i].first) << ":" - << sensor::to_string(field_types[i].second); + ss << to_string(field_types[i]); } ss << ")"; return ss.str(); @@ -363,43 +544,44 @@ std::string to_string(const LidarScanFieldTypes& field_types) { std::string to_string(const LidarScan& ls) { std::stringstream ss; - LidarScanFieldTypes field_types(ls.begin(), ls.end()); + LidarScanFieldTypes field_types = ls.field_types(); ss << "LidarScan: {h = " << ls.h << ", w = " << ls.w << ", packets_per_frame = " << ls.packet_timestamp().size() << ", fid = " << ls.frame_id << "," << std::endl - << " frame status = " << std::hex << ls.frame_status << std::dec + << " frame status = " << std::hex << ls.frame_status << std::dec << ", thermal_shutdown status = " << to_string(ls.thermal_shutdown()) << ", shot_limiting status = " << to_string(ls.shot_limiting()) << "," << std::endl << " field_types = " << to_string(field_types) << "," << std::endl; - if (!field_types.empty()) { - ss << " fields = [" << std::endl; - img_t key{ls.h, ls.w}; - for (const auto& ft : ls) { - impl::visit_field(ls, ft.first, impl::read_and_cast(), key); - ss << " " << to_string(ft.first) << ":" << to_string(ft.second) - << " = ("; - ss << key.minCoeff() << "; " << key.mean() << "; " - << key.maxCoeff(); - ss << ")," << std::endl; + auto read_eigen = [](auto ref, std::stringstream& ss) { + ss << "min: " << (double)ref.minCoeff() + << "; mean: " << ref.template cast().mean() + << "; max: " << (double)ref.maxCoeff(); + }; + + auto read_field = [&read_eigen](const Field& f, const std::string& name, + std::stringstream& ss) { + ss << " " << name << " type:" << to_string(f.tag()) << " shape: ("; + + const auto& shape = f.shape(); + for (size_t i = 0; i < shape.size(); ++i) { + ss << shape[i]; + if (i < shape.size() - 1) { + ss << ", "; + } } - ss << " ]," << std::endl; - } - - const auto& ts = ls.timestamp(); - ss << " timestamp = (" << ts.minCoeff() << "; " << ts.mean() << "; " - << ts.maxCoeff() << ")," << std::endl; - const auto& packet_ts = ls.packet_timestamp(); - ss << " packet_timestamp = (" << packet_ts.minCoeff() << "; " - << packet_ts.mean() << "; " << packet_ts.maxCoeff() << ")," << std::endl; - const auto& mid = ls.measurement_id().cast(); - ss << " measurement_id = (" << mid.minCoeff() << "; " << mid.mean() << "; " - << mid.maxCoeff() << ")," << std::endl; - const auto& st = ls.status().cast(); - ss << " status = (" << st.minCoeff() << "; " << st.mean() << "; " - << st.maxCoeff() << ")" << std::endl; - ss << " poses = (size: " << ls.pose().size() << ")" << std::endl; + ss << ") "; + + FieldView flat_view = f.reshape(1, f.size()); + impl::visit_field_2d(flat_view, read_eigen, ss); + ss << std::endl; + }; + + for (auto&& kv : ls.fields()) { + read_field(kv.second, kv.first, ss); + } + ss << "}"; return ss.str(); } @@ -492,7 +674,7 @@ XYZLut make_xyz_lut(size_t w, size_t h, double range_unit, } LidarScan::Points cartesian(const LidarScan& scan, const XYZLut& lut) { - return cartesian(scan.field(ChanField::RANGE), lut); + return cartesian(scan.field(sensor::ChanField::RANGE), lut); } LidarScan::Points cartesian(const Eigen::Ref>& range, @@ -529,8 +711,8 @@ namespace { */ struct zero_field_cols { template - void operator()(Eigen::Ref> field, ChanField, std::ptrdiff_t start, - std::ptrdiff_t end) const { + void operator()(Eigen::Ref> field, const std::string&, + std::ptrdiff_t start, std::ptrdiff_t end) const { field.block(0, start, field.rows(), end - start).setZero(); } }; @@ -550,16 +732,13 @@ void zero_header_cols(LidarScan& ls, std::ptrdiff_t start, std::ptrdiff_t end) { */ struct parse_field_col { template - void operator()(Eigen::Ref> field, ChanField f, uint16_t m_id, - const sensor::packet_format& pf, + void operator()(Eigen::Ref> field, const std::string& f, + uint16_t m_id, const sensor::packet_format& pf, const uint8_t* col_buf) const { - // user defined fields that we shouldn't change - if (f >= ChanField::CUSTOM0 && f <= ChanField::CUSTOM9) return; - // RAW_HEADERS field is populated separately because it has // a different processing scheme and doesn't fit into existing field // model (i.e. data packed per column rather than per pixel) - if (f == ChanField::RAW_HEADERS) return; + if (f == sensor::ChanField::RAW_HEADERS) return; pf.col_field(col_buf, f, field.col(m_id).data(), field.cols()); } @@ -587,7 +766,7 @@ uint64_t frame_status(const uint8_t thermal_shutdown, */ struct pack_raw_headers_col { template - void operator()(Eigen::Ref> rh_field, ChanField, + void operator()(Eigen::Ref> rh_field, const std::string&, const sensor::packet_format& pf, uint16_t col_idx, const uint8_t* packet_buf) const { const uint8_t* col_buf = pf.nth_col(col_idx, packet_buf); @@ -643,15 +822,15 @@ void ScanBatcher::_parse_by_col(const uint8_t* packet_buf, LidarScan& ls) { if (raw_headers) { // zero out missing columns if we jumped forward if (m_id >= next_headers_m_id) { - impl::visit_field(ls, ChanField::RAW_HEADERS, zero_field_cols(), - ChanField::RAW_HEADERS, next_headers_m_id, + impl::visit_field(ls, sensor::ChanField::RAW_HEADERS, + zero_field_cols{}, "", next_headers_m_id, m_id); next_headers_m_id = m_id + 1; } - impl::visit_field(ls, ChanField::RAW_HEADERS, - pack_raw_headers_col(), ChanField::RAW_HEADERS, - pf, icol, packet_buf); + impl::visit_field( + ls, sensor::ChanField::RAW_HEADERS, pack_raw_headers_col(), + sensor::ChanField::RAW_HEADERS, pf, icol, packet_buf); } // drop invalid @@ -659,14 +838,8 @@ void ScanBatcher::_parse_by_col(const uint8_t* packet_buf, LidarScan& ls) { // zero out missing columns if we jumped forward if (m_id >= next_valid_m_id) { - for (const auto& field_type : ls) { - if (field_type.first == ChanField::RAW_HEADERS) continue; - if (field_type.first >= ChanField::CUSTOM0 && - field_type.first <= ChanField::CUSTOM9) - continue; - impl::visit_field(ls, field_type.first, zero_field_cols(), - field_type.first, next_valid_m_id, m_id); - } + impl::foreach_channel_field(ls, pf, zero_field_cols{}, + next_valid_m_id, m_id); zero_header_cols(ls, next_valid_m_id, m_id); next_valid_m_id = m_id + 1; } @@ -676,7 +849,8 @@ void ScanBatcher::_parse_by_col(const uint8_t* packet_buf, LidarScan& ls) { ls.measurement_id()[m_id] = m_id; ls.status()[m_id] = status; - impl::foreach_field(ls, parse_field_col(), m_id, pf, col_buf); + impl::foreach_channel_field(ls, pf, parse_field_col{}, m_id, pf, + col_buf); } } @@ -687,12 +861,9 @@ void ScanBatcher::_parse_by_col(const uint8_t* packet_buf, LidarScan& ls) { template struct parse_field_block { template - void operator()(Eigen::Ref> field, ChanField f, + void operator()(Eigen::Ref> field, const std::string& f, const sensor::packet_format& pf, const uint8_t* packet_buf) const { - // user defined fields that we shouldn't change - if (f >= ChanField::CUSTOM0 && f <= ChanField::CUSTOM9) return; - pf.block_field(field, f, packet_buf); } }; @@ -702,13 +873,8 @@ void ScanBatcher::_parse_by_block(const uint8_t* packet_buf, LidarScan& ls) { const uint16_t first_m_id = pf.col_measurement_id(pf.nth_col(0, packet_buf)); if (first_m_id >= next_valid_m_id) { - for (const auto& field_type : ls) { - if (field_type.first >= ChanField::CUSTOM0 && - field_type.first <= ChanField::CUSTOM9) - continue; - impl::visit_field(ls, field_type.first, zero_field_cols(), - field_type.first, next_valid_m_id, first_m_id); - } + impl::foreach_channel_field(ls, pf, zero_field_cols{}, next_valid_m_id, + first_m_id); zero_header_cols(ls, next_valid_m_id, first_m_id); next_valid_m_id = first_m_id + pf.columns_per_packet; } @@ -727,13 +893,16 @@ void ScanBatcher::_parse_by_block(const uint8_t* packet_buf, LidarScan& ls) { switch (pf.block_parsable()) { case 16: - impl::foreach_field(ls, parse_field_block<16>{}, pf, packet_buf); + impl::foreach_channel_field(ls, pf, parse_field_block<16>{}, pf, + packet_buf); break; case 8: - impl::foreach_field(ls, parse_field_block<8>{}, pf, packet_buf); + impl::foreach_channel_field(ls, pf, parse_field_block<8>{}, pf, + packet_buf); break; case 4: - impl::foreach_field(ls, parse_field_block<4>{}, pf, packet_buf); + impl::foreach_channel_field(ls, pf, parse_field_block<4>{}, pf, + packet_buf); break; default: throw std::invalid_argument("Invalid block dim for packet format"); @@ -753,7 +922,8 @@ bool ScanBatcher::operator()(const uint8_t* packet_buf, uint64_t packet_ts, LidarScan& ls) { if (ls.w != w || ls.h != h) throw std::invalid_argument("unexpected scan dimensions"); - if (ls.packet_timestamp().rows() != ls.w / pf.columns_per_packet) + if (static_cast(ls.packet_timestamp().rows()) != + ls.w / pf.columns_per_packet) throw std::invalid_argument("unexpected scan columns_per_packet: " + std::to_string(pf.columns_per_packet)); @@ -764,7 +934,7 @@ bool ScanBatcher::operator()(const uint8_t* packet_buf, uint64_t packet_ts, this->operator()(cache.data(), cache_packet_ts, ls); } - const uint64_t f_id = pf.frame_id(packet_buf); + const int64_t f_id = pf.frame_id(packet_buf); const bool raw_headers = impl::raw_headers_enabled(pf, ls); @@ -778,21 +948,18 @@ bool ScanBatcher::operator()(const uint8_t* packet_buf, uint64_t packet_ts, const uint8_t f_thermal_shutdown = pf.thermal_shutdown(packet_buf); const uint8_t f_shot_limiting = pf.shot_limiting(packet_buf); ls.frame_status = frame_status(f_thermal_shutdown, f_shot_limiting); - } else if (ls.frame_id == ((f_id + 1) % (pf.max_frame_id + 1))) { + } else if (ls.frame_id == + ((f_id + 1) % (static_cast(pf.max_frame_id) + 1))) { // drop reordered packets from the previous frame return false; } else if (ls.frame_id != f_id) { // got a packet from a new frame - for (const auto& field_type : ls) { - auto end_m_id = next_valid_m_id; - if (raw_headers && field_type.first == ChanField::RAW_HEADERS) { - end_m_id = next_headers_m_id; - } - if (field_type.first >= ChanField::CUSTOM0 && - field_type.first <= ChanField::CUSTOM9) - continue; - impl::visit_field(ls, field_type.first, zero_field_cols(), - field_type.first, end_m_id, w); + impl::foreach_channel_field(ls, pf, zero_field_cols{}, next_valid_m_id, + w); + + if (raw_headers) { + impl::visit_field(ls, sensor::ChanField::RAW_HEADERS, + zero_field_cols{}, "", next_headers_m_id, w); } // store packet buf and ts data to the cache for later processing @@ -834,4 +1001,24 @@ bool ScanBatcher::operator()(const uint8_t* packet_buf, uint64_t packet_ts, return false; } +FieldType::FieldType() {} + +FieldType::FieldType(const std::string& name_, + sensor::ChanFieldType element_type_, + const std::vector extra_dims_, FieldClass class_) + : name(name_), + element_type(element_type_), + extra_dims(extra_dims_), + field_class(class_) {} + +bool operator==(const FieldType& a, const FieldType& b) { + return a.name == b.name && a.element_type == b.element_type && + a.field_class == b.field_class && a.extra_dims == b.extra_dims; +} + +bool operator!=(const FieldType& a, const FieldType& b) { + return a.name != b.name || a.element_type != b.element_type || + a.field_class != b.field_class || a.extra_dims != b.extra_dims; +} + } // namespace ouster diff --git a/ouster_client/src/logging.cpp b/ouster_client/src/logging.cpp index e62f6cb4..16ea6a87 100644 --- a/ouster_client/src/logging.cpp +++ b/ouster_client/src/logging.cpp @@ -1,20 +1,10 @@ -#include "logging.h" +#include "ouster/impl/logging.h" -#include - -#if (SPDLOG_VER_MAJOR < 1) -namespace spdlog { -namespace sinks { -// rename simple_file_sink_st to basic_file_sink_st -using basic_file_sink_st = simple_file_sink_st; -} // namespace sinks -} // namespace spdlog -#else #include +#include #include -#endif - #include +#include #include @@ -36,32 +26,20 @@ Logger::Logger() logger_->flush_on(spdlog::level::info); } -void Logger::update_sink_and_log_level(spdlog::sink_ptr sink, - const std::string& log_level) { -#if (SPDLOG_VER_MAJOR < 1) - // recreate the logger with the new sink - logger_ = std::make_unique(logger_name, sink); - - using namespace spdlog::level; - auto idx = std::find(std::begin(level_names), std::end(level_names), - log_level.c_str()); - auto ll = idx == std::end(level_names) - ? spdlog::level::off - : static_cast( - std::distance(std::begin(level_names), idx)); -#else +void Logger::configure_generic_sink(spdlog::sink_ptr sink, + const std::string& log_level) { // replace the logger sink with the new sink logger_->sinks() = {sink}; auto ll = spdlog::level::from_str(log_level); -#endif + logger_->set_level(ll); logger_->flush_on(ll); } bool Logger::configure_stdout_sink(const std::string& log_level) { try { - update_sink_and_log_level( + configure_generic_sink( std::make_shared(), log_level); } catch (const spdlog::spdlog_ex& ex) { std::cerr << logger_name << " init_logger failed: " << ex.what() @@ -86,7 +64,7 @@ bool Logger::configure_file_sink(const std::string& log_level, file_sink = std::make_shared( log_file_path, true); } - update_sink_and_log_level(file_sink, log_level); + configure_generic_sink(file_sink, log_level); } catch (const spdlog::spdlog_ex& ex) { std::cerr << logger_name << " init_logger failed: " << ex.what() << std::endl; diff --git a/ouster_client/src/parsing.cpp b/ouster_client/src/parsing.cpp index a88180f1..fb9136eb 100644 --- a/ouster_client/src/parsing.cpp +++ b/ouster_client/src/parsing.cpp @@ -37,12 +37,12 @@ struct FieldInfo { }; struct ProfileEntry { - const std::pair* fields; + const std::pair* fields; size_t n_fields; size_t chan_data_size; }; -static const Table legacy_field_info{{ +static const Table legacy_field_info{{ {ChanField::RANGE, {UINT32, 0, 0x000fffff, 0}}, {ChanField::FLAGS, {UINT8, 3, 0, 4}}, {ChanField::REFLECTIVITY, {UINT16, 4, 0, 0}}, @@ -53,7 +53,7 @@ static const Table legacy_field_info{{ {ChanField::RAW32_WORD3, {UINT32, 8, 0, 0}}, }}; -static const Table lb_field_info{{ +static const Table lb_field_info{{ {ChanField::RANGE, {UINT32, 0, 0x7fff, -3}}, {ChanField::FLAGS, {UINT8, 1, 0b10000000, 7}}, {ChanField::REFLECTIVITY, {UINT8, 2, 0, 0}}, @@ -61,7 +61,7 @@ static const Table lb_field_info{{ {ChanField::RAW32_WORD1, {UINT32, 0, 0, 0}}, }}; -static const Table dual_field_info{{ +static const Table dual_field_info{{ {ChanField::RANGE, {UINT32, 0, 0x0007ffff, 0}}, {ChanField::FLAGS, {UINT8, 2, 0b11111000, 3}}, {ChanField::REFLECTIVITY, {UINT8, 3, 0, 0}}, @@ -77,7 +77,7 @@ static const Table dual_field_info{{ {ChanField::RAW32_WORD4, {UINT32, 12, 0, 0}}, }}; -static const Table single_field_info{{ +static const Table single_field_info{{ {ChanField::RANGE, {UINT32, 0, 0x0007ffff, 0}}, {ChanField::FLAGS, {UINT8, 2, 0b11111000, 3}}, {ChanField::REFLECTIVITY, {UINT8, 4, 0, 0}}, @@ -88,7 +88,7 @@ static const Table single_field_info{{ {ChanField::RAW32_WORD3, {UINT32, 8, 0, 0}}, }}; -static const Table five_word_pixel_info{{ +static const Table five_word_pixel_info{{ {ChanField::RANGE, {UINT32, 0, 0x0007ffff, 0}}, {ChanField::FLAGS, {UINT8, 2, 0b11111000, 3}}, {ChanField::REFLECTIVITY, {UINT8, 3, 0, 0}}, @@ -105,7 +105,7 @@ static const Table five_word_pixel_info{{ {ChanField::RAW32_WORD5, {UINT32, 16, 0, 0}}, }}; -static const Table fusa_two_word_pixel_info{{ +static const Table fusa_two_word_pixel_info{{ {ChanField::RANGE, {UINT32, 0, 0x7fff, -3}}, {ChanField::FLAGS, {UINT8, 1, 0b10000000, 7}}, {ChanField::REFLECTIVITY, {UINT8, 2, 0xff, 0}}, @@ -191,7 +191,7 @@ struct packet_format::Impl { size_t measurement_id_offset; size_t status_offset; - std::map fields; + std::map fields; Impl(UDPProfileLidar profile, size_t pixels_per_column, size_t columns_per_packet) { @@ -254,8 +254,27 @@ packet_format::packet_format(const sensor_info& info) info.format.pixels_per_column, info.format.columns_per_packet) {} +template +class SameSizeInt { + public: + typedef T value; +}; + +template <> +class SameSizeInt { + public: + typedef uint32_t value; +}; + +template <> +class SameSizeInt { + public: + typedef uint64_t value; +}; + template -void packet_format::block_field_impl(Eigen::Ref> field, ChanField chan, +void packet_format::block_field_impl(Eigen::Ref> field, + const std::string& chan, const uint8_t* packet_buf) const { if (sizeof(T) < sizeof(SRC)) throw std::invalid_argument("Dest type too small for specified field"); @@ -283,7 +302,9 @@ void packet_format::block_field_impl(Eigen::Ref> field, ChanField chan, for (int x = 0; x < N; ++x) { auto px_src = col_buf[x] + col_header_size + (px * channel_data_size); - T dst = *reinterpret_cast(px_src + offset); + typename SameSizeInt::value dst = + *reinterpret_cast::value*>( + px_src + offset); if (mask) dst &= mask; if (shift > 0) dst >>= shift; if (shift < 0) dst <<= std::abs(shift); @@ -293,9 +314,9 @@ void packet_format::block_field_impl(Eigen::Ref> field, ChanField chan, } } -template ::value, T>::type> -void packet_format::block_field(Eigen::Ref> field, ChanField chan, +template +void packet_format::block_field(Eigen::Ref> field, + const std::string& chan, const uint8_t* packet_buf) const { const auto& f = impl_->fields.at(chan); @@ -312,6 +333,24 @@ void packet_format::block_field(Eigen::Ref> field, ChanField chan, case UINT64: block_field_impl(field, chan, packet_buf); break; + case INT8: + block_field_impl(field, chan, packet_buf); + break; + case INT16: + block_field_impl(field, chan, packet_buf); + break; + case INT32: + block_field_impl(field, chan, packet_buf); + break; + case INT64: + block_field_impl(field, chan, packet_buf); + break; + case FLOAT32: + block_field_impl(field, chan, packet_buf); + break; + case FLOAT64: + block_field_impl(field, chan, packet_buf); + break; default: throw std::invalid_argument("Invalid field for packet format"); } @@ -319,40 +358,94 @@ void packet_format::block_field(Eigen::Ref> field, ChanField chan, // explicitly instantiate for each field type / block dim template void packet_format::block_field( - Eigen::Ref> field, ChanField chan, + Eigen::Ref> field, const std::string& chan, const uint8_t* packet_buf) const; template void packet_format::block_field( - Eigen::Ref> field, ChanField chan, + Eigen::Ref> field, const std::string& chan, const uint8_t* packet_buf) const; template void packet_format::block_field( - Eigen::Ref> field, ChanField chan, + Eigen::Ref> field, const std::string& chan, const uint8_t* packet_buf) const; template void packet_format::block_field( - Eigen::Ref> field, ChanField chan, + Eigen::Ref> field, const std::string& chan, + const uint8_t* packet_buf) const; +template void packet_format::block_field( + Eigen::Ref> field, const std::string& chan, + const uint8_t* packet_buf) const; +template void packet_format::block_field( + Eigen::Ref> field, const std::string& chan, + const uint8_t* packet_buf) const; +template void packet_format::block_field( + Eigen::Ref> field, const std::string& chan, + const uint8_t* packet_buf) const; +template void packet_format::block_field( + Eigen::Ref> field, const std::string& chan, + const uint8_t* packet_buf) const; +template void packet_format::block_field( + Eigen::Ref> field, const std::string& chan, + const uint8_t* packet_buf) const; +template void packet_format::block_field( + Eigen::Ref> field, const std::string& chan, const uint8_t* packet_buf) const; template void packet_format::block_field( - Eigen::Ref> field, ChanField chan, + Eigen::Ref> field, const std::string& chan, const uint8_t* packet_buf) const; template void packet_format::block_field( - Eigen::Ref> field, ChanField chan, + Eigen::Ref> field, const std::string& chan, const uint8_t* packet_buf) const; template void packet_format::block_field( - Eigen::Ref> field, ChanField chan, + Eigen::Ref> field, const std::string& chan, const uint8_t* packet_buf) const; template void packet_format::block_field( - Eigen::Ref> field, ChanField chan, + Eigen::Ref> field, const std::string& chan, + const uint8_t* packet_buf) const; +template void packet_format::block_field( + Eigen::Ref> field, const std::string& chan, + const uint8_t* packet_buf) const; +template void packet_format::block_field( + Eigen::Ref> field, const std::string& chan, + const uint8_t* packet_buf) const; +template void packet_format::block_field( + Eigen::Ref> field, const std::string& chan, + const uint8_t* packet_buf) const; +template void packet_format::block_field( + Eigen::Ref> field, const std::string& chan, + const uint8_t* packet_buf) const; +template void packet_format::block_field( + Eigen::Ref> field, const std::string& chan, + const uint8_t* packet_buf) const; +template void packet_format::block_field( + Eigen::Ref> field, const std::string& chan, const uint8_t* packet_buf) const; template void packet_format::block_field( - Eigen::Ref> field, ChanField chan, + Eigen::Ref> field, const std::string& chan, const uint8_t* packet_buf) const; template void packet_format::block_field( - Eigen::Ref> field, ChanField chan, + Eigen::Ref> field, const std::string& chan, const uint8_t* packet_buf) const; template void packet_format::block_field( - Eigen::Ref> field, ChanField chan, + Eigen::Ref> field, const std::string& chan, const uint8_t* packet_buf) const; template void packet_format::block_field( - Eigen::Ref> field, ChanField chan, + Eigen::Ref> field, const std::string& chan, + const uint8_t* packet_buf) const; +template void packet_format::block_field( + Eigen::Ref> field, const std::string& chan, + const uint8_t* packet_buf) const; +template void packet_format::block_field( + Eigen::Ref> field, const std::string& chan, + const uint8_t* packet_buf) const; +template void packet_format::block_field( + Eigen::Ref> field, const std::string& chan, + const uint8_t* packet_buf) const; +template void packet_format::block_field( + Eigen::Ref> field, const std::string& chan, + const uint8_t* packet_buf) const; +template void packet_format::block_field( + Eigen::Ref> field, const std::string& chan, + const uint8_t* packet_buf) const; +template void packet_format::block_field( + Eigen::Ref> field, const std::string& chan, const uint8_t* packet_buf) const; template @@ -367,18 +460,60 @@ static void col_field_impl(const uint8_t* col_buf, DST* dst, size_t offset, auto px_src = col_buf + col_header_size + offset + (px * channel_data_size); DST* px_dst = dst + px * dst_stride; - DST dst = *reinterpret_cast(px_src); + typename SameSizeInt::value dst = + *reinterpret_cast::value*>(px_src); if (mask) dst &= mask; if (shift > 0) dst >>= shift; if (shift < 0) dst <<= std::abs(shift); - *px_dst = dst; + *px_dst = *reinterpret_cast(&dst); } } -template ::value, T>::type> -void packet_format::col_field(const uint8_t* col_buf, ChanField i, T* dst, - int dst_stride) const { +template +static void col_field_impl(const uint8_t* col_buf, float* dst, size_t offset, + uint64_t mask, int shift, int pixels_per_column, + int dst_stride, size_t channel_data_size, + size_t col_header_size) { + if (sizeof(float) < sizeof(SRC)) + throw std::invalid_argument("Dest type too small for specified field"); + + for (int px = 0; px < pixels_per_column; px++) { + auto px_src = + col_buf + col_header_size + offset + (px * channel_data_size); + float* px_dst = dst + px * dst_stride; + typename SameSizeInt::value dst = + *reinterpret_cast::value*>(px_src); + if (mask) dst &= mask; + if (shift > 0) dst >>= shift; + if (shift < 0) dst <<= std::abs(shift); + memcpy(px_dst, &dst, sizeof(float)); + } +} + +template +static void col_field_impl(const uint8_t* col_buf, double* dst, size_t offset, + uint64_t mask, int shift, int pixels_per_column, + int dst_stride, size_t channel_data_size, + size_t col_header_size) { + if (sizeof(float) < sizeof(SRC)) + throw std::invalid_argument("Dest type too small for specified field"); + + for (int px = 0; px < pixels_per_column; px++) { + auto px_src = + col_buf + col_header_size + offset + (px * channel_data_size); + double* px_dst = dst + px * dst_stride; + typename SameSizeInt::value dst = + *reinterpret_cast::value*>(px_src); + if (mask) dst &= mask; + if (shift > 0) dst >>= shift; + if (shift < 0) dst <<= std::abs(shift); + memcpy(px_dst, &dst, sizeof(double)); + } +} + +template +void packet_format::col_field(const uint8_t* col_buf, const std::string& i, + T* dst, int dst_stride) const { const auto& f = impl_->fields.at(i); switch (f.ty_tag) { @@ -402,22 +537,64 @@ void packet_format::col_field(const uint8_t* col_buf, ChanField i, T* dst, col_buf, dst, f.offset, f.mask, f.shift, pixels_per_column, dst_stride, impl_->channel_data_size, impl_->col_header_size); break; + case INT8: + col_field_impl( + col_buf, dst, f.offset, f.mask, f.shift, pixels_per_column, + dst_stride, impl_->channel_data_size, impl_->col_header_size); + break; + case INT16: + col_field_impl( + col_buf, dst, f.offset, f.mask, f.shift, pixels_per_column, + dst_stride, impl_->channel_data_size, impl_->col_header_size); + break; + case INT32: + col_field_impl( + col_buf, dst, f.offset, f.mask, f.shift, pixels_per_column, + dst_stride, impl_->channel_data_size, impl_->col_header_size); + break; + case INT64: + col_field_impl( + col_buf, dst, f.offset, f.mask, f.shift, pixels_per_column, + dst_stride, impl_->channel_data_size, impl_->col_header_size); + break; + case FLOAT32: + col_field_impl( + col_buf, dst, f.offset, f.mask, f.shift, pixels_per_column, + dst_stride, impl_->channel_data_size, impl_->col_header_size); + break; + case FLOAT64: + col_field_impl( + col_buf, dst, f.offset, f.mask, f.shift, pixels_per_column, + dst_stride, impl_->channel_data_size, impl_->col_header_size); + break; default: throw std::invalid_argument("Invalid field for packet format"); } } // explicitly instantiate for each field type -template void packet_format::col_field(const uint8_t*, ChanField, uint8_t*, - int) const; -template void packet_format::col_field(const uint8_t*, ChanField, uint16_t*, - int) const; -template void packet_format::col_field(const uint8_t*, ChanField, uint32_t*, - int) const; -template void packet_format::col_field(const uint8_t*, ChanField, uint64_t*, - int) const; - -ChanFieldType packet_format::field_type(ChanField f) const { +template void packet_format::col_field(const uint8_t*, const std::string&, + uint8_t*, int) const; +template void packet_format::col_field(const uint8_t*, const std::string&, + uint16_t*, int) const; +template void packet_format::col_field(const uint8_t*, const std::string&, + uint32_t*, int) const; +template void packet_format::col_field(const uint8_t*, const std::string&, + uint64_t*, int) const; +template void packet_format::col_field(const uint8_t*, const std::string&, + int8_t*, int) const; +template void packet_format::col_field(const uint8_t*, const std::string&, + int16_t*, int) const; +template void packet_format::col_field(const uint8_t*, const std::string&, + int32_t*, int) const; +template void packet_format::col_field(const uint8_t*, const std::string&, + int64_t*, int) const; +template void packet_format::col_field(const uint8_t*, const std::string&, + float*, int) const; +template void packet_format::col_field(const uint8_t*, const std::string&, + double*, int) const; + +ChanFieldType packet_format::field_type(const std::string& f) const { return impl_->fields.count(f) ? impl_->fields.at(f).ty_tag : ChanFieldType::VOID; } @@ -450,7 +627,9 @@ uint16_t packet_format::packet_type(const uint8_t* lidar_buf) const { uint32_t packet_format::frame_id(const uint8_t* lidar_buf) const { if (udp_profile_lidar == UDPProfileLidar::PROFILE_LIDAR_LEGACY) { - return col_frame_id(nth_col(0, lidar_buf)); + uint16_t res = 0; + std::memcpy(&res, nth_col(0, lidar_buf) + 10, sizeof(uint16_t)); + return res; } // eUDP frame id is 16 bits, but FUSA frame id is 32 bits @@ -604,7 +783,7 @@ const uint8_t* packet_format::nth_px(int n, const uint8_t* col_buf) const { } template -T packet_format::px_field(const uint8_t* px_buf, ChanField i) const { +T packet_format::px_field(const uint8_t* px_buf, const std::string& i) const { const auto& f = impl_->fields.at(i); if (sizeof(T) < field_type_size(f.ty_tag)) @@ -707,12 +886,12 @@ const packet_format& get_format(UDPProfileLidar udp_profile_lidar, return *cache.at(k); } -uint64_t packet_format::field_value_mask(ChanField i) const { +uint64_t packet_format::field_value_mask(const std::string& i) const { const auto& f = impl_->fields.at(i); return get_value_mask(f); } -int packet_format::field_bitness(ChanField i) const { +int packet_format::field_bitness(const std::string& i) const { const auto& f = impl_->fields.at(i); return get_bitness(f); } @@ -890,25 +1069,43 @@ void packet_writer::set_prod_sn(uint8_t* lidar_buf, uint64_t sn) const { } template -void packet_writer::set_px(uint8_t* px_buf, ChanField i, T value) const { +void packet_writer::set_px(uint8_t* px_buf, const std::string& i, + T value) const { const auto& f = impl_->fields.at(i); - if (f.shift > 0) value <<= f.shift; - if (f.shift < 0) value >>= std::abs(f.shift); - if (f.mask) value &= f.mask; - T* ptr = reinterpret_cast(px_buf + f.offset); + typename SameSizeInt::value int_value; + memcpy(&int_value, &value, sizeof(T)); + if (f.shift > 0) int_value <<= f.shift; + if (f.shift < 0) int_value >>= std::abs(f.shift); + if (f.mask) int_value &= f.mask; + auto ptr = + reinterpret_cast::value*>(px_buf + f.offset); *ptr &= ~f.mask; - *ptr |= value; + *ptr |= int_value; } -template void packet_writer::set_px(uint8_t*, ChanField, uint8_t) const; -template void packet_writer::set_px(uint8_t*, ChanField, uint16_t) const; -template void packet_writer::set_px(uint8_t*, ChanField, uint32_t) const; -template void packet_writer::set_px(uint8_t*, ChanField, uint64_t) const; +template void packet_writer::set_px(uint8_t*, const std::string&, + uint8_t) const; +template void packet_writer::set_px(uint8_t*, const std::string&, + uint16_t) const; +template void packet_writer::set_px(uint8_t*, const std::string&, + uint32_t) const; +template void packet_writer::set_px(uint8_t*, const std::string&, + uint64_t) const; +template void packet_writer::set_px(uint8_t*, const std::string&, int8_t) const; +template void packet_writer::set_px(uint8_t*, const std::string&, + int16_t) const; +template void packet_writer::set_px(uint8_t*, const std::string&, + int32_t) const; +template void packet_writer::set_px(uint8_t*, const std::string&, + int64_t) const; +template void packet_writer::set_px(uint8_t*, const std::string&, float) const; +template void packet_writer::set_px(uint8_t*, const std::string&, double) const; template void packet_writer::set_block_impl(Eigen::Ref> field, - ChanField chan, uint8_t* lidar_buf) const { + const std::string& chan, + uint8_t* lidar_buf) const { constexpr int N = 32; if (columns_per_packet > N) throw std::runtime_error("Recompile set_block_impl with larger N"); @@ -942,7 +1139,8 @@ void packet_writer::set_block_impl(Eigen::Ref> field, if (shift > 0) value <<= shift; if (shift < 0) value >>= std::abs(shift); if (mask) value &= mask; - DST* ptr = reinterpret_cast(px_dst + offset); + DST* ptr = reinterpret_cast::value*>( + px_dst + offset); *ptr &= ~mask; *ptr |= value; } @@ -950,8 +1148,8 @@ void packet_writer::set_block_impl(Eigen::Ref> field, } template -void packet_writer::set_block(Eigen::Ref> field, ChanField i, - uint8_t* lidar_buf) const { +void packet_writer::set_block(Eigen::Ref> field, + const std::string& i, uint8_t* lidar_buf) const { const auto& f = impl_->fields.at(i); switch (f.ty_tag) { @@ -967,19 +1165,59 @@ void packet_writer::set_block(Eigen::Ref> field, ChanField i, case UINT64: set_block_impl(field, i, lidar_buf); break; + case INT8: + set_block_impl(field, i, lidar_buf); + break; + case INT16: + set_block_impl(field, i, lidar_buf); + break; + case INT32: + set_block_impl(field, i, lidar_buf); + break; + case INT64: + set_block_impl(field, i, lidar_buf); + break; + case FLOAT32: + set_block_impl(field, i, lidar_buf); + break; + case FLOAT64: + set_block_impl(field, i, lidar_buf); + break; default: throw std::invalid_argument("Invalid field for packet format"); } } template void packet_writer::set_block(Eigen::Ref> field, - ChanField i, uint8_t* lidar_buf) const; + const std::string& i, + uint8_t* lidar_buf) const; template void packet_writer::set_block(Eigen::Ref> field, - ChanField i, uint8_t* lidar_buf) const; + const std::string& i, + uint8_t* lidar_buf) const; template void packet_writer::set_block(Eigen::Ref> field, - ChanField i, uint8_t* lidar_buf) const; + const std::string& i, + uint8_t* lidar_buf) const; template void packet_writer::set_block(Eigen::Ref> field, - ChanField i, uint8_t* lidar_buf) const; + const std::string& i, + uint8_t* lidar_buf) const; +template void packet_writer::set_block(Eigen::Ref> field, + const std::string& i, + uint8_t* lidar_buf) const; +template void packet_writer::set_block(Eigen::Ref> field, + const std::string& i, + uint8_t* lidar_buf) const; +template void packet_writer::set_block(Eigen::Ref> field, + const std::string& i, + uint8_t* lidar_buf) const; +template void packet_writer::set_block(Eigen::Ref> field, + const std::string& i, + uint8_t* lidar_buf) const; +template void packet_writer::set_block(Eigen::Ref> field, + const std::string& i, + uint8_t* lidar_buf) const; +template void packet_writer::set_block(Eigen::Ref> field, + const std::string& i, + uint8_t* lidar_buf) const; template void packet_writer::unpack_raw_headers(Eigen::Ref> field, @@ -1032,6 +1270,18 @@ template void packet_writer::unpack_raw_headers( Eigen::Ref> field, uint8_t* lidar_buf) const; template void packet_writer::unpack_raw_headers( Eigen::Ref> field, uint8_t* lidar_buf) const; +template void packet_writer::unpack_raw_headers( + Eigen::Ref> field, uint8_t* lidar_buf) const; +template void packet_writer::unpack_raw_headers( + Eigen::Ref> field, uint8_t* lidar_buf) const; +template void packet_writer::unpack_raw_headers( + Eigen::Ref> field, uint8_t* lidar_buf) const; +template void packet_writer::unpack_raw_headers( + Eigen::Ref> field, uint8_t* lidar_buf) const; +template void packet_writer::unpack_raw_headers( + Eigen::Ref> field, uint8_t* lidar_buf) const; +template void packet_writer::unpack_raw_headers( + Eigen::Ref> field, uint8_t* lidar_buf) const; } // namespace impl } // namespace sensor diff --git a/ouster_client/src/profile_extension.cpp b/ouster_client/src/profile_extension.cpp index 86199ca3..ec9b5137 100644 --- a/ouster_client/src/profile_extension.cpp +++ b/ouster_client/src/profile_extension.cpp @@ -14,7 +14,6 @@ template using Table = std::array, N>; -using ouster::sensor::ChanField; using ouster::sensor::ChanFieldType; using ouster::sensor::UDPProfileLidar; using ouster::sensor::impl::MAX_NUM_PROFILES; @@ -25,7 +24,7 @@ namespace impl { // definition copied from lidar_scan.cpp struct DefaultFieldsEntry { - const std::pair* fields; + const std::pair* fields; size_t n_fields; }; @@ -35,7 +34,7 @@ extern Table static void extend_default_scan_fields( UDPProfileLidar profile, - const std::vector>& scan_fields) { + const std::vector>& scan_fields) { auto end = impl::default_scan_fields.end(); auto it = std::find_if(impl::default_scan_fields.begin(), end, [](const auto& kv) { return kv.first == 0; }); @@ -54,8 +53,8 @@ namespace impl { struct ExtendedProfile { UDPProfileLidar profile; std::string name; - std::vector> slots; - std::vector> fields; + std::vector> slots; + std::vector> fields; size_t chan_data_size; }; @@ -69,7 +68,7 @@ std::list extended_profiles_data{}; // definition copied from parsing.cpp struct ProfileEntry { - const std::pair* fields; + const std::pair* fields; size_t n_fields; size_t chan_data_size; }; @@ -82,7 +81,7 @@ extern Table profiles; static void extend_profile_entries( UDPProfileLidar profile, - const std::vector>& fields, + const std::vector>& fields, size_t chan_data_size) { auto end = impl::profiles.end(); auto it = std::find_if(impl::profiles.begin(), end, @@ -123,11 +122,11 @@ void extend_udp_profile_lidar_strings(UDPProfileLidar profile, *it = std::make_pair(profile, name); } -void add_custom_profile(int profile_nr, // int as UDPProfileLidar - const std::string& name, - const std::vector>& - fields, // int as ChanField - size_t chan_data_size) { +void add_custom_profile( + int profile_nr, // int as UDPProfileLidar + const std::string& name, + const std::vector>& fields, + size_t chan_data_size) { if (profile_nr == 0) throw std::invalid_argument("profile_nr of 0 are not allowed"); @@ -138,10 +137,8 @@ void add_custom_profile(int profile_nr, // int as UDPProfileLidar impl::ExtendedProfile profile{ udp_profile, name, {}, {}, chan_data_size}; for (auto&& pair : fields) { - ChanField chan = static_cast(pair.first); - - profile.slots.emplace_back(chan, pair.second.ty_tag); - profile.fields.emplace_back(chan, pair.second); + profile.slots.emplace_back(pair.first, pair.second.ty_tag); + profile.fields.emplace_back(pair.first, pair.second); } impl::extended_profiles_data.push_back(std::move(profile)); diff --git a/ouster_client/src/sensor_http.cpp b/ouster_client/src/sensor_http.cpp index 9691e798..0685b3a8 100644 --- a/ouster_client/src/sensor_http.cpp +++ b/ouster_client/src/sensor_http.cpp @@ -13,9 +13,8 @@ using namespace ouster::sensor::impl; string SensorHttp::firmware_version_string(const string& hostname, int timeout_sec) { - auto http_client = - std::make_unique("http://" + hostname, timeout_sec); - return http_client->get("api/v1/system/firmware"); + auto http_client = std::make_unique("http://" + hostname); + return http_client->get("api/v1/system/firmware", timeout_sec); } version SensorHttp::firmware_version(const string& hostname, int timeout_sec) { @@ -40,13 +39,15 @@ std::unique_ptr SensorHttp::create(const string& hostname, // FW 2.0 doesn't work properly with http return std::make_unique(hostname); case 1: - return std::make_unique(hostname, - timeout_sec); + return std::make_unique(hostname); case 2: - return std::make_unique(hostname, - timeout_sec); + case 3: + return std::make_unique(hostname); } } + if ((fw.major == 2 && fw.minor == 4) || (fw.major == 3 && fw.minor == 0)) { + return std::make_unique(hostname); + } - return std::make_unique(hostname, timeout_sec); + return std::make_unique(hostname); } diff --git a/ouster_client/src/sensor_http_imp.cpp b/ouster_client/src/sensor_http_imp.cpp index 46ee7a67..29e50126 100644 --- a/ouster_client/src/sensor_http_imp.cpp +++ b/ouster_client/src/sensor_http_imp.cpp @@ -2,151 +2,202 @@ #include "curl_client.h" +using ouster::sensor::util::UserDataAndPolicy; using std::string; using namespace ouster::sensor::impl; -SensorHttpImp::SensorHttpImp(const string& hostname, int timeout_sec) - : http_client( - std::make_unique("http://" + hostname, timeout_sec)) {} +SensorHttpImp::SensorHttpImp(const string& hostname) + : http_client(std::make_unique("http://" + hostname)) {} SensorHttpImp::~SensorHttpImp() = default; -Json::Value SensorHttpImp::metadata() const { - return get_json("api/v1/sensor/metadata"); +Json::Value SensorHttpImp::metadata(int timeout_sec) const { + return get_json("api/v1/sensor/metadata", timeout_sec); } -Json::Value SensorHttpImp::sensor_info() const { - return get_json("api/v1/sensor/metadata/sensor_info"); +Json::Value SensorHttpImp::sensor_info(int timeout_sec) const { + return get_json("api/v1/sensor/metadata/sensor_info", timeout_sec); } -string SensorHttpImp::get_config_params(bool active) const { +string SensorHttpImp::get_config_params(bool active, int timeout_sec) const { auto config_type = active ? "active" : "staged"; - return get(string("api/v1/sensor/cmd/get_config_param?args=") + - config_type); + return get(string("api/v1/sensor/cmd/get_config_param?args=") + config_type, + timeout_sec); } -void SensorHttpImp::set_config_param(const string& key, - const string& value) const { +void SensorHttpImp::set_config_param(const string& key, const string& value, + int timeout_sec) const { auto encoded_value = http_client->encode(value); // encode config params auto url = "api/v1/sensor/cmd/set_config_param?args=" + key + "+" + encoded_value; - execute(url, "\"set_config_param\""); + execute(url, "\"set_config_param\"", timeout_sec); } -Json::Value SensorHttpImp::active_config_params() const { - return get_json("api/v1/sensor/cmd/get_config_param?args=active"); +Json::Value SensorHttpImp::active_config_params(int timeout_sec) const { + return get_json("api/v1/sensor/cmd/get_config_param?args=active", + timeout_sec); } -Json::Value SensorHttpImp::staged_config_params() const { - return get_json("api/v1/sensor/cmd/get_config_param?args=staged"); +Json::Value SensorHttpImp::staged_config_params(int timeout_sec) const { + return get_json("api/v1/sensor/cmd/get_config_param?args=staged", + timeout_sec); } -void SensorHttpImp::set_udp_dest_auto() const { - execute("api/v1/sensor/cmd/set_udp_dest_auto", "{}"); +void SensorHttpImp::set_udp_dest_auto(int timeout_sec) const { + execute("api/v1/sensor/cmd/set_udp_dest_auto", "{}", timeout_sec); } -Json::Value SensorHttpImp::beam_intrinsics() const { - return get_json("api/v1/sensor/metadata/beam_intrinsics"); +Json::Value SensorHttpImp::beam_intrinsics(int timeout_sec) const { + return get_json("api/v1/sensor/metadata/beam_intrinsics", timeout_sec); } -Json::Value SensorHttpImp::imu_intrinsics() const { - return get_json("api/v1/sensor/metadata/imu_intrinsics"); +Json::Value SensorHttpImp::imu_intrinsics(int timeout_sec) const { + return get_json("api/v1/sensor/metadata/imu_intrinsics", timeout_sec); } -Json::Value SensorHttpImp::lidar_intrinsics() const { - return get_json("api/v1/sensor/metadata/lidar_intrinsics"); +Json::Value SensorHttpImp::lidar_intrinsics(int timeout_sec) const { + return get_json("api/v1/sensor/metadata/lidar_intrinsics", timeout_sec); } -Json::Value SensorHttpImp::lidar_data_format() const { - return get_json("api/v1/sensor/metadata/lidar_data_format"); +Json::Value SensorHttpImp::lidar_data_format(int timeout_sec) const { + return get_json("api/v1/sensor/metadata/lidar_data_format", timeout_sec); } -Json::Value SensorHttpImp::calibration_status() const { - return get_json("api/v1/sensor/metadata/calibration_status"); +Json::Value SensorHttpImp::calibration_status(int timeout_sec) const { + return get_json("api/v1/sensor/metadata/calibration_status", timeout_sec); } // reinitialize to activate new settings -void SensorHttpImp::reinitialize() const { - execute("api/v1/sensor/cmd/reinitialize", "{}"); +void SensorHttpImp::reinitialize(int timeout_sec) const { + execute("api/v1/sensor/cmd/reinitialize", "{}", timeout_sec); } -void SensorHttpImp::save_config_params() const { - execute("api/v1/sensor/cmd/save_config_params", "{}"); +void SensorHttpImp::save_config_params(int timeout_sec) const { + execute("api/v1/sensor/cmd/save_config_params", "{}", timeout_sec); } -string SensorHttpImp::get(const string& url) const { - return http_client->get(url); +std::string SensorHttpImp::get_user_data(int timeout_sec) const { + return get_json("api/v1/user/data", timeout_sec).asString(); } -Json::Value SensorHttpImp::get_json(const string& url) const { +UserDataAndPolicy SensorHttpImp::get_user_data_and_policy( + int timeout_sec) const { + auto json = get_json("api/v1/user/data?include_metadata=true", timeout_sec); + return {json["policy"].asString() != "clear_on_config_delete", + json["value"].asString()}; +} + +void SensorHttpImp::set_user_data(const std::string& data, + bool keep_on_config_delete, + int timeout_sec) const { + Json::StreamWriterBuilder wbuilder; + std::string json = Json::writeString(wbuilder, Json::Value(data)); + http_client->put( + string("api/v1/user/data") + + (keep_on_config_delete ? "?policy=keep_on_config_delete" : ""), + json, timeout_sec); +} + +void SensorHttpImp::delete_user_data(int timeout_sec) const { + http_client->del("api/v1/user/data", timeout_sec); +} + +string SensorHttpImp::get(const string& url, int timeout_sec) const { + return http_client->get(url, timeout_sec); +} + +Json::Value SensorHttpImp::get_json(const string& url, int timeout_sec) const { Json::CharReaderBuilder builder; auto reader = std::unique_ptr{builder.newCharReader()}; Json::Value root; - auto result = get(url); + auto result = get(url, timeout_sec); if (!reader->parse(result.c_str(), result.c_str() + result.size(), &root, nullptr)) throw std::runtime_error("SensorHttpImp::get_json failed! url: " + url); return root; } -void SensorHttpImp::execute(const string& url, const string& validation) const { - auto result = get(url); +void SensorHttpImp::execute(const string& url, const string& validation, + int timeout_sec) const { + auto result = get(url, timeout_sec); if (result != validation) throw std::runtime_error("SensorHttpImp::execute failed! url: " + url + " returned [" + result + "], expected [" + validation + "]"); } -SensorHttpImp_2_2::SensorHttpImp_2_2(const string& hostname, int timeout_sec) - : SensorHttpImp(hostname, timeout_sec) {} +SensorHttpImp_2_2::SensorHttpImp_2_2(const string& hostname) + : SensorHttpImp_2_4_or_3(hostname) {} -void SensorHttpImp_2_2::set_udp_dest_auto() const { +void SensorHttpImp_2_2::set_udp_dest_auto(int timeout_sec) const { return execute("api/v1/sensor/cmd/set_udp_dest_auto", - "\"set_config_param\""); + "\"set_config_param\"", timeout_sec); } -SensorHttpImp_2_1::SensorHttpImp_2_1(const string& hostname, int timeout_sec) - : SensorHttpImp_2_2(hostname, timeout_sec) {} +SensorHttpImp_2_1::SensorHttpImp_2_1(const string& hostname) + : SensorHttpImp_2_2(hostname) {} -Json::Value SensorHttpImp_2_1::metadata() const { +Json::Value SensorHttpImp_2_1::metadata(int timeout_sec) const { Json::Value root; - root["sensor_info"] = sensor_info(); - root["beam_intrinsics"] = beam_intrinsics(); - root["imu_intrinsics"] = imu_intrinsics(); - root["lidar_intrinsics"] = lidar_intrinsics(); - root["lidar_data_format"] = lidar_data_format(); - root["calibration_status"] = calibration_status(); + root["sensor_info"] = sensor_info(timeout_sec); + root["beam_intrinsics"] = beam_intrinsics(timeout_sec); + root["imu_intrinsics"] = imu_intrinsics(timeout_sec); + root["lidar_intrinsics"] = lidar_intrinsics(timeout_sec); + root["lidar_data_format"] = lidar_data_format(timeout_sec); + root["calibration_status"] = calibration_status(timeout_sec); Json::CharReaderBuilder builder; auto reader = std::unique_ptr{builder.newCharReader()}; Json::Value node; - auto res = get_config_params(true); + auto res = get_config_params(true, timeout_sec); auto parse_success = reader->parse(res.c_str(), res.c_str() + res.size(), &node, nullptr); root["config_params"] = parse_success ? node : res; return root; } -Json::Value SensorHttpImp_2_1::sensor_info() const { - return get_json("api/v1/sensor/cmd/get_sensor_info"); +Json::Value SensorHttpImp_2_1::sensor_info(int timeout_sec) const { + return get_json("api/v1/sensor/cmd/get_sensor_info", timeout_sec); } -Json::Value SensorHttpImp_2_1::beam_intrinsics() const { - return get_json("api/v1/sensor/cmd/get_beam_intrinsics"); +Json::Value SensorHttpImp_2_1::beam_intrinsics(int timeout_sec) const { + return get_json("api/v1/sensor/cmd/get_beam_intrinsics", timeout_sec); } -Json::Value SensorHttpImp_2_1::imu_intrinsics() const { - return get_json("api/v1/sensor/cmd/get_imu_intrinsics"); +Json::Value SensorHttpImp_2_1::imu_intrinsics(int timeout_sec) const { + return get_json("api/v1/sensor/cmd/get_imu_intrinsics", timeout_sec); +} + +Json::Value SensorHttpImp_2_1::lidar_intrinsics(int timeout_sec) const { + return get_json("api/v1/sensor/cmd/get_lidar_intrinsics", timeout_sec); +} + +Json::Value SensorHttpImp_2_1::lidar_data_format(int timeout_sec) const { + return get_json("api/v1/sensor/cmd/get_lidar_data_format", timeout_sec); +} + +Json::Value SensorHttpImp_2_1::calibration_status(int timeout_sec) const { + return get_json("api/v1/sensor/cmd/get_calibration_status", timeout_sec); +} + +SensorHttpImp_2_4_or_3::SensorHttpImp_2_4_or_3(const string& hostname) + : SensorHttpImp(hostname) {} + +std::string SensorHttpImp_2_4_or_3::get_user_data(int /*timeout_sec*/) const { + throw std::runtime_error("user data API not supported on this FW version"); } -Json::Value SensorHttpImp_2_1::lidar_intrinsics() const { - return get_json("api/v1/sensor/cmd/get_lidar_intrinsics"); +UserDataAndPolicy SensorHttpImp_2_4_or_3::get_user_data_and_policy( + int /*timeout_sec*/) const { + throw std::runtime_error("user data API not supported on this FW version"); } -Json::Value SensorHttpImp_2_1::lidar_data_format() const { - return get_json("api/v1/sensor/cmd/get_lidar_data_format"); +void SensorHttpImp_2_4_or_3::set_user_data(const std::string& /*data*/, + bool /*keep_on_config_delete*/, + int /*timeout_sec*/) const { + throw std::runtime_error("user data API not supported on this FW version"); } -Json::Value SensorHttpImp_2_1::calibration_status() const { - return get_json("api/v1/sensor/cmd/get_calibration_status"); +void SensorHttpImp_2_4_or_3::delete_user_data(int /*timeout_sec*/) const { + throw std::runtime_error("user data API not supported on this FW version"); } diff --git a/ouster_client/src/sensor_http_imp.h b/ouster_client/src/sensor_http_imp.h index 578ef70d..33c6d030 100644 --- a/ouster_client/src/sensor_http_imp.h +++ b/ouster_client/src/sensor_http_imp.h @@ -25,9 +25,8 @@ class SensorHttpImp : public util::SensorHttp { * Constructs an http interface to communicate with the sensor. * * @param[in] hostname Hostname of the sensor to communicate with. - * @param[in] timeout_sec The timeout to use in seconds. */ - SensorHttpImp(const std::string& hostname, int timeout_sec); + SensorHttpImp(const std::string& hostname); /** * Deconstruct the sensor http interface. @@ -39,23 +38,25 @@ class SensorHttpImp : public util::SensorHttp { * * @return returns a Json object of the sensor metadata. */ - Json::Value metadata() const override; + Json::Value metadata(int timeout_sec = 1) const override; /** * Queries the sensor_info. * * @return returns a Json object representing the sensor_info. */ - Json::Value sensor_info() const override; + Json::Value sensor_info(int timeout_sec = 1) const override; /** * Queries active/staged configuration on the sensor * * @param[in] active if true retrieve active, otherwise get staged configs. + * @param[in] timeout_sec The timeout for the request in seconds. * * @return a string represnting the active or staged config */ - std::string get_config_params(bool active) const override; + std::string get_config_params(bool active, + int timeout_sec = 1) const override; /** * Set the value of a specfic configuration on the sensor, the changed @@ -63,77 +64,130 @@ class SensorHttpImp : public util::SensorHttp { * * @param[in] key name of the config to change. * @param[in] value the new value to set for the selected configuration. + * @param[in] timeout_sec The timeout for the request in seconds. */ - void set_config_param(const std::string& key, - const std::string& value) const override; + void set_config_param(const std::string& key, const std::string& value, + int timeout_sec = 1) const override; /** * Retrieves the active configuration on the sensor */ - Json::Value active_config_params() const override; + Json::Value active_config_params(int timeout_sec = 1) const override; /** * Retrieves the staged configuration on the sensor */ - Json::Value staged_config_params() const override; + Json::Value staged_config_params(int timeout_sec = 1) const override; /** * Enables automatic assignment of udp destination ports. */ - void set_udp_dest_auto() const override; + void set_udp_dest_auto(int timeout_sec = 1) const override; /** * Retrieves beam intrinsics of the sensor. */ - Json::Value beam_intrinsics() const override; + Json::Value beam_intrinsics(int timeout_sec = 1) const override; /** * Retrieves imu intrinsics of the sensor. */ - Json::Value imu_intrinsics() const override; + Json::Value imu_intrinsics(int timeout_sec = 1) const override; /** * Retrieves lidar intrinsics of the sensor. */ - Json::Value lidar_intrinsics() const override; + Json::Value lidar_intrinsics(int timeout_sec = 1) const override; /** * Retrieves lidar data format. */ - Json::Value lidar_data_format() const override; + Json::Value lidar_data_format(int timeout_sec = 1) const override; /** * Gets the calibaration status of the sensor. */ - Json::Value calibration_status() const override; + Json::Value calibration_status(int timeout_sec = 1) const override; /** * Restarts the sensor applying all staged configurations. */ - void reinitialize() const override; + void reinitialize(int timeout_sec = 1) const override; /** * Persist active configuration parameters to the sensor. */ - void save_config_params() const override; + void save_config_params(int timeout_sec = 1) const override; + + /** + * Gets the user data stored on the sensor. + */ + std::string get_user_data(int timeout_sec = 1) const override; + + /** + * Gets the user data stored on the sensor and the retention policy. + */ + util::UserDataAndPolicy get_user_data_and_policy( + int timeout_sec = 1) const override; + + /** + * Sets the user data stored on the sensor. + */ + void set_user_data(const std::string& data, + bool keep_on_config_delete = true, + int timeout_sec = 1) const override; + + /** + * Deletes the user data stored on the sensor. + */ + void delete_user_data(int timeout_sec = 1) const override; protected: - std::string get(const std::string& url) const; + std::string get(const std::string& url, int timeout_sec) const; - Json::Value get_json(const std::string& url) const; + Json::Value get_json(const std::string& url, int timeout_sec) const; - void execute(const std::string& url, const std::string& validation) const; + void execute(const std::string& url, const std::string& validation, + int timeout_sec) const; protected: std::unique_ptr http_client; }; +class SensorHttpImp_2_4_or_3 : public SensorHttpImp { + public: + SensorHttpImp_2_4_or_3(const std::string& hostname); + + /** + * Gets the user data stored on the sensor. + */ + std::string get_user_data(int timeout_sec = 1) const override; + + /** + * Gets the user data stored on the sensor and the retention policy. + */ + util::UserDataAndPolicy get_user_data_and_policy( + int timeout_sec = 1) const override; + + /** + * Sets the user data stored on the sensor. + */ + void set_user_data(const std::string& data, + bool keep_on_config_delete = true, + int timeout_sec = 1) const override; + + /** + * Deletes the user data stored on the sensor. + */ + void delete_user_data(int timeout_sec = 1) const override; +}; + // TODO: remove when firmware 2.2 has been fully phased out -class SensorHttpImp_2_2 : public SensorHttpImp { +class SensorHttpImp_2_2 : public SensorHttpImp_2_4_or_3 { public: - SensorHttpImp_2_2(const std::string& hostname, int timeout_sec); + SensorHttpImp_2_2(const std::string& hostname); - void set_udp_dest_auto() const override; + void set_udp_dest_auto(int timeout_sec = 1) const override; }; /** @@ -145,48 +199,47 @@ class SensorHttpImp_2_1 : public SensorHttpImp_2_2 { * Constructs an http interface to communicate with the sensor. * * @param[in] hostname hostname of the sensor to communicate with. - * @param[in] timeout_sec The timeout to use in seconds. */ - SensorHttpImp_2_1(const std::string& hostname, int timeout_sec); + SensorHttpImp_2_1(const std::string& hostname); /** * Queries the sensor metadata. * * @return returns a Json object of the sensor metadata. */ - Json::Value metadata() const override; + Json::Value metadata(int timeout_sec = 1) const override; /** * Queries the sensor_info. * * @return returns a Json object representing the sensor_info. */ - Json::Value sensor_info() const override; + Json::Value sensor_info(int timeout_sec = 1) const override; /** * Retrieves beam intrinsics of the sensor. */ - Json::Value beam_intrinsics() const override; + Json::Value beam_intrinsics(int timeout_sec = 1) const override; /** * Retrieves imu intrinsics of the sensor. */ - Json::Value imu_intrinsics() const override; + Json::Value imu_intrinsics(int timeout_sec = 1) const override; /** * Retrieves lidar intrinsics of the sensor. */ - Json::Value lidar_intrinsics() const override; + Json::Value lidar_intrinsics(int timeout_sec = 1) const override; /** * Retrieves lidar data format. */ - Json::Value lidar_data_format() const override; + Json::Value lidar_data_format(int timeout_sec = 1) const override; /** * Gets the calibaration status of the sensor. */ - Json::Value calibration_status() const override; + Json::Value calibration_status(int timeout_sec = 1) const override; }; } // namespace impl diff --git a/ouster_client/src/sensor_info.cpp b/ouster_client/src/sensor_info.cpp index 8d3b8eff..30e2c45b 100644 --- a/ouster_client/src/sensor_info.cpp +++ b/ouster_client/src/sensor_info.cpp @@ -16,8 +16,8 @@ #include #include -#include "logging.h" #include "ouster/impl/build.h" +#include "ouster/impl/logging.h" #include "ouster/types.h" #include "ouster/util.h" #include "ouster/version.h" @@ -46,8 +46,7 @@ extern Json::Value config_to_json(const sensor_config& config); /* Equality operators and functions */ bool operator==(const sensor_info& lhs, const sensor_info& rhs) { - return (lhs.has_fields_equal(rhs) && - lhs.original_string() == rhs.original_string()); + return lhs.has_fields_equal(rhs); } bool operator!=(const sensor_info& lhs, const sensor_info& rhs) { @@ -56,8 +55,7 @@ bool operator!=(const sensor_info& lhs, const sensor_info& rhs) { bool sensor_info::has_fields_equal(const sensor_info& other) const { return ( - this->name == other.name && this->sn == other.sn && - this->fw_rev == other.fw_rev && this->mode == other.mode && + this->sn == other.sn && this->fw_rev == other.fw_rev && this->prod_line == other.prod_line && this->format == other.format && this->beam_azimuth_angles == other.beam_azimuth_angles && this->beam_altitude_angles == other.beam_altitude_angles && @@ -67,24 +65,21 @@ bool sensor_info::has_fields_equal(const sensor_info& other) const { this->imu_to_sensor_transform == other.imu_to_sensor_transform && this->lidar_to_sensor_transform == other.lidar_to_sensor_transform && this->extrinsic == other.extrinsic && this->init_id == other.init_id && - this->udp_port_lidar == other.udp_port_lidar && - this->udp_port_imu == other.udp_port_imu && this->build_date == other.build_date && this->image_rev == other.image_rev && this->prod_pn == other.prod_pn && this->status == other.status && this->cal == other.cal && - this->config == other.config); + this->config == other.config && this->user_data == other.user_data); } /* Default values */ sensor_info default_sensor_info(lidar_mode mode) { auto info = sensor_info(); - - info.name = "UNKNOWN"; info.sn = "000000000000"; info.fw_rev = "UNKNOWN"; - info.mode = mode; + info.prod_line = "OS-1-64"; + info.format = default_data_format(mode); info.beam_azimuth_angles = gen1_azimuth_angles; info.beam_altitude_angles = gen1_altitude_angles; @@ -96,14 +91,16 @@ sensor_info default_sensor_info(lidar_mode mode) { info.lidar_to_sensor_transform = default_lidar_to_sensor_transform; info.extrinsic = mat4d::Identity(); info.init_id = 0; - info.udp_port_lidar = 0; - info.udp_port_imu = 0; info.build_date = ""; info.image_rev = ""; info.prod_pn = ""; info.status = ""; + info.user_data = ""; info.cal = default_calibration_status(); info.config = sensor_config{}; + info.config.lidar_mode = mode; + info.config.udp_port_lidar = 0; + info.config.udp_port_imu = 0; return info; } @@ -158,18 +155,7 @@ const std::map nonlegacy_metadata_fields = { // clang-format on -static bool is_new_format(const std::string& metadata) { - Json::Value root{}; - Json::CharReaderBuilder builder{}; - std::string errors{}; - std::stringstream ss{metadata}; - - if (metadata.size()) { - if (!Json::parseFromStream(builder, ss, &root, &errors)) - throw std::runtime_error{ - "Error parsing metadata when checking format: " + errors}; - } - +static bool is_new_format(const Json::Value& root) { size_t nonlegacy_fields_present = 0; std::string missing_fields = ""; for (const auto& field_pair : nonlegacy_metadata_fields) { @@ -195,94 +181,66 @@ static bool is_new_format(const std::string& metadata) { return nonlegacy_fields_present == nonlegacy_metadata_fields.size(); } -void parse_legacy(sensor_info& info, const std::string& metadata, - bool skip_beam_validation, bool suppress_legacy_warnings) { - Json::Value root{}; - Json::CharReaderBuilder builder{}; - std::string errors{}; - std::stringstream ss{metadata}; - - if (metadata.size()) { - if (!Json::parseFromStream(builder, ss, &root, &errors)) - throw std::runtime_error{ - "Errors parsing metadata for parse_metadata: " + errors}; - } - - // NOTE[pb]: DF development metadata.json doesn't have beam_altitude_angles - // and beam_azimuth_angles and instead provides beam_xyz. However - // final implementation should have azimuth/altitude angles and - // we may uncomment the validation back closer to the release. - // const std::vector minimum_legacy_metadata_fields{ - // "beam_altitude_angles", "beam_azimuth_angles", "lidar_mode"}; - - // NOTE[pb]: DF metadata doesn't have lidar_mode, but because it's - // going through convert_to_legacy() function it get's the empty - // string lidar_mode ... hmmm, fine for now... - const std::vector minimum_legacy_metadata_fields{"lidar_mode"}; - - for (auto field : minimum_legacy_metadata_fields) { +static void parse_metadata(sensor_info& info, const Json::Value& root, + bool skip_beam_validation) { + const std::vector minimum_metadata_fields{"config_params", + "beam_intrinsics"}; + for (auto field : minimum_metadata_fields) { if (!root.isMember(field)) { throw std::runtime_error{"Metadata must contain: " + field}; } } - // nice to have fields which we will use defaults for if they don't - // exist - const std::vector desired_legacy_metadata_fields{ - "imu_to_sensor_transform", - "lidar_to_sensor_transform", - "prod_line", - "prod_sn", - "build_rev", - "config_params"}; - for (auto field : desired_legacy_metadata_fields) { + // nice to have fields which we will use defaults for if they don't exist + const std::vector desired_metadata_fields{"imu_intrinsics", + "lidar_intrinsics"}; + for (auto field : desired_metadata_fields) { if (!root.isMember(field)) { - if (suppress_legacy_warnings && field != "config_params") { - logger().warn( - "No " + field + - " found in metadata. Will be left blank or filled in " - "with default legacy values"); - } + logger().warn("No " + field + + " found in metadata. Will be left blank or filled in " + "with default legacy values"); } } - // will be empty string if not present - info.name = root["hostname"].asString(); - // if these are not present they are also empty strings - info.build_date = root["build_date"].asString(); - info.fw_rev = root["build_rev"].asString(); - info.image_rev = root["image_rev"].asString(); - info.prod_line = root["prod_line"].asString(); - info.prod_pn = root["prod_pn"].asString(); - info.sn = root["prod_sn"].asString(); - info.status = root["status"].asString(); + auto sensor_info = root["sensor_info"]; + info.build_date = sensor_info["build_date"].asString(); + info.fw_rev = sensor_info["build_rev"].asString(); + info.image_rev = sensor_info["image_rev"].asString(); + info.prod_line = sensor_info["prod_line"].asString(); + info.prod_pn = sensor_info["prod_pn"].asString(); + info.sn = sensor_info["prod_sn"].asString(); + info.status = sensor_info["status"].asString(); // default to 0 if init_id key not present - info.init_id = root["initialization_id"].asInt(); + info.init_id = sensor_info["initialization_id"].asInt(); // checked that lidar_mode is present already - never empty string - info.mode = lidar_mode_of_string(root["lidar_mode"].asString()); + auto mode = + lidar_mode_of_string(root["config_params"]["lidar_mode"].asString()); // "data_format" introduced in fw 2.0. Fall back to 1.13 - if (root.isMember("data_format")) { - info.format = parse_data_format(root["data_format"]); + if (root.isMember("lidar_data_format") && + root["lidar_data_format"].isObject()) { + info.format = parse_data_format(root["lidar_data_format"]); // data_format.fps was added for DF sensors, so we are backfilling // fps value for OS sensors here if it's not present in metadata if (info.format.fps == 0) { - info.format.fps = frequency_of_lidar_mode(info.mode); + info.format.fps = frequency_of_lidar_mode(mode); } } else { - logger().warn("No data_format found. Using default legacy data format"); - info.format = default_data_format(info.mode); + logger().warn( + "No lidar_data_format found. Using default legacy data format"); + info.format = default_data_format(mode); } // "lidar_origin_to_beam_origin_mm" introduced in fw 2.0 BUT missing // on OS-DOME. Handle falling back to FW 1.13 or setting to 0 // according to prod-line - if (root.isMember("lidar_origin_to_beam_origin_mm")) { + auto beam_intrinsics = root["beam_intrinsics"]; + if (beam_intrinsics.isMember("lidar_origin_to_beam_origin_mm")) { info.lidar_origin_to_beam_origin_mm = - root["lidar_origin_to_beam_origin_mm"].asDouble(); + beam_intrinsics["lidar_origin_to_beam_origin_mm"].asDouble(); } else { if (info.prod_line.find("OS-DOME-") == 0) { // is an OS-DOME - fill with 0 @@ -300,12 +258,12 @@ void parse_legacy(sensor_info& info, const std::string& metadata, } // beam_to_lidar_transform" introduced in fw 2.5/fw 3.0 - if (root.isMember("beam_to_lidar_transform")) { + if (beam_intrinsics.isMember("beam_to_lidar_transform")) { for (int i = 0; i < 4; i++) { for (int j = 0; j < 4; j++) { const Json::Value::ArrayIndex ind = i * 4 + j; info.beam_to_lidar_transform(i, j) = - root["beam_to_lidar_transform"][ind].asDouble(); + beam_intrinsics["beam_to_lidar_transform"][ind].asDouble(); } } } else { @@ -315,18 +273,21 @@ void parse_legacy(sensor_info& info, const std::string& metadata, info.lidar_origin_to_beam_origin_mm; } - if (root["beam_altitude_angles"].size() != 0 && - root["beam_altitude_angles"].size() != info.format.pixels_per_column) + if (beam_intrinsics["beam_altitude_angles"].size() != 0 && + beam_intrinsics["beam_altitude_angles"].size() != + info.format.pixels_per_column) throw std::runtime_error{"Unexpected number of beam_altitude_angles"}; - if (root["beam_azimuth_angles"].size() != 0 && - root["beam_azimuth_angles"].size() != info.format.pixels_per_column) + if (beam_intrinsics["beam_azimuth_angles"].size() != 0 && + beam_intrinsics["beam_azimuth_angles"].size() != + info.format.pixels_per_column) throw std::runtime_error{"Unexpected number of beam_azimuth_angles"}; - if (root["beam_altitude_angles"].size() == info.format.pixels_per_column) { - if (root["beam_altitude_angles"][0].isArray()) { + if (beam_intrinsics["beam_altitude_angles"].size() == + info.format.pixels_per_column) { + if (beam_intrinsics["beam_altitude_angles"][0].isArray()) { // DF sensor path - for (const auto& row : root["beam_altitude_angles"]) + for (const auto& row : beam_intrinsics["beam_altitude_angles"]) for (const auto& v : row) info.beam_altitude_angles.push_back(v.asDouble()); @@ -337,15 +298,16 @@ void parse_legacy(sensor_info& info, const std::string& metadata, } } else { // OS sensor path - for (const auto& v : root["beam_altitude_angles"]) + for (const auto& v : beam_intrinsics["beam_altitude_angles"]) info.beam_altitude_angles.push_back(v.asDouble()); } } - if (root["beam_azimuth_angles"].size() == info.format.pixels_per_column) { - if (root["beam_azimuth_angles"][0].isArray()) { + if (beam_intrinsics["beam_azimuth_angles"].size() == + info.format.pixels_per_column) { + if (beam_intrinsics["beam_azimuth_angles"][0].isArray()) { // DF sensor path - for (const auto& row : root["beam_azimuth_angles"]) { + for (const auto& row : beam_intrinsics["beam_azimuth_angles"]) { for (const auto& v : row) info.beam_azimuth_angles.push_back(v.asDouble()); } @@ -357,42 +319,20 @@ void parse_legacy(sensor_info& info, const std::string& metadata, } } else { // OS sensor path - for (const auto& v : root["beam_azimuth_angles"]) + for (const auto& v : beam_intrinsics["beam_azimuth_angles"]) info.beam_azimuth_angles.push_back(v.asDouble()); } } - // NOTE[pb]: this block that handles beam_xyz shouldn't survive past - // the DF development phase and we need to swith to azimuth/altitude - // angles in the metadata, because they take less space and they - // are less redundant configuration of intrinsics than unit xyz vectors - if (info.beam_altitude_angles.empty() && info.beam_azimuth_angles.empty()) { - if (root["beam_xyz"].size() != - 3 * info.format.pixels_per_column * info.format.columns_per_frame) { - throw std::runtime_error{"Unexpected number of beam_xyz"}; - } - - // DF sensor path - auto& xyz = root["beam_xyz"]; - for (Json::Value::ArrayIndex idx = 0; idx < xyz.size(); idx += 3) { - auto x = xyz[idx + 0].asDouble(); - auto y = xyz[idx + 1].asDouble(); - auto z = xyz[idx + 2].asDouble(); - auto al = std::atan2(z, sqrt(x * x + y * y)) * 180.0 / M_PI; - auto az = std::atan2(y, x) * 180.0 / M_PI; - info.beam_altitude_angles.push_back(al); - info.beam_azimuth_angles.push_back(az); - } - } - // "imu_to_sensor_transform" may be absent in sensor config // produced by Ouster Studio, so we backfill it with default value - if (root["imu_to_sensor_transform"].size() == 16) { + auto imu_intrinsics = root["imu_intrinsics"]; + if (imu_intrinsics["imu_to_sensor_transform"].size() == 16) { for (int i = 0; i < 4; i++) { for (int j = 0; j < 4; j++) { const Json::Value::ArrayIndex ind = i * 4 + j; info.imu_to_sensor_transform(i, j) = - root["imu_to_sensor_transform"][ind].asDouble(); + imu_intrinsics["imu_to_sensor_transform"][ind].asDouble(); } } } else { @@ -404,12 +344,14 @@ void parse_legacy(sensor_info& info, const std::string& metadata, // "lidar_to_sensor_transform" may be absent in sensor config // produced by Ouster Studio, so we backfill it with default value - if (root["lidar_to_sensor_transform"].size() == 16) { + auto lidar_intrinsics = root["lidar_intrinsics"]; + if (lidar_intrinsics["lidar_to_sensor_transform"].size() == 16) { for (int i = 0; i < 4; i++) { for (int j = 0; j < 4; j++) { const Json::Value::ArrayIndex ind = i * 4 + j; info.lidar_to_sensor_transform(i, j) = - root["lidar_to_sensor_transform"][ind].asDouble(); + lidar_intrinsics["lidar_to_sensor_transform"][ind] + .asDouble(); } } } else { @@ -439,29 +381,116 @@ void parse_legacy(sensor_info& info, const std::string& metadata, info.extrinsic = mat4d::Identity(); - // default to 0 if keys are not present - info.udp_port_lidar = root["udp_port_lidar"].asInt(); - info.udp_port_imu = root["udp_port_imu"].asInt(); + if (root.isMember("ouster-sdk")) { + auto sdk_group = root["ouster-sdk"]; + if (sdk_group["extrinsic"].size() == 16) { + for (int i = 0; i < 4; i++) { + for (int j = 0; j < 4; j++) { + const Json::Value::ArrayIndex ind = i * 4 + j; + info.extrinsic(i, j) = + sdk_group["extrinsic"][ind].asDouble(); + } + } + } else { + logger().info("No valid extrinsics found. Using identity."); + } + } + + // we are guaranteed calibration_status as a key exists so don't need to + // check again + if (root["calibration_status"].isObject()) { + if (root["calibration_status"]["reflectivity"]["valid"].isBool()) { + info.cal.reflectivity_status = + root["calibration_status"]["reflectivity"]["valid"].asBool(); + } else { + logger().warn( + "metadata field calibration_status.reflectivity.valid is " + "not Bool value, but: {}. Using False instead.", + root["calibration_status"]["reflectivity"]["valid"].asString()); + } + + if (info.cal.reflectivity_status) { + info.cal.reflectivity_timestamp = + root["calibration_status"]["reflectivity"]["timestamp"] + .asString(); + } + } + + info.config = parse_config(root["config_params"]); + info.user_data = root["user_data"].asString(); } -static void update_json_obj(Json::Value& dst, const Json::Value& src) { - const std::vector& members = src.getMemberNames(); - for (const auto& key : members) { - dst[key] = src[key]; +static void parse_legacy(sensor_info& info, const Json::Value& root, + bool skip_beam_validation) { + // just convert to non-legacy and run the non-legacy parse + const std::vector config_fields{ + "udp_port_imu", + "udp_port_lidar", + "lidar_mode", + }; + + const std::vector beam_intrinsics_fields{ + "lidar_origin_to_beam_origin_mm", "beam_altitude_angles", + "beam_azimuth_angles", "beam_to_lidar_transform"}; + + const std::vector sensor_info_fields{ + "prod_line", "status", "prod_pn", "prod_sn", + "initialization_id", "build_rev", "build_date", "image_rev", + }; + + // Error if we dont have required fields + const std::vector minimum_metadata_fields{"lidar_mode"}; + + for (auto field : minimum_metadata_fields) { + if (!root.isMember(field)) { + throw std::runtime_error{"Metadata must contain: " + field}; + } + } + + Json::Value result; + if (root.isMember("lidar_to_sensor_transform")) { + result["lidar_intrinsics"]["lidar_to_sensor_transform"] = + root["lidar_to_sensor_transform"]; + } + if (root.isMember("imu_to_sensor_transform")) { + result["imu_intrinsics"]["imu_to_sensor_transform"] = + root["imu_to_sensor_transform"]; + } + if (root.isMember("data_format")) { + result["lidar_data_format"] = root["data_format"]; + } + if (root.isMember("client_version")) { + result["ouster-sdk"]["client_version"] = root["client_version"]; + } + for (const auto& field : config_fields) { + if (root.isMember(field)) { + result["config_params"][field] = root[field]; + } + } + + for (const auto& field : beam_intrinsics_fields) { + if (root.isMember(field)) { + result["beam_intrinsics"][field] = root[field]; + } } + + for (const auto& field : sensor_info_fields) { + if (root.isMember(field)) { + result["sensor_info"][field] = root[field]; + } + } + + parse_metadata(info, result, skip_beam_validation); } sensor_info::sensor_info() { // TODO - understand why this seg faults in CI when uncommented // logger().warn("Initializing sensor_info without original metadata // string"); - original_metadata_string = ""; } sensor_info::sensor_info(const std::string& metadata, bool skip_beam_validation) { - original_metadata_string = metadata; - Json::Value root{}; Json::CharReaderBuilder builder{}; std::string errors{}; @@ -473,46 +502,17 @@ sensor_info::sensor_info(const std::string& metadata, errors}; } - if (is_new_format(metadata)) { + if (is_new_format(root)) { + was_legacy_ = false; logger().info("parsing non-legacy metadata format"); - parse_legacy(*this, convert_to_legacy(metadata), skip_beam_validation, - true); - // also parse the sensor_config - - // we are guaranteed calibration_status as a key exists so don't need to - // check again - if (root["calibration_status"].isObject()) { - if (root["calibration_status"]["reflectivity"]["valid"].isBool()) { - this->cal.reflectivity_status = - root["calibration_status"]["reflectivity"]["valid"] - .asBool(); - } else { - logger().warn( - "metadata field calibration_status.reflectivity.valid is " - "not Bool value, but: {}. Using False instead.", - root["calibration_status"]["reflectivity"]["valid"] - .asString()); - } - - if (this->cal.reflectivity_status) { - this->cal.reflectivity_timestamp = - root["calibration_status"]["reflectivity"]["timestamp"] - .asString(); - } - } - - this->config = parse_config(root["config_params"]); - + parse_metadata(*this, root, skip_beam_validation); } else { + was_legacy_ = true; logger().info("parsing legacy metadata format"); - parse_legacy(*this, metadata, skip_beam_validation, false); + parse_legacy(*this, root, skip_beam_validation); } } -std::string sensor_info::original_string() const { - return original_metadata_string; -} - void mat4d_to_json(Json::Value& val, mat4d mat) { for (size_t i = 0; i < 4; i++) { for (size_t j = 0; j < 4; j++) { @@ -521,73 +521,6 @@ void mat4d_to_json(Json::Value& val, mat4d mat) { } } -/* DO NOT make public - internal logic use only - * Powers outputting a sensor_info to a flat JSON resembling legacy metadata - */ -Json::Value info_to_flat_json(const sensor_info& info) { - Json::Value result{}; - - result["hostname"] = info.name; - result["prod_sn"] = info.sn; - result["build_rev"] = info.fw_rev; - result["lidar_mode"] = to_string(info.mode); - result["prod_line"] = info.prod_line; - - // data_format - result["data_format"]["pixels_per_column"] = info.format.pixels_per_column; - result["data_format"]["columns_per_packet"] = - info.format.columns_per_packet; - result["data_format"]["columns_per_frame"] = info.format.columns_per_frame; - result["data_format"]["fps"] = info.format.fps; - result["data_format"]["column_window"].append( - info.format.column_window.first); - result["data_format"]["column_window"].append( - info.format.column_window.second); - result["data_format"]["udp_profile_lidar"] = - to_string(info.format.udp_profile_lidar); - result["data_format"]["udp_profile_imu"] = - to_string(info.format.udp_profile_imu); - for (auto i : info.format.pixel_shift_by_row) - result["data_format"]["pixel_shift_by_row"].append(i); - - result["lidar_origin_to_beam_origin_mm"] = - info.lidar_origin_to_beam_origin_mm; - - if (info.beam_azimuth_angles.size() == - info.format.pixels_per_column * info.format.columns_per_frame) { - // Don't output for DF for now - ; - } else { - for (auto i : info.beam_azimuth_angles) - result["beam_azimuth_angles"].append(i); - - for (auto i : info.beam_altitude_angles) - result["beam_altitude_angles"].append(i); - } - - mat4d_to_json(result["beam_to_lidar_transform"], - info.beam_to_lidar_transform); - mat4d_to_json(result["imu_to_sensor_transform"], - info.imu_to_sensor_transform); - mat4d_to_json(result["lidar_to_sensor_transform"], - info.lidar_to_sensor_transform); - mat4d_to_json(result["extrinsic"], info.extrinsic); - - result["initialization_id"] = info.init_id; - result["udp_port_lidar"] = info.udp_port_lidar; - result["udp_port_imu"] = info.udp_port_imu; - - result["build_date"] = info.build_date; - result["image_rev"] = info.image_rev; - result["prod_pn"] = info.prod_pn; - result["status"] = info.status; - - result["calibration_status"] = cal_to_json(info.cal); - result["config_params"] = config_to_json(info.config); - - return result; -} - /* DO NOT make public - internal logic use only * Powers outputting a sensor_info to a nested json resembling non-legacy * metadata @@ -638,39 +571,37 @@ Json::Value info_to_nested_json(const sensor_info& info) { for (auto angle : info.beam_altitude_angles) result["beam_intrinsics"]["beam_altitude_angles"].append(angle); } else { - ; - - // TODO: debug DF beam_altitude/beam_azimuthoutpu - /* + // DF sensor path int j = 0; - for (size_t i=0; iformat == orig_info.format) - root_new.removeMember("lidar_data_format"); - } else { - // format was not auto-populated so now check fps, - // pixel_shift_by_row, column_window, udp_profile_lidar, - // udp_profile_imu - if (!root_orig["lidar_data_format"].isMember("fps") && - this->format.fps == orig_info.format.fps) - root_new["lidar_data_format"].removeMember("fps"); - if (!root_orig["lidar_data_format"].isMember("column_window") && - this->format.column_window == - orig_info.format.column_window) - root_new["lidar_data_format"].removeMember("column_window"); - if (!root_orig["lidar_data_format"].isMember( - "pixel_shift_by_row") && - this->format.pixel_shift_by_row == - orig_info.format.pixel_shift_by_row) - root_new["lidar_data_format"].removeMember( - "pixel_shift_by_row"); - if (!root_orig["lidar_data_format"].isMember( - "udp_profile_lidar") && - this->format.udp_profile_lidar == - orig_info.format.udp_profile_lidar) - root_new["lidar_data_format"].removeMember( - "udp_profile_lidar"); - if (!root_orig["lidar_data_format"].isMember( - "udp_profile_imu") && - this->format.udp_profile_imu == - orig_info.format.udp_profile_imu) - root_new["lidar_data_format"].removeMember( - "udp_profile_imu"); - } - - // check beam_intrinsics.imu_to_sensor_transform - - // NO NEED - FW 1.12 already had this - skip check - // check lidar_intrinsics.lidar_to_sensor_transform - - // NO NEED - FW 1.12 already had this - skip check - - // check lidar_origin_to_beam_origin_mm - if (!root_orig["beam_intrinsics"].isMember( - "lidar_origin_to_beam_origin_mm") && - this->lidar_origin_to_beam_origin_mm == - orig_info.lidar_origin_to_beam_origin_mm) - root_new["beam_intrinsics"].removeMember( - "lidar_origin_to_beam_origin_mm"); - - // check beam_intinrics.beam_to_lidar_transform - if (!root_orig["beam_intrinsics"].isMember( - "beam_to_lidar_transform") && - this->beam_to_lidar_transform == - orig_info.beam_to_lidar_transform) - root_new["beam_intrinsics"].removeMember( - "beam_to_lidar_transform"); - - if (!root_orig["sensor_info"].isMember("initialization_id") && - this->init_id == orig_info.init_id) - root_new["sensor_info"].removeMember("initialization_id"); - - if (root_orig["calibration_status"] == - "error: Command not recognized." && - this->cal == orig_info.cal) { - root_new["calibration_status"] = - "error: Command not recognized."; - } - - if (root_orig["lidar_data_format"] == - "error: Command not recognized." && - this->format == orig_info.format) { - root_new["lidar_data_format"] = - "error: Command not recognized."; - } - - } else { - // have original metadata string that is legacy format - warn users - // what they will lose Users who initialize with legacy metadata but - // want to change these values ... - // ... should upgrade to non-legacy format - if (this->config != sensor_config() || - this->cal != calibration_status()) { - logger().warn( - "Your sensor_info has set sensor_config and/or " - "calibration_status " - "items despite starting with legacy metadata. These will " - "be " - "disregarded in your output (which will be of legacy " - "format."); - } - root_new = info_to_flat_json(*this); - - root_new.removeMember("config_params"); - root_new.removeMember("calibration_status"); - if (this->udp_port_imu == orig_info.udp_port_imu) { - root_new.removeMember("udp_port_imu"); - } - if (this->udp_port_lidar == orig_info.udp_port_lidar) { - root_new.removeMember("udp_port_lidar"); - } - if (this->extrinsic == orig_info.extrinsic) { - root_new.removeMember("extrinsic"); - } - - // check format - if (!root_orig["data_format"].isObject()) { - if (this->format == orig_info.format) - root_new.removeMember("data_format"); - } else { - // format was not auto-populated so now check fps, - // pixel_shift_by_row, column_window, udp_profile_lidar, - // udp_profile_imu - if (!root_orig["data_format"].isMember("fps") && - this->format.fps == orig_info.format.fps) - root_new["data_format"].removeMember("fps"); - if (!root_orig["data_format"].isMember("pixel_shift_by_row") && - this->format.pixel_shift_by_row == - orig_info.format.pixel_shift_by_row) - root_new["data_format"].removeMember("pixel_shift_by_row"); - if (!root_orig["data_format"].isMember("column_window") && - this->format.column_window == - orig_info.format.column_window) - root_new["data_format"].removeMember("column_window"); - if (!root_orig["data_format"].isMember("udp_profile_lidar") && - this->format.udp_profile_lidar == - orig_info.format.udp_profile_lidar) - root_new["data_format"].removeMember("udp_profile_lidar"); - if (!root_orig["data_format"].isMember("udp_profile_imu") && - this->format.udp_profile_imu == - orig_info.format.udp_profile_imu) - root_new["data_format"].removeMember("udp_profile_imu"); - } - // check imu_to_sensor_transform - if (!root_orig.isMember("imu_to_sensor_transform") && - this->imu_to_sensor_transform == - orig_info.imu_to_sensor_transform) - root_new.removeMember("imu_to_sensor_transform"); - - // check lidar_to_sensor_transform - if (!root_orig.isMember("lidar_to_sensor_transform") && - this->lidar_to_sensor_transform == - orig_info.lidar_to_sensor_transform) - root_new.removeMember("lidar_to_sensor_transform"); - // check lidar_origin_to_beam_origin_mm - if (!root_orig.isMember("lidar_origin_to_beam_origin_mm") && - this->lidar_origin_to_beam_origin_mm == - orig_info.lidar_origin_to_beam_origin_mm) - root_new.removeMember("lidar_origin_to_beam_origin_mm"); - // check beam_to_lidar_transform - if (!root_orig.isMember("beam_to_lidar_transform") && - this->beam_to_lidar_transform == - orig_info.beam_to_lidar_transform) - root_new.removeMember("beam_to_lidar_transform"); - } - } - - std::vector changed; - result = ouster::combined(root_orig, root_new, changed); - - // Relevant for both non-legacy and legacy - result["ouster-sdk"]["output_source"] = "updated_metadata_string"; + result["ouster-sdk"]["output_source"] = "sensor_info_to_string"; result["ouster-sdk"]["client_version"] = client_version(); - for (auto& changed_str : changed) - result["ouster-sdk"]["changed_fields"].append(changed_str); Json::StreamWriterBuilder write_builder; write_builder["enableYAMLCompatibility"] = true; @@ -902,60 +627,12 @@ std::string sensor_info::updated_metadata_string() const { return Json::writeString(write_builder, result); } -std::string convert_to_legacy(const std::string& metadata) { - if (!is_new_format(metadata)) - throw std::invalid_argument( - "Invalid non-legacy metadata format provided"); - - Json::Value root{}; - Json::CharReaderBuilder read_builder{}; - std::string errors{}; - std::stringstream ss{metadata}; - - if (metadata.size()) { - if (!Json::parseFromStream(read_builder, ss, &root, &errors)) { - throw std::runtime_error{ - "Errors parsing metadata for convert_to_legacy: " + errors}; - } - } - Json::Value result{}; - - if (root.isMember("config_params")) { - result["lidar_mode"] = root["config_params"]["lidar_mode"]; - result["udp_port_lidar"] = root["config_params"]["udp_port_lidar"]; - result["udp_port_imu"] = root["config_params"]["udp_port_imu"]; - } - - if (root.isMember("client_version")) - result["client_version"] = root["client_version"]; - - if (root.isMember("ouster-sdk")) result["ouster-sdk"] = root["ouster-sdk"]; - - // TODO eventually remove - // NOTE: DO NOT REMOVE until mid 2024 - // json-calibration-version powers any legacy conversion being done for - // users still on Kitware Ouster Studio probably best to announce removal - // "breakage" by Beginning 2024 - result["json_calibration_version"] = FW_2_2; - - result["hostname"] = root["hostname"].asString(); - - update_json_obj(result, root["sensor_info"]); - update_json_obj(result, root["beam_intrinsics"]); - update_json_obj(result, root["imu_intrinsics"]); - update_json_obj(result, root["lidar_intrinsics"]); - - if (root.isMember("lidar_data_format") && - root["lidar_data_format"].isObject()) { - result["data_format"] = Json::Value{}; - update_json_obj(result["data_format"], root["lidar_data_format"]); - } +ouster::util::version sensor_info::get_version() const { + return ouster::util::version_from_string(image_rev); +} - Json::StreamWriterBuilder write_builder; - write_builder["enableYAMLCompatibility"] = true; - write_builder["precision"] = 6; - write_builder["indentation"] = " "; - return Json::writeString(write_builder, result); +product_info sensor_info::get_product_info() const { + return product_info::create_product_info(prod_line); } sensor_info metadata_from_json(const std::string& json_file, @@ -972,40 +649,17 @@ sensor_info metadata_from_json(const std::string& json_file, throw std::runtime_error{ss.str()}; } - return parse_metadata(buf.str(), skip_beam_validation); -} - -// TODO - fix up according to debug output desires -std::string to_string(const sensor_info& info) { - logger().warn( - "Calling debug to_string on sensor_info. Does NOT produce valid " - "metadata.json"); - Json::StreamWriterBuilder builder; - builder["enableYAMLCompatibility"] = true; - builder["precision"] = 6; - builder["indentation"] = " "; - - auto root = info_to_flat_json(info); - root["ouster-sdk"]["output_source"] = "DEBUG:to_string"; - - return Json::writeString(builder, root); + return sensor_info(buf.str(), skip_beam_validation); } -sensor_info parse_metadata(const std::string& metadata, - bool skip_beam_validation) { - return sensor_info(metadata, skip_beam_validation); -} +std::string to_string(const sensor_info& info) { return info.to_json_string(); } -// TODO: do we need to expose this method? std::string get_firmware_version(const Json::Value& metadata_root) { auto fw_ver = std::string{}; if (metadata_root["sensor_info"].isObject()) { - if (metadata_root["sensor_info"].isMember("semver")) { - // This is only true for 3.2 and later - fw_ver = metadata_root["sensor_info"]["semver"].asString(); - } else if (metadata_root["sensor_info"].isMember("build_rev")) { - // fall back to build_rev - fw_ver = metadata_root["sensor_info"]["build_rev"].asString(); + if (metadata_root["sensor_info"].isMember("image_rev")) { + // image_rev is preferred over build_rev + fw_ver = metadata_root["sensor_info"]["image_rev"].asString(); } } return fw_ver; diff --git a/ouster_client/src/sensor_tcp_imp.cpp b/ouster_client/src/sensor_tcp_imp.cpp index a2f95d96..8825a8fd 100644 --- a/ouster_client/src/sensor_tcp_imp.cpp +++ b/ouster_client/src/sensor_tcp_imp.cpp @@ -9,8 +9,9 @@ #include #include -#include "logging.h" +#include "ouster/impl/logging.h" +using ouster::sensor::util::UserDataAndPolicy; using std::string; using namespace ouster::sensor::impl; @@ -20,17 +21,17 @@ SensorTcpImp::SensorTcpImp(const string& hostname) SensorTcpImp::~SensorTcpImp() { socket_close(socket_handle); } -Json::Value SensorTcpImp::metadata() const { +Json::Value SensorTcpImp::metadata(int timeout_sec) const { Json::Value root; - root["sensor_info"] = sensor_info(); - root["beam_intrinsics"] = beam_intrinsics(); - root["imu_intrinsics"] = imu_intrinsics(); - root["lidar_intrinsics"] = lidar_intrinsics(); - root["lidar_data_format"] = lidar_data_format(); - root["calibration_status"] = calibration_status(); + root["sensor_info"] = sensor_info(timeout_sec); + root["beam_intrinsics"] = beam_intrinsics(timeout_sec); + root["imu_intrinsics"] = imu_intrinsics(timeout_sec); + root["lidar_intrinsics"] = lidar_intrinsics(timeout_sec); + root["lidar_data_format"] = lidar_data_format(timeout_sec); + root["calibration_status"] = calibration_status(timeout_sec); Json::CharReaderBuilder builder; auto reader = std::unique_ptr{builder.newCharReader()}; - auto res = get_config_params(true); + auto res = get_config_params(true, timeout_sec); Json::Value node; auto parse_success = reader->parse(res.c_str(), res.c_str() + res.size(), &node, nullptr); @@ -38,11 +39,11 @@ Json::Value SensorTcpImp::metadata() const { return root; } -Json::Value SensorTcpImp::sensor_info() const { +Json::Value SensorTcpImp::sensor_info(int /*timeout_sec*/) const { return tcp_cmd_json({"get_sensor_info"}); } -string SensorTcpImp::get_config_params(bool active) const { +string SensorTcpImp::get_config_params(bool active, int /*timeout_sec*/) const { auto config_type = active ? "active" : "staged"; return tcp_cmd({"get_config_param", config_type}); } @@ -58,53 +59,72 @@ std::string rtrim(const std::string& s) { } } // namespace -void SensorTcpImp::set_config_param(const string& key, - const string& value) const { +void SensorTcpImp::set_config_param(const string& key, const string& value, + int /*timeout_sec*/) const { tcp_cmd_with_validation({"set_config_param", key, rtrim(value)}, "set_config_param"); } -Json::Value SensorTcpImp::active_config_params() const { +Json::Value SensorTcpImp::active_config_params(int /*timeout_sec*/) const { return tcp_cmd_json({"get_config_param", "active"}); } -Json::Value SensorTcpImp::staged_config_params() const { +Json::Value SensorTcpImp::staged_config_params(int /*timeout_sec*/) const { return tcp_cmd_json({"get_config_param", "staged"}); } -void SensorTcpImp::set_udp_dest_auto() const { +void SensorTcpImp::set_udp_dest_auto(int /*timeout_sec*/) const { tcp_cmd_with_validation({"set_udp_dest_auto"}, "set_udp_dest_auto"); } -Json::Value SensorTcpImp::beam_intrinsics() const { +Json::Value SensorTcpImp::beam_intrinsics(int /*timeout_sec*/) const { return tcp_cmd_json({"get_beam_intrinsics"}); } -Json::Value SensorTcpImp::imu_intrinsics() const { +Json::Value SensorTcpImp::imu_intrinsics(int /*timeout_sec*/) const { return tcp_cmd_json({"get_imu_intrinsics"}); } -Json::Value SensorTcpImp::lidar_intrinsics() const { +Json::Value SensorTcpImp::lidar_intrinsics(int /*timeout_sec*/) const { return tcp_cmd_json({"get_lidar_intrinsics"}); } -Json::Value SensorTcpImp::lidar_data_format() const { +Json::Value SensorTcpImp::lidar_data_format(int /*timeout_sec*/) const { return tcp_cmd_json({"get_lidar_data_format"}, false); } -Json::Value SensorTcpImp::calibration_status() const { +Json::Value SensorTcpImp::calibration_status(int /*timeout_sec*/) const { return tcp_cmd_json({"get_calibration_status"}, false); } -void SensorTcpImp::reinitialize() const { +void SensorTcpImp::reinitialize(int /*timeout_sec*/) const { // reinitialize to make all staged parameters effective tcp_cmd_with_validation({"reinitialize"}, "reinitialize"); } -void SensorTcpImp::save_config_params() const { +void SensorTcpImp::save_config_params(int /*timeout_sec*/) const { tcp_cmd_with_validation({"write_config_txt"}, "write_config_txt"); } +std::string SensorTcpImp::get_user_data(int /*timeout_sec*/) const { + throw std::runtime_error("user data API not supported on this FW version"); +} + +UserDataAndPolicy SensorTcpImp::get_user_data_and_policy( + int /*timeout_sec*/) const { + throw std::runtime_error("user data API not supported on this FW version"); +} + +void SensorTcpImp::set_user_data(const std::string& /*data*/, + bool /*keep_on_config_delete*/, + int /*timeout_sec*/) const { + throw std::runtime_error("user data API not supported on this FW version"); +} + +void SensorTcpImp::delete_user_data(int /*timeout_sec*/) const { + throw std::runtime_error("user data API not supported on this FW version"); +} + SOCKET SensorTcpImp::cfg_socket(const char* addr) { struct addrinfo hints, *info_start, *ai; diff --git a/ouster_client/src/sensor_tcp_imp.h b/ouster_client/src/sensor_tcp_imp.h index c70d375f..8e6ae7bb 100644 --- a/ouster_client/src/sensor_tcp_imp.h +++ b/ouster_client/src/sensor_tcp_imp.h @@ -40,25 +40,31 @@ class SensorTcpImp : public util::SensorHttp { /** * Queries the sensor metadata. * + * @param[in] timeout_sec The timeout for the request in seconds. + * * @return returns a Json object of the sensor metadata. */ - Json::Value metadata() const override; + Json::Value metadata(int timeout_sec = 1) const override; /** * Queries the sensor_info. * + * @param[in] timeout_sec The timeout for the request in seconds. + * * @return returns a Json object representing the sensor_info. */ - Json::Value sensor_info() const override; + Json::Value sensor_info(int timeout_sec = 1) const override; /** * Queries active/staged configuration on the sensor * * @param[in] active if true retrieve active, otherwise get staged configs. + * @param[in] timeout_sec The timeout for the request in seconds. * * @return a string represnting the active or staged config */ - std::string get_config_params(bool active) const override; + std::string get_config_params(bool active, + int timeout_sec = 1) const override; /** * Set the value of a specfic configuration on the sensor, the changed @@ -66,59 +72,114 @@ class SensorTcpImp : public util::SensorHttp { * * @param[in] key name of the config to change. * @param[in] value the new value to set for the selected configuration. + * @param[in] timeout_sec The timeout for the request in seconds. */ - void set_config_param(const std::string& key, - const std::string& value) const override; + void set_config_param(const std::string& key, const std::string& value, + int timeout_sec = 1) const override; /** * Retrieves the active configuration on the sensor + * + * @param[in] timeout_sec The timeout for the request in seconds. */ - Json::Value active_config_params() const override; + Json::Value active_config_params(int timeout_sec = 1) const override; /** * Retrieves the staged configuration on the sensor + * + * @param[in] timeout_sec The timeout for the request in seconds. */ - Json::Value staged_config_params() const override; + Json::Value staged_config_params(int timeout_sec = 1) const override; /** * Enables automatic assignment of udp destination ports. + * + * @param[in] timeout_sec The timeout for the request in seconds. */ - void set_udp_dest_auto() const override; + void set_udp_dest_auto(int timeout_sec = 1) const override; /** * Retrieves beam intrinsics of the sensor. + * + * @param[in] timeout_sec The timeout for the request in seconds. */ - Json::Value beam_intrinsics() const override; + Json::Value beam_intrinsics(int timeout_sec = 1) const override; /** * Retrieves imu intrinsics of the sensor. + * + * @param[in] timeout_sec The timeout for the request in seconds. */ - Json::Value imu_intrinsics() const override; + Json::Value imu_intrinsics(int timeout_sec = 1) const override; /** * Retrieves lidar intrinsics of the sensor. + * + * @param[in] timeout_sec The timeout for the request in seconds. */ - Json::Value lidar_intrinsics() const override; + Json::Value lidar_intrinsics(int timeout_sec = 1) const override; /** * Retrieves lidar data format. + * + * @param[in] timeout_sec The timeout for the request in seconds. */ - Json::Value lidar_data_format() const override; + Json::Value lidar_data_format(int timeout_sec = 1) const override; /** * Gets the calibaration status of the sensor. + * + * @param[in] timeout_sec The timeout for the request in seconds. */ - Json::Value calibration_status() const override; + Json::Value calibration_status(int timeout_sec = 1) const override; /** * Restarts the sensor applying all staged configurations. + * + * @param[in] timeout_sec The timeout for the request in seconds. */ - void reinitialize() const override; + void reinitialize(int timeout_sec = 1) const override; /** * Persist active configuration parameters to the sensor. + * + * @param[in] timeout_sec The timeout for the request in seconds. + */ + void save_config_params(int timeout_sec = 1) const override; + + /** + * Gets the user data stored on the sensor. + * + * @param[in] timeout_sec The timeout for the request in seconds. + */ + std::string get_user_data(int timeout_sec = 1) const override; + + /** + * Gets the user data stored on the sensor and the retention policy. + * + * @param[in] timeout_sec The timeout for the request in seconds. + */ + util::UserDataAndPolicy get_user_data_and_policy( + int timeout_sec = 1) const override; + + /** + * Sets the user data stored on the sensor. + * + * @param[in] data Value of userdata to set on the sensor. + * @param[in] keep_on_config_delete If true, keep the userdata when + configuration is deleted from the sensor + * @param[in] timeout_sec The timeout for the request in seconds. + */ + void set_user_data(const std::string& data, + bool keep_on_config_delete = true, + int timeout_sec = 1) const override; + + /** + * Deletes the user data stored on the sensor. + * + * @param[in] timeout_sec The timeout for the request in seconds. */ - void save_config_params() const override; + void delete_user_data(int timeout_sec = 1) const override; private: SOCKET cfg_socket(const char* addr); diff --git a/ouster_client/src/types.cpp b/ouster_client/src/types.cpp index 2aa5a5c7..c42eb09e 100644 --- a/ouster_client/src/types.cpp +++ b/ouster_client/src/types.cpp @@ -19,9 +19,9 @@ #include #include -#include "logging.h" #include "ouster/defaults.h" #include "ouster/impl/build.h" +#include "ouster/impl/logging.h" #include "ouster/version.h" namespace ouster { @@ -73,38 +73,6 @@ extern const Table polarity_strings{ extern const Table nmea_baud_rate_strings{ {{BAUD_9600, "BAUD_9600"}, {BAUD_115200, "BAUD_115200"}}}; -Table chanfield_strings{{ - {ChanField::RANGE, "RANGE"}, - {ChanField::RANGE2, "RANGE2"}, - {ChanField::SIGNAL, "SIGNAL"}, - {ChanField::SIGNAL2, "SIGNAL2"}, - {ChanField::REFLECTIVITY, "REFLECTIVITY"}, - {ChanField::REFLECTIVITY2, "REFLECTIVITY2"}, - {ChanField::NEAR_IR, "NEAR_IR"}, - {ChanField::FLAGS, "FLAGS"}, - {ChanField::FLAGS2, "FLAGS2"}, - {ChanField::RAW_HEADERS, "RAW_HEADERS"}, - {ChanField::CUSTOM0, "CUSTOM0"}, - {ChanField::CUSTOM1, "CUSTOM1"}, - {ChanField::CUSTOM2, "CUSTOM2"}, - {ChanField::CUSTOM3, "CUSTOM3"}, - {ChanField::CUSTOM4, "CUSTOM4"}, - {ChanField::CUSTOM5, "CUSTOM5"}, - {ChanField::CUSTOM6, "CUSTOM6"}, - {ChanField::CUSTOM7, "CUSTOM7"}, - {ChanField::CUSTOM8, "CUSTOM8"}, - {ChanField::CUSTOM9, "CUSTOM9"}, - {ChanField::RAW32_WORD1, "RAW32_WORD1"}, - {ChanField::RAW32_WORD2, "RAW32_WORD2"}, - {ChanField::RAW32_WORD3, "RAW32_WORD3"}, - {ChanField::RAW32_WORD4, "RAW32_WORD4"}, - {ChanField::RAW32_WORD5, "RAW32_WORD5"}, - {ChanField::RAW32_WORD6, "RAW32_WORD6"}, - {ChanField::RAW32_WORD7, "RAW32_WORD7"}, - {ChanField::RAW32_WORD8, "RAW32_WORD8"}, - {ChanField::RAW32_WORD9, "RAW32_WORD9"}, -}}; - // clang-format off Table udp_profile_lidar_strings{{ {PROFILE_LIDAR_UNKNOWN, "UNKNOWN"}, @@ -185,7 +153,8 @@ bool operator==(const sensor_config& lhs, const sensor_config& rhs) { return (lhs.udp_dest == rhs.udp_dest && lhs.udp_port_lidar == rhs.udp_port_lidar && lhs.udp_port_imu == rhs.udp_port_imu && - lhs.ts_mode == rhs.ts_mode && lhs.ld_mode == rhs.ld_mode && + lhs.timestamp_mode == rhs.timestamp_mode && + lhs.lidar_mode == rhs.lidar_mode && lhs.operating_mode == rhs.operating_mode && lhs.multipurpose_io_mode == rhs.multipurpose_io_mode && lhs.azimuth_window == rhs.azimuth_window && @@ -322,7 +291,7 @@ std::string client_version() { /* String conversion */ template -static optional lookup(const impl::Table table, const K& k) { +static optional lookup(const impl::Table& table, const K& k) { auto end = table.end(); auto res = std::find_if(table.begin(), end, [&](const std::pair& p) { return p.first == k; @@ -332,7 +301,7 @@ static optional lookup(const impl::Table table, const K& k) { } template -static optional rlookup(const impl::Table table, +static optional rlookup(const impl::Table& table, const char* v) { auto end = table.end(); auto res = std::find_if(table.begin(), end, @@ -414,11 +383,6 @@ std::string to_string(AzimuthWindow azimuth_window) { return ss.str(); } -std::string to_string(ChanField field) { - auto res = lookup(impl::chanfield_strings, field); - return res ? res.value() : "UNKNOWN"; -} - std::string to_string(ChanFieldType ft) { switch (ft) { case sensor::ChanFieldType::VOID: @@ -431,6 +395,18 @@ std::string to_string(ChanFieldType ft) { return "UINT32"; case sensor::ChanFieldType::UINT64: return "UINT64"; + case sensor::ChanFieldType::INT8: + return "INT8"; + case sensor::ChanFieldType::INT16: + return "INT16"; + case sensor::ChanFieldType::INT32: + return "INT32"; + case sensor::ChanFieldType::INT64: + return "INT64"; + case sensor::ChanFieldType::FLOAT32: + return "FLOAT32"; + case sensor::ChanFieldType::FLOAT64: + return "FLOAT64"; default: return "UNKNOWN"; } @@ -438,13 +414,19 @@ std::string to_string(ChanFieldType ft) { size_t field_type_size(ChanFieldType ft) { switch (ft) { + case sensor::ChanFieldType::INT8: case sensor::ChanFieldType::UINT8: return 1; + case sensor::ChanFieldType::INT16: case sensor::ChanFieldType::UINT16: return 2; + case sensor::ChanFieldType::INT32: case sensor::ChanFieldType::UINT32: + case sensor::ChanFieldType::FLOAT32: return 4; + case sensor::ChanFieldType::INT64: case sensor::ChanFieldType::UINT64: + case sensor::ChanFieldType::FLOAT64: return 8; default: return 0; @@ -624,10 +606,10 @@ sensor_config parse_config(const Json::Value& root) { if (!root["udp_port_imu"].empty()) config.udp_port_imu = root["udp_port_imu"].asInt(); if (!root["timestamp_mode"].empty()) - config.ts_mode = + config.timestamp_mode = timestamp_mode_of_string(root["timestamp_mode"].asString()); if (!root["lidar_mode"].empty()) - config.ld_mode = lidar_mode_of_string(root["lidar_mode"].asString()); + config.lidar_mode = lidar_mode_of_string(root["lidar_mode"].asString()); if (!root["azimuth_window"].empty()) config.azimuth_window = @@ -791,27 +773,27 @@ sensor_config parse_config(const std::string& config) { Json::Value config_to_json(const sensor_config& config) { Json::Value root{Json::objectValue}; - if (config.udp_dest) + if (config.udp_dest) root["udp_dest"] = config.udp_dest.value(); - if (config.udp_port_lidar) + if (config.udp_port_lidar) root["udp_port_lidar"] = config.udp_port_lidar.value(); - if (config.udp_port_imu) + if (config.udp_port_imu) root["udp_port_imu"] = config.udp_port_imu.value(); - if (config.ts_mode) - root["timestamp_mode"] = to_string(config.ts_mode.value()); + if (config.timestamp_mode) + root["timestamp_mode"] = to_string(config.timestamp_mode.value()); - if (config.ld_mode) - root["lidar_mode"] = to_string(config.ld_mode.value()); + if (config.lidar_mode) + root["lidar_mode"] = to_string(config.lidar_mode.value()); if (config.operating_mode) { auto mode = config.operating_mode.value(); root["operating_mode"] = to_string(mode); } - if (config.multipurpose_io_mode) + if (config.multipurpose_io_mode) root["multipurpose_io_mode"] = to_string(config.multipurpose_io_mode.value()); @@ -830,59 +812,57 @@ Json::Value config_to_json(const sensor_config& config) { } else { // jsoncpp < 1.7.7 strips 0s off of exact representation // so 2.0 becomes 2 - // On ubuntu 18.04, the default jsoncpp is 1.7.4-3 Fix was: - // https://github.com/open-source-parsers/jsoncpp/pull/547 // Work around by always casting to int before writing out to json int signal_multiplier_int = int(config.signal_multiplier.value()); root["signal_multiplier"] = signal_multiplier_int; } } - if (config.sync_pulse_out_angle) + if (config.sync_pulse_out_angle) root["sync_pulse_out_angle"] = config.sync_pulse_out_angle.value(); - if (config.sync_pulse_out_pulse_width) + if (config.sync_pulse_out_pulse_width) root["sync_pulse_out_pulse_width"] = config.sync_pulse_out_pulse_width.value(); - if (config.nmea_in_polarity) + if (config.nmea_in_polarity) root["nmea_in_polarity"] = to_string(config.nmea_in_polarity.value()); - if (config.nmea_baud_rate) + if (config.nmea_baud_rate) root["nmea_baud_rate"] = to_string(config.nmea_baud_rate.value()); - if (config.nmea_ignore_valid_char) + if (config.nmea_ignore_valid_char) root["nmea_ignore_valid_char"] = config.nmea_ignore_valid_char.value() ? 1 : 0; - if (config.nmea_leap_seconds) + if (config.nmea_leap_seconds) root["nmea_leap_seconds"] = config.nmea_leap_seconds.value(); - if (config.sync_pulse_in_polarity) + if (config.sync_pulse_in_polarity) root["sync_pulse_in_polarity"] = to_string(config.sync_pulse_in_polarity.value()); - if (config.sync_pulse_out_polarity) + if (config.sync_pulse_out_polarity) root["sync_pulse_out_polarity"] = to_string(config.sync_pulse_out_polarity.value()); - if (config.sync_pulse_out_frequency) + if (config.sync_pulse_out_frequency) root["sync_pulse_out_frequency"] = config.sync_pulse_out_frequency.value(); - if (config.phase_lock_enable) + if (config.phase_lock_enable) root["phase_lock_enable"] = config.phase_lock_enable.value(); - if (config.phase_lock_offset) + if (config.phase_lock_offset) root["phase_lock_offset"] = config.phase_lock_offset.value(); - if (config.columns_per_packet) + if (config.columns_per_packet) root["columns_per_packet"] = config.columns_per_packet.value(); - if (config.udp_profile_lidar) + if (config.udp_profile_lidar) root["udp_profile_lidar"] = to_string(config.udp_profile_lidar.value()); - if (config.udp_profile_imu) + if (config.udp_profile_imu) root["udp_profile_imu"] = to_string(config.udp_profile_imu.value()); // Firmware 3.1 and higher options @@ -911,44 +891,124 @@ std::string to_string(const sensor_config& config) { return Json::writeString(builder, root); } -} // namespace sensor +PacketValidationFailure LidarPacket::validate(const sensor_info& info, + const packet_format& format) { + if (buf.size() != format.lidar_packet_size) { + return PacketValidationFailure::PACKET_SIZE; + } -namespace util { + auto init_id = format.init_id(buf.data()); + if (info.init_id != 0 && init_id != 0 && init_id != info.init_id) { + return PacketValidationFailure::ID; + } -std::string to_string(const version& v) { - if (v == invalid_version) { - return "UNKNOWN"; + if (info.sn.length() > 0) { + auto p_sn = format.prod_sn(buf.data()); + auto m_sn = std::stoull(info.sn); + if (p_sn != 0 && p_sn != m_sn) { + return PacketValidationFailure::ID; + } } + return PacketValidationFailure::NONE; +} - std::stringstream ss{}; - ss << "v" << v.major << "." << v.minor << "." << v.patch; - return ss.str(); +PacketValidationFailure ImuPacket::validate(const sensor_info& /*info*/, + const packet_format& format) { + if (buf.size() != format.imu_packet_size) { + return PacketValidationFailure::PACKET_SIZE; + } + return PacketValidationFailure::NONE; } -version version_of_string(const std::string& s) { - std::istringstream is{s}; - char c1, c2, c3; - version v; +product_info product_info::create_product_info( + std::string product_info_string) { + std::regex product_regex("^(\\w+)-(\\d+|DOME)?(?:-(\\d+))?(?:-((?!SR)\\w+))?-?(SR)?"); + std::smatch matches; + if(product_info_string.length() > 0) { + if (regex_search(product_info_string, matches, product_regex) == true) { + std::string form_factor = matches.str(1) + matches.str(2); + bool short_range = (matches.str(5).length() > 0); + auto beam_config = matches.str(4); + if(beam_config.length() <= 0) { + beam_config = "U"; + } + int beam_count; + try { + beam_count = stoi(matches.str(3)); + } + catch(const std::exception &e) { + beam_count = 0; + } + + return product_info(product_info_string, + form_factor, + short_range, + beam_config, + beam_count); + } else { + throw std::runtime_error("Product Info \"" + product_info_string + "\" is not a recognized product info"); + } + } + return product_info(); +} - is >> c1 >> v.major >> c2 >> v.minor >> c3 >> v.patch; +product_info::product_info() : product_info("", "", false, "", 0) {}; - if (is && c1 == 'v' && c2 == '.' && c3 == '.') - return v; - else - return invalid_version; +product_info::product_info(std::string product_info_string, + std::string form_factor, + bool short_range, + std::string beam_config, + int beam_count) : + full_product_info(product_info_string), + form_factor(form_factor), + short_range(short_range), + beam_config(beam_config), + beam_count(beam_count) { } +bool operator==(const product_info& lhs, const product_info& rhs) { + return lhs.full_product_info == rhs.full_product_info && + lhs.form_factor == rhs.form_factor && + lhs.short_range == rhs.short_range && + lhs.beam_config == rhs.beam_config && + lhs.beam_count == rhs.beam_count; +} + +bool operator!=(const product_info& lhs, const product_info& rhs) { + return !(lhs == rhs); +} + +std::string to_string(const product_info& info) { + std::stringstream output; + output << "Product Info: " << std::endl; + output << "\tFull Product Info: \"" << info.full_product_info << "\"" << std::endl; + output << "\tForm Factor: \"" << info.form_factor << "\"" << std::endl; + output << "\tShort Range: \"" << info.short_range << "\"" << std::endl; + output << "\tBeam Config: \"" << info.beam_config << "\"" << std::endl; + output << "\tBeam Count: \"" << info.beam_count << "\"" << std::endl; + return output.str(); +} +} // namespace sensor + +namespace util { + version version_from_string(const std::string& v) { - auto rgx = std::regex(R"(v?(\d+).(\d+)\.(\d+))"); + auto rgx = std::regex(R"((([\w\d]*)-([\w\d]*)-)?v?(\d*)\.(\d*)\.(\d*)-?([\d\w.]*)?\+?([\d\w.]*)?)"); std::smatch matches; std::regex_search(v, matches, rgx); - if (matches.size() < 4) return invalid_version; + if (matches.size() < 9) return invalid_version; try { - return version{static_cast(stoul(matches[1])), - static_cast(stoul(matches[2])), - static_cast(stoul(matches[3]))}; + version v; + v.major = static_cast(stoul(matches[4])); + v.minor = static_cast(stoul(matches[5])); + v.patch = static_cast(stoul(matches[6])); + v.stage = matches[2]; + v.machine = matches[3]; + v.prerelease = matches[7]; + v.build = matches[8]; + return v; } catch (const std::exception&) { return invalid_version; } diff --git a/ouster_client/src/udp_packet_source.cpp b/ouster_client/src/udp_packet_source.cpp index f07d6178..95ab0659 100644 --- a/ouster_client/src/udp_packet_source.cpp +++ b/ouster_client/src/udp_packet_source.cpp @@ -11,9 +11,9 @@ #include #include -#include "logging.h" #include "ouster/client.h" #include "ouster/impl/client_poller.h" +#include "ouster/impl/logging.h" #include "ouster/types.h" namespace ouster { diff --git a/ouster_osf/CMakeLists.txt b/ouster_osf/CMakeLists.txt index ae07a49d..50279847 100644 --- a/ouster_osf/CMakeLists.txt +++ b/ouster_osf/CMakeLists.txt @@ -102,7 +102,6 @@ add_library(ouster_osf STATIC src/compat_ops.cpp src/operations.cpp src/json_utils.cpp src/fb_utils.cpp - src/pcap_source.cpp src/writer.cpp ) diff --git a/ouster_osf/fb/os_sensor/common.fbs b/ouster_osf/fb/os_sensor/common.fbs new file mode 100644 index 00000000..3a0e3538 --- /dev/null +++ b/ouster_osf/fb/os_sensor/common.fbs @@ -0,0 +1,34 @@ +namespace ouster.osf.v2; + +// sensor::ChanFieldType enum mapping +enum CHAN_FIELD_TYPE:uint8 { + VOID = 0, + UINT8 = 1, + UINT16 = 2, + UINT32 = 3, + UINT64 = 4, + INT8 = 5, + INT16 = 6, + INT32 = 7, + INT64 = 8, + FLOAT32 = 9, + FLOAT64 = 10 +} + +// int64 because of this: https://github.com/google/flatbuffers/issues/5161 +enum FIELD_CLASS:int64 { + NONE = 0, + PIXEL_FIELD = 1, + COLUMN_FIELD = 2, + PACKET_FIELD = 3, + SCAN_FIELD = 4 +} + +table Field { + name:string; + tag:CHAN_FIELD_TYPE; + shape:[uint64]; + field_class:FIELD_CLASS; + data:[uint8]; + bytes:uint64; +} diff --git a/ouster_osf/fb/os_sensor/lidar_scan_stream.fbs b/ouster_osf/fb/os_sensor/lidar_scan_stream.fbs index d24125cb..f72197b3 100644 --- a/ouster_osf/fb/os_sensor/lidar_scan_stream.fbs +++ b/ouster_osf/fb/os_sensor/lidar_scan_stream.fbs @@ -1,3 +1,5 @@ +include "common.fbs"; + namespace ouster.osf.v2; // sensor::ChanField enum mapping @@ -34,15 +36,6 @@ enum CHAN_FIELD:uint8 { RAW32_WORD4 = 63 } -// sensor::ChanFieldType enum mapping -enum CHAN_FIELD_TYPE:uint8 { - VOID = 0, - UINT8 = 1, - UINT16 = 2, - UINT32 = 3, - UINT64 = 4 -} - // PNG encoded channel fields of LidarScan table ChannelData { buffer:[uint8]; @@ -71,6 +64,9 @@ table LidarScanMsg { // has a col-major storage pose:[double]; packet_timestamp:[uint64]; + + // extra fields support for extensible lidar scan + custom_fields:[Field]; } // Scan data from a lidar sensor. One scan is a sweep of a sensor (360 degree). @@ -78,7 +74,7 @@ table LidarScanStream { sensor_id:uint32; // referenced to metadata.entry[].id with LidarScan // LidarScan field_types spec, used only as a HINT about what type - // of messages to expect from LidarScanMsg in a stream. + // of messages to expect from LidarScanMsg in a stream. // NOTE: For LidarScanMsg decoding field types from // LidarScanMsg.field_types[] should be used. field_types:[ChannelField]; @@ -86,4 +82,4 @@ table LidarScanStream { // MetadataEntry.type: ouster/v1/os_sensor/LidarScanStream root_type LidarScanStream; -file_identifier "oLSS"; \ No newline at end of file +file_identifier "oLSS"; diff --git a/ouster_osf/include/ouster/osf/basics.h b/ouster_osf/include/ouster/osf/basics.h index 5899f37d..d90e05d8 100644 --- a/ouster_osf/include/ouster/osf/basics.h +++ b/ouster_osf/include/ouster/osf/basics.h @@ -15,6 +15,7 @@ #include "ouster/types.h" // OSF basic types for LidarSensor and LidarScan/Imu Streams +#include "os_sensor/common_generated.h" #include "os_sensor/lidar_scan_stream_generated.h" #include "os_sensor/lidar_sensor_generated.h" @@ -148,36 +149,5 @@ bool check_prefixed_size_block_crc( /** @defgroup OsfBatchingFunctions Osf Batching Functions. */ -/** - * Makes the closure to batch lidar_packets and emit LidarScan object. - * Result returned through callback handler(ts, LidarScan). - * LidarScan uses user modified field types - * - * @ingroup OsfBatchingFunctions - * - * @param[in] info The sensor metadata to use. - * @param[in] ls_field_types The field types to use. - * @param[in] handler The callback to use on the results. - * @return Closure to batch and emit LidarScan objects. - */ -std::function make_build_ls( - const ouster::sensor::sensor_info& info, - const LidarScanFieldTypes& ls_field_types, - std::function handler); - -/** - * The above make_build_ls() function overload. In this function, LidarScan - * uses default field types by the profile - * - * @ingroup OsfBatchingFunctions - * - * @param[in] info The sensor metadata to use. - * @param[in] handler The callback to use on the results. - * @return Closure to batch and emit LidarScan objects. - */ -std::function make_build_ls( - const ouster::sensor::sensor_info& info, - std::function handler); - } // namespace osf } // namespace ouster diff --git a/ouster_osf/include/ouster/osf/metadata.h b/ouster_osf/include/ouster/osf/metadata.h index c93a3bb8..45999b39 100644 --- a/ouster_osf/include/ouster/osf/metadata.h +++ b/ouster_osf/include/ouster/osf/metadata.h @@ -261,6 +261,9 @@ std::shared_ptr metadata_pointer_as( } }; +/** @internal */ +void RegisterMetadata_internal_error_function_(std::string error); + /** * Registrar class helper to add static from_buffer() function of the concrete * derived metadata class to the registry. @@ -351,9 +354,12 @@ struct RegisterMetadata { auto& registry = MetadataEntry::get_registry(); auto type = metadata_type(); if (registry.find(type) != registry.end()) { - std::cerr << "ERROR: Duplicate metadata type? Already registered " - "type found: " - << type << std::endl; + std::stringstream error_string; + error_string << "ERROR: Duplicate metadata type?"; + error_string << "Already registered "; + error_string << "type found: "; + error_string << type; + RegisterMetadata_internal_error_function_(error_string.str()); return false; } registry.insert(std::make_pair(type, MetadataDerived::from_buffer)); diff --git a/ouster_osf/include/ouster/osf/pcap_source.h b/ouster_osf/include/ouster/osf/pcap_source.h deleted file mode 100644 index 85d30244..00000000 --- a/ouster_osf/include/ouster/osf/pcap_source.h +++ /dev/null @@ -1,144 +0,0 @@ -/** - * Copyright (c) 2021, Ouster, Inc. - * All rights reserved. - * - * @file pcap_source.h - * @brief Pcap raw data source - */ -#pragma once - -#include -#include - -#include "ouster/lidar_scan.h" -#include "ouster/os_pcap.h" -#include "ouster/osf/basics.h" -#include "ouster/types.h" - -namespace ouster { -namespace osf { - -/** - * Wrapper to process pcap files with lidar packet sensor data. - * Currently supports a single sensor, but can be extended easily for - * multisensor pcaps. - */ -class PcapRawSource { - public: - using ts_t = std::chrono::nanoseconds; - - /** - * Lidar data callbacks - * - * @param[in] timestamp The timestamp for the scan. - * @param[in] scan The LidarScan object. - */ - using LidarDataHandler = std::function; - - /** - * General pcap packet handler - * - * @param[in] info The sensor_info for the packet. - * @param[in] buf The raw buffer for the packet. - */ - using PacketHandler = std::function; - - /** - * Predicate to control the bag run loop - * - * @param[in] info The sensor_info for the packet. - * @return True if the loop should continue, False if the loop should halt. - */ - using PacketInfoPredicate = - std::function; - - /** - * Opens pcap file and checks available packets inside with - * heuristics applied to guess Ouster lidar port with data. - * - * @param[in] filename The filename of the pcap file to open. - */ - PcapRawSource(const std::string& filename); - - /** - * Attach lidar data handler to the port that receives already - * batched LidarScans with a timestamp of the first UDP lidar packet. - * LidarScan uses default field types by the profile - * - * @param[in] dst_port The destination port for the target lidar stream. - * @param[in] info The sensor info for the stream. - * @param[in] lidar_handler The callback to call on packet. - */ - void addLidarDataHandler(int dst_port, - const ouster::sensor::sensor_info& info, - LidarDataHandler&& lidar_handler); - - /** - * @copydoc addLidarDataHandler - * @param[in] ls_field_types The LidarScan field types to use. - */ - void addLidarDataHandler(int dst_port, - const ouster::sensor::sensor_info& info, - const LidarScanFieldTypes& ls_field_types, - LidarDataHandler&& lidar_handler); - - /** - * Read all packets from pcap and pass data to the attached handlers - * based on `dst_port` from pcap packets. - */ - void runAll(); - - /** - * Run the internal loop through all packets while the - * `pred(pinfo) == true`. - * `pred` function called before reading packet buffer and passing to the - * appropriate handlers. - * - * @param[in] pred The predicate function to decide whether to continue or - * not. - */ - void runWhile(const PacketInfoPredicate& pred); - - /** - * Close the pcap file. - */ - ~PcapRawSource(); - - private: - /// Remove some stuff - PcapRawSource(const PcapRawSource&) = delete; - PcapRawSource& operator=(const PcapRawSource&) = delete; - - /** - * Read current packet and dispatch handlers accordingly. - * - * @param[in] pinfo The new packet info. - */ - void handleCurrentPacket(const sensor_utils::packet_info& pinfo); - - /** - * The pcap file path. - */ - std::string pcap_filename_; - - /** - * The associated sensor_info. - */ - ouster::sensor::sensor_info info_; - - /** - * The internal pcap file handler. - */ - std::shared_ptr pcap_handle_{ - nullptr}; - - /** - * Map containing a 'destination port' to 'handler' mapping. - */ - std::map packet_handlers_{}; -}; - -} // namespace osf -} // namespace ouster diff --git a/ouster_osf/include/ouster/osf/stream_lidar_scan.h b/ouster_osf/include/ouster/osf/stream_lidar_scan.h index cf393138..c06eae86 100644 --- a/ouster_osf/include/ouster/osf/stream_lidar_scan.h +++ b/ouster_osf/include/ouster/osf/stream_lidar_scan.h @@ -28,7 +28,7 @@ namespace osf { * @return a copy of `ls_src` with transformed fields. */ LidarScan slice_with_cast(const LidarScan& ls_src, - const LidarScanFieldTypes& field_types); + const ouster::LidarScanFieldTypes& field_types); /** * Metadata entry for LidarScanStream to store reference to a sensor and @@ -48,7 +48,7 @@ class LidarScanStreamMeta : public MetadataEntryHelper { * @param[in] field_types LidarScan fields specs, this argument is optional. */ LidarScanStreamMeta(const uint32_t sensor_meta_id, - const LidarScanFieldTypes field_types = {}); + const ouster::LidarScanFieldTypes field_types = {}); /** * Return the sensor meta id. @@ -57,13 +57,6 @@ class LidarScanStreamMeta : public MetadataEntryHelper { */ uint32_t sensor_meta_id() const; - /** - * Return the field types. - * - * @return The field types. - */ - const LidarScanFieldTypes& field_types() const; - /** * @copydoc MetadataEntry::buffer */ @@ -106,7 +99,7 @@ class LidarScanStreamMeta : public MetadataEntryHelper { * Flat Buffer Reference: * fb/os_sensor/lidar_scan_stream.fbs :: LidarScanStream :: field_types */ - LidarScanFieldTypes field_types_; + ouster::LidarScanFieldTypes field_types_; }; /** @defgroup OSFTraitsLidarScanStreamMeta Templated struct for traits.*/ @@ -190,7 +183,7 @@ class LidarScanStream : public MessageStream { * @param[in] field_types LidarScan fields specs, this argument is optional. */ LidarScanStream(Token key, Writer& writer, const uint32_t sensor_meta_id, - const LidarScanFieldTypes& field_types = {}); + const ouster::LidarScanFieldTypes& field_types = {}); /** * Return the concrete metadata type. @@ -226,6 +219,11 @@ class LidarScanStream : public MessageStream { * The internal sensor_info data. */ sensor::sensor_info sensor_info_; + + /** + * The internal field_types data. + */ + ouster::LidarScanFieldTypes field_types_; }; } // namespace osf diff --git a/ouster_osf/include/ouster/osf/writer.h b/ouster_osf/include/ouster/osf/writer.h index dc97c662..7addcb93 100644 --- a/ouster_osf/include/ouster/osf/writer.h +++ b/ouster_osf/include/ouster/osf/writer.h @@ -81,13 +81,14 @@ class Writer { * @param[in] info The sensor info to use for a single stream OSF file. * @param[in] chunk_size The chunksize to use for the OSF file, this * parameter is optional. - * @param[in] field_types The fields from scans to actually save into the - * OSF. If not provided uses the fields from the - * first saved lidar scan for each stream. This - * parameter is optional. + * @param[in] fields_to_write The fields from scans to actually save into + * the OSF. If not provided uses the fields from + * the first saved lidar scan for this sensor. + * This parameter is optional. */ Writer(const std::string& filename, const ouster::sensor::sensor_info& info, - const LidarScanFieldTypes& field_types = LidarScanFieldTypes(), + const std::vector& fields_to_write = + std::vector(), uint32_t chunk_size = 0); /** @@ -96,14 +97,15 @@ class Writer { * file. * @param[in] chunk_size The chunksize to use for the OSF file, this * parameter is optional. - * @param[in] field_types The fields from scans to actually save into the - * OSF. If not provided uses the fields from the - * first saved lidar scan for each stream. This - * parameter is optional. + * @param[in] fields_to_write The fields from scans to actually save into + * the OSF. If not provided uses the fields from + * the first saved lidar scan for this sensor. + * This parameter is optional. */ Writer(const std::string& filename, const std::vector& info, - const LidarScanFieldTypes& field_types = LidarScanFieldTypes(), + const std::vector& fields_to_write = + std::vector(), uint32_t chunk_size = 0); /** @@ -191,16 +193,16 @@ class Writer { * to write scans to it's stream. * * @param[in] info The info of the sensor to add to the file. - * @param[in] field_types The fields from scans to actually save into the - * OSF. If not provided uses the fields from the - * first saved lidar scan for this sensor. This - * parameter is optional. + * @param[in] fields_to_write The fields from scans to actually save into + * the OSF. If not provided uses the fields from + * the first saved lidar scan for this sensor. + * This parameter is optional. * * @return The stream index for the newly added sensor. */ - uint32_t add_sensor( - const ouster::sensor::sensor_info& info, - const LidarScanFieldTypes& field_types = LidarScanFieldTypes()); + uint32_t add_sensor(const ouster::sensor::sensor_info& info, + const std::vector& fields_to_write = + std::vector()); /** * Save a single scan to the specified stream_index in an OSF file. @@ -530,6 +532,11 @@ class Writer { */ std::vector field_types_; + /** + * Internal store of what fields the user wants to save from each scan. + */ + std::vector> desired_fields_; + /** * Internal stream index to metadata map. */ diff --git a/ouster_osf/src/basics.cpp b/ouster_osf/src/basics.cpp index 9d8c69ac..471fe9b5 100644 --- a/ouster_osf/src/basics.cpp +++ b/ouster_osf/src/basics.cpp @@ -11,8 +11,11 @@ #include #include "nonstd/optional.hpp" +#include "ouster/impl/logging.h" #include "ouster/osf/crc32.h" +using namespace ouster::sensor; + namespace ouster { namespace osf { @@ -119,8 +122,9 @@ bool check_prefixed_size_block_crc(const uint8_t* buf, uint32_t prefixed_size = get_prefixed_size(buf); if (buf_length < prefixed_size + FLATBUFFERS_PREFIX_LENGTH + ouster::osf::CRC_BYTES_SIZE) { - std::cerr << "ERROR: CRC32 validation failed!" - << " (out of buffer legth)" << std::endl; + logger().error( + "ERROR: CRC32 validation failed!" + " (out of buffer legth)"); return false; } @@ -132,52 +136,17 @@ bool check_prefixed_size_block_crc(const uint8_t* buf, const bool res = (crc_stored == crc_calculated); if (!res) { - std::cerr << "ERROR: CRC32 validation failed!" << std::endl; - std::cerr << std::hex << " CRC - stored: " << crc_stored - << std::dec << std::endl; - std::cerr << std::hex << " CRC - calculated: " << crc_calculated - << std::dec << std::endl; + std::stringstream error_string_stream{}; + error_string_stream << "ERROR: CRC32 validation failed!" << std::endl; + error_string_stream << std::hex << " CRC - stored: " << crc_stored + << std::dec << std::endl; + error_string_stream + << std::hex << " CRC - calculated: " << crc_calculated << std::dec + << std::endl; + logger().error(error_string_stream.str()); } return res; } -std::function make_build_ls( - const ouster::sensor::sensor_info& info, - const LidarScanFieldTypes& ls_field_types, - std::function handler) { - const auto w = info.format.columns_per_frame; - const auto h = info.format.pixels_per_column; - auto temp_ls_field_types = ls_field_types; - std::shared_ptr ls(nullptr); - if (temp_ls_field_types.empty()) { - temp_ls_field_types = get_field_types(info); - } - ls = std::make_shared(w, h, temp_ls_field_types.begin(), - temp_ls_field_types.end()); - - auto pf = ouster::sensor::get_format(info); - auto build_ls_imp = ScanBatcher(w, pf); - osf::ts_t first_msg_ts{-1}; - return [handler, build_ls_imp, ls, first_msg_ts]( - const osf::ts_t msg_ts, const uint8_t* buf) mutable { - if (first_msg_ts == osf::ts_t{-1}) { - first_msg_ts = msg_ts; - } - if (build_ls_imp(buf, *ls)) { - handler(first_msg_ts, *ls); - // At this point we've just started accumulating new LidarScan, so - // we are saving the msg_ts (i.e. timestamp of a UDP packet) - // which contained the first lidar_packet - first_msg_ts = msg_ts; - } - }; -} - -std::function make_build_ls( - const ouster::sensor::sensor_info& info, - std::function handler) { - return make_build_ls(info, {}, handler); -} - } // namespace osf } // namespace ouster diff --git a/ouster_osf/src/compat_ops.cpp b/ouster_osf/src/compat_ops.cpp index 0e316cfd..715ece8e 100644 --- a/ouster_osf/src/compat_ops.cpp +++ b/ouster_osf/src/compat_ops.cpp @@ -9,6 +9,8 @@ #include #include +#include "ouster/impl/logging.h" + #ifdef _WIN32 #include #include @@ -25,6 +27,8 @@ #include #endif +using namespace ouster::sensor; + namespace ouster { namespace osf { @@ -48,8 +52,7 @@ LPCTSTR ErrorMessage(DWORD error) { // Simple wrapper function for error output. void PrintError(LPCTSTR errDesc) { LPCTSTR errMsg = ErrorMessage(GetLastError()); - _ftprintf(stderr, TEXT("\nERROR: %s SYSTEM RETURNED: %s\n"), errDesc, - errMsg); + logger().error("ERROR: {} SYSTEM RETURNED: {}", errDesc, errMsg); LocalFree((LPVOID)errMsg); } @@ -89,7 +92,9 @@ bool is_dir(const std::string& path) { #else struct stat statbuf; if (stat(path.c_str(), &statbuf) != 0) { - if (errno != ENOENT) printf("ERROR: stat: %s", std::strerror(errno)); + if (errno != ENOENT) { + logger().error("ERROR: stat: {}", std::strerror(errno)); + } return false; } return S_ISDIR(statbuf.st_mode); @@ -146,14 +151,14 @@ bool make_tmp_dir(std::string& tmp_path) { return true; } } - PrintError(TEXT("Can't create temp dir.")); + logger().error("ERROR: Can't create temp dir."); return false; #else // TODO[pb]: Check that it works on Mac OS and especially that // temp files are cleaned correctly and don't feel up the CI machine ... char tmpdir[] = "/tmp/ouster-test.XXXXXX"; if (::mkdtemp(tmpdir) == nullptr) { - std::cerr << "ERROR: Can't create temp dir." << std::endl; + logger().error("ERROR: Can't create temp dir."); return false; }; tmp_path = tmpdir; @@ -164,10 +169,11 @@ bool make_tmp_dir(std::string& tmp_path) { /// Make directory bool make_dir(const std::string& path) { #ifdef _WIN32 + logger().error("ERROR: Can't create dir: {}", path); return (CreateDirectoryA(path.c_str(), NULL) != 0); #else if (mkdir(path.c_str(), 0777) != 0) { - printf("ERROR: Can't create dir: %s\n", path.c_str()); + logger().error("ERROR: Can't create dir: {}", path); return false; } return true; @@ -317,7 +323,9 @@ int64_t truncate_file(const std::string& path, uint64_t filesize) { _close(file_handle); } #else - truncate(path.c_str(), filesize); + if (truncate(path.c_str(), filesize) != 0) { + return -1; + } #endif return file_size(path); } @@ -340,19 +348,15 @@ int64_t append_binary_file(const std::string& append_to_file_name, if (append_to_file_stream.is_open()) { if (append_from_file_stream.is_open()) { - append_from_file_stream.seekg(0, std::ios::end); - uint64_t from_file_size = append_from_file_stream.tellg(); - append_from_file_stream.seekg(0, std::ios::beg); - - append_to_file_stream.seekg(0, std::ios::end); - append_to_file_stream << append_from_file_stream.rdbuf(); saved_size = append_to_file_stream.tellg(); } else { - std::cerr << "fail to open " << append_to_file_name << std::endl; + logger().error("ERROR: Failed to open {} for appending", + append_to_file_name); } } else { - std::cerr << "fail to open " << append_from_file_name << std::endl; + logger().error("ERROR: Failed to open {} for appending", + append_to_file_name); } if (append_to_file_stream.is_open()) append_to_file_stream.close(); @@ -389,10 +393,10 @@ int64_t copy_file_trailing_bytes(const std::string& source_file, target_file_stream << source_file_stream.rdbuf(); saved_size = target_file_stream.tellg(); } else { - std::cerr << "fail to open " << source_file << std::endl; + logger().error("ERROR: Failed to open {} for copy", target_file); } } else { - std::cerr << "fail to open " << target_file << std::endl; + logger().error("ERROR: Failed to open {} for copy", target_file); } if (source_file_stream.is_open()) source_file_stream.close(); diff --git a/ouster_osf/src/fb_utils.cpp b/ouster_osf/src/fb_utils.cpp index f563ef3a..5e5f0642 100644 --- a/ouster_osf/src/fb_utils.cpp +++ b/ouster_osf/src/fb_utils.cpp @@ -8,9 +8,12 @@ #include #include +#include "ouster/impl/logging.h" #include "ouster/osf/basics.h" #include "ouster/osf/crc32.h" +using namespace ouster::sensor; + namespace ouster { namespace osf { @@ -68,7 +71,7 @@ uint64_t buffer_to_file(const uint8_t* buf, const uint64_t size, file_stream.close(); saved_size = size + 4; } else { - std::cerr << "fail to open " << filename << std::endl; + logger().error("ERROR: Failed to open {} for writing", filename); } return saved_size; } @@ -126,7 +129,7 @@ uint64_t finish_osf_file(const std::string& filename, file_stream.close(); } else { - std::cout << "fail to open " << filename << std::endl; + logger().error("ERROR: Failed to open {} for writing", filename); } return saved_size; diff --git a/ouster_osf/src/file.cpp b/ouster_osf/src/file.cpp index 70011abf..b2714af9 100644 --- a/ouster_osf/src/file.cpp +++ b/ouster_osf/src/file.cpp @@ -14,8 +14,11 @@ #include "compat_ops.h" #include "fb_utils.h" +#include "ouster/impl/logging.h" #include "ouster/osf/crc32.h" +using namespace ouster::sensor; + namespace ouster { namespace osf { @@ -24,7 +27,7 @@ namespace { // Print errors only in DEBUG mode #ifndef NDEBUG inline void print_error(const std::string& filename, const std::string& msg) { - fprintf(stderr, "Error Osf[%s]: %s\n", filename.c_str(), msg.c_str()); + logger().error("ERROR: Osf[{}]: {}", filename, msg); } #else #define print_error(a, b) ((void)0) diff --git a/ouster_osf/src/meta_lidar_sensor.cpp b/ouster_osf/src/meta_lidar_sensor.cpp index 6162a68c..a986f136 100644 --- a/ouster_osf/src/meta_lidar_sensor.cpp +++ b/ouster_osf/src/meta_lidar_sensor.cpp @@ -55,11 +55,10 @@ std::unique_ptr restore_lidar_sensor( } LidarSensor::LidarSensor(const sensor_info& si) - : sensor_info_(si), metadata_(si.updated_metadata_string()) {} + : sensor_info_(si), metadata_(si.to_json_string()) {} LidarSensor::LidarSensor(const std::string& sensor_metadata) - : sensor_info_(sensor::parse_metadata(sensor_metadata)), - metadata_(sensor_metadata) {} + : sensor_info_(sensor_metadata), metadata_(sensor_metadata) {} const sensor_info& LidarSensor::info() const { return sensor_info_; } diff --git a/ouster_osf/src/metadata.cpp b/ouster_osf/src/metadata.cpp index 02623534..4efa2081 100644 --- a/ouster_osf/src/metadata.cpp +++ b/ouster_osf/src/metadata.cpp @@ -6,10 +6,17 @@ #include "ouster/osf/metadata.h" #include "fb_utils.h" +#include "ouster/impl/logging.h" + +using namespace ouster::sensor; namespace ouster { namespace osf { +void RegisterMetadata_internal_error_function_(std::string error) { + logger().error(error); +} + std::string MetadataEntry::repr() const { auto b = this->buffer(); std::stringstream ss; @@ -42,13 +49,12 @@ std::unique_ptr MetadataEntry::from_buffer( auto& registry = MetadataEntry::get_registry(); auto registered_type = registry.find(type_str); if (registered_type == registry.end()) { - std::cout << "UNKNOWN TYPE: " << type_str << std::endl; + logger().error("UNKNOWN TYPE: {}", type_str); return nullptr; } auto m = registered_type->second(buf); if (m == nullptr) { - std::cout << "UNRECOVERABLE FROM BUFFER as type: " << type_str - << std::endl; + logger().error("UNRECOVERABLE FROM BUFFER as type: {}", type_str); return nullptr; } return m; @@ -87,12 +93,12 @@ std::unique_ptr MetadataEntryRef::as_type() const { auto& registry = MetadataEntry::get_registry(); auto registered_type = registry.find(type()); if (registered_type == registry.end()) { - std::cout << "UNKNOWN TYPE FOUND: " << type() << std::endl; + logger().error("UNKNOWN TYPE FOUND: {}", type()); return nullptr; } auto m = registered_type->second(buffer()); if (m == nullptr) { - std::cout << "UNRECOVERABLE FROM BUFFER: " << to_string() << std::endl; + logger().error("UNRECOVERABLE FROM BUFFER: {}", to_string()); return nullptr; } m->setId(id()); @@ -121,8 +127,8 @@ uint32_t MetadataStore::add(MetadataEntry& entry) { /// the Reader case assignId(entry); } else if (metadata_entries_.find(entry.id()) != metadata_entries_.end()) { - std::cout << "WARNING: MetadataStore: ENTRY EXISTS! id = " << entry.id() - << std::endl; + logger().warn("WARNING: MetadataStore: ENTRY EXISTS! id = {}", + entry.id()); return entry.id(); } else if (next_meta_id_ == entry.id()) { // Find next available next_meta_id_ so we avoid id collisions diff --git a/ouster_osf/src/operations.cpp b/ouster_osf/src/operations.cpp index b1522335..4f95311f 100644 --- a/ouster_osf/src/operations.cpp +++ b/ouster_osf/src/operations.cpp @@ -13,15 +13,17 @@ #include "fb_utils.h" #include "json/json.h" #include "json_utils.h" +#include "ouster/impl/logging.h" #include "ouster/lidar_scan.h" #include "ouster/osf/file.h" #include "ouster/osf/meta_extrinsics.h" #include "ouster/osf/meta_lidar_sensor.h" -#include "ouster/osf/pcap_source.h" #include "ouster/osf/reader.h" #include "ouster/osf/stream_lidar_scan.h" #include "ouster/osf/writer.h" +using namespace ouster::sensor; + namespace ouster { namespace osf { @@ -198,14 +200,13 @@ flatbuffers::FlatBufferBuilder _generate_modify_metadata_fbb( /// @todo on OsfFile refactor, make a copy constructor for MetadataStore MetadataStore new_meta_store; auto old_meta_store = reader.meta_store(); - std::cout << "Looking for non sensor info metadata in old metastore" - << std::endl; + logger().info("Looking for non sensor info metadata in old metastore"); for (const auto& entry : old_meta_store.entries()) { std::cout << "Found: " << entry.second->type() << " "; /// @todo figure out why there isnt an easy def for this if (entry.second->type() != "ouster/v1/os_sensor/LidarSensor") { new_meta_store.add(*entry.second); - std::cout << "Is non sensor_info, adding" << std::endl; + logger().info("Is non sensor_info, adding"); } else { std::cout << std::endl; } diff --git a/ouster_osf/src/pcap_source.cpp b/ouster_osf/src/pcap_source.cpp deleted file mode 100644 index aa80778b..00000000 --- a/ouster_osf/src/pcap_source.cpp +++ /dev/null @@ -1,79 +0,0 @@ -/** - * Copyright(c) 2021, Ouster, Inc. - * All rights reserved. - */ - -#include "ouster/osf/pcap_source.h" - -namespace ouster { -namespace osf { - -template -PcapRawSource::PacketHandler use_packet(H&& handler) { - return - [handler](const sensor_utils::packet_info& p_info, const uint8_t* buf) { - osf::ts_t ts(p_info.timestamp); - handler(ts, buf); - }; -} - -PcapRawSource::PcapRawSource(const std::string& filename) - : pcap_filename_{filename} { - pcap_handle_ = sensor_utils::replay_initialize(pcap_filename_); -} - -void PcapRawSource::runAll() { - sensor_utils::packet_info p_info; - while (sensor_utils::next_packet_info(*pcap_handle_, p_info)) { - handleCurrentPacket(p_info); - } -} - -void PcapRawSource::runWhile(const PacketInfoPredicate& pred) { - sensor_utils::packet_info p_info; - while (sensor_utils::next_packet_info(*pcap_handle_, p_info)) { - if (!pred(p_info)) { - break; - } - handleCurrentPacket(p_info); - } -} - -void PcapRawSource::addLidarDataHandler(int dst_port, - const sensor::sensor_info& info, - LidarDataHandler&& lidar_handler) { - auto build_ls = osf::make_build_ls(info, lidar_handler); - packet_handlers_.insert({dst_port, use_packet(build_ls)}); -} - -void PcapRawSource::addLidarDataHandler( - int dst_port, const sensor::sensor_info& info, - const LidarScanFieldTypes& ls_field_types, - LidarDataHandler&& lidar_handler) { - auto build_ls = osf::make_build_ls(info, ls_field_types, lidar_handler); - packet_handlers_.insert({dst_port, use_packet(build_ls)}); -} - -void PcapRawSource::handleCurrentPacket( - const sensor_utils::packet_info& pinfo) { - constexpr uint32_t buf_size = 65536; // 2^16 - uint8_t buf[buf_size]; - auto handler_it = packet_handlers_.find(pinfo.dst_port); - if (handler_it != packet_handlers_.end()) { - auto size_read = - sensor_utils::read_packet(*pcap_handle_, buf, buf_size); - if (size_read > 0 && size_read < buf_size && - size_read == pinfo.payload_size) { - handler_it->second(pinfo, buf); - } - } -} - -PcapRawSource::~PcapRawSource() { - if (pcap_handle_) { - sensor_utils::replay_uninitialize(*pcap_handle_); - } -} - -} // namespace osf -} // namespace ouster diff --git a/ouster_osf/src/png_tools.cpp b/ouster_osf/src/png_tools.cpp index 82cd213d..e18f38fd 100644 --- a/ouster_osf/src/png_tools.cpp +++ b/ouster_osf/src/png_tools.cpp @@ -13,12 +13,15 @@ #include #include +#include "ouster/impl/logging.h" #include "ouster/lidar_scan.h" +using namespace ouster::sensor; + namespace ouster { namespace osf { -/* +/** * Effect of png_set_compression(comp level): * - (no png out): 2s, n/a * - comp level 1: 39s, 648M (60% speedup vs default, 10% size increase) @@ -29,7 +32,7 @@ namespace osf { * - libpng default: 98s, 586M * - comp level 9: 328s, 580M * - * TODO: investigate other zlib options + * @todo investigate other zlib options */ static constexpr int PNG_OSF_ZLIB_COMPRESSION_LEVEL = 4; @@ -57,10 +60,43 @@ struct VectorReader { * Error callback that will be fired on libpng errors */ void png_osf_error(png_structp png_ptr, png_const_charp msg) { - std::cout << "ERROR libpng osf: " << msg << std::endl; + logger().error("ERROR libpng osf: {}", msg); longjmp(png_jmpbuf(png_ptr), 1); }; +/** @internal */ +inline void print_incompatable_image_size(png_uint_32 actual_width, + png_uint_32 actual_height, + png_uint_32 expected_width, + png_uint_32 expected_height) { + logger().error( + "ERROR: img contains data of incompatible size: " + " {}x{}, expected: {}x{}", + actual_width, actual_height, expected_width, expected_height); +} + +/** @internal */ +inline void print_bad_sample_depth(int actual, int expected) { + logger().error( + "ERROR: encoded img contains data " + "with incompatible sample_depth: {}" + ", expected: {}", + actual, expected); +} + +/** @internal */ +inline void print_bad_color_type(int actual, int expected) { + logger().error( + "ERROR: encoded img contains data with incompatible " + "color type: {}, expected: {}", + actual, expected); +} + +inline void print_bad_pixel_size() { + logger().error( + "WARNING: Attempt to decode image" + " of bigger pixel size"); +} /** * Custom png_write handler to write data to std::vector buffer */ @@ -96,13 +132,13 @@ bool png_osf_write_init(png_structpp png_ptrp, png_infopp png_info_ptrp) { *png_ptrp = png_create_write_struct(PNG_LIBPNG_VER_STRING, nullptr, png_osf_error, png_osf_error); if (!*png_ptrp) { - std::cout << "ERROR: no png_ptr\n"; + logger().error("ERROR: no png_ptr"); return true; } *png_info_ptrp = png_create_info_struct(*png_ptrp); if (!*png_info_ptrp) { - std::cout << "ERROR: no png_info_ptr\n"; + logger().error("ERROR: no png_info_ptr"); png_destroy_write_struct(png_ptrp, nullptr); return true; } @@ -117,13 +153,13 @@ bool png_osf_read_init(png_structpp png_ptrp, png_infopp png_info_ptrp) { *png_ptrp = png_create_read_struct(PNG_LIBPNG_VER_STRING, nullptr, png_osf_error, png_osf_error); if (!*png_ptrp) { - std::cout << "ERROR: no png_ptr\n"; + logger().error("ERROR: no png_ptr"); return true; } *png_info_ptrp = png_create_info_struct(*png_ptrp); if (!*png_info_ptrp) { - std::cout << "ERROR: no png_info_ptr\n"; + logger().error("ERROR: no png_info_ptr"); png_destroy_read_struct(png_ptrp, nullptr, nullptr); return true; } @@ -631,18 +667,17 @@ void fieldEncodeMulti(const LidarScan& lidar_scan, auto err = fieldEncode(lidar_scan, field_types[i], px_offset, scan_data, scan_idxs[i]); if (err) { - std::cerr << "ERROR: fieldEncode: Can't encode field [" - << sensor::to_string(field_types[i]) - << "] (in " - "fieldEncodeMulti)" - << std::endl; + logger().error( + "ERROR: fieldEncode: Can't encode field [{}]" + "(in fieldEncodeMulti)", + field_types[i].first); } } } bool fieldEncode( const LidarScan& lidar_scan, - const std::pair field_type, + const std::pair& field_type, const std::vector& px_offset, ScanData& scan_data, size_t scan_idx) { if (scan_idx >= scan_data.size()) { throw std::invalid_argument( @@ -672,49 +707,50 @@ bool fieldEncode( px_offset); break; default: - std::cerr << "ERROR: fieldEncode: UNKNOWN: ChanFieldType not yet " - "implemented" - << std::endl; + logger().error( + "ERROR: fieldEncode: UNKNOWN:" + "ChanFieldType not yet " + "implemented"); break; } if (res) { - std::cerr << "ERROR: fieldEncode: Can't encode field " - << sensor::to_string(field_type.first) << std::endl; + logger().error("ERROR: fieldEncode: Can't encode field {}", + field_type.first); } return res; } ScanData scanEncode(const LidarScan& lidar_scan, - const std::vector& px_offset) { + const std::vector& px_offset, + const LidarScanFieldTypes& field_types) { #ifdef OUSTER_OSF_NO_THREADING - return scanEncodeFieldsSingleThread(lidar_scan, px_offset, - {lidar_scan.begin(), lidar_scan.end()}); + return scanEncodeFieldsSingleThread(lidar_scan, px_offset, field_types); #else - return scanEncodeFields(lidar_scan, px_offset, - {lidar_scan.begin(), lidar_scan.end()}); + return scanEncodeFields(lidar_scan, px_offset, field_types); #endif } // ========== Decode Functions =================================== bool scanDecode(LidarScan& lidar_scan, const ScanData& scan_data, - const std::vector& px_offset) { + const std::vector& px_offset, + const ouster::LidarScanFieldTypes& field_types) { #ifdef OUSTER_OSF_NO_THREADING - return scanDecodeFieldsSingleThread(lidar_scan, scan_data, px_offset); + return scanDecodeFieldsSingleThread(lidar_scan, scan_data, px_offset, + field_types); #else - return scanDecodeFields(lidar_scan, scan_data, px_offset); + return scanDecodeFields(lidar_scan, scan_data, px_offset, field_types); #endif } bool fieldDecode( LidarScan& lidar_scan, const ScanData& scan_data, size_t start_idx, - const std::pair field_type, + const std::pair& field_type, const std::vector& px_offset) { switch (field_type.second) { case sensor::ChanFieldType::UINT8: return decode8bitImage(lidar_scan.field(field_type.first), scan_data[start_idx], px_offset); - return true; case sensor::ChanFieldType::UINT16: return decode16bitImage( lidar_scan.field(field_type.first), @@ -727,11 +763,11 @@ bool fieldDecode( return decode64bitImage( lidar_scan.field(field_type.first), scan_data[start_idx], px_offset); - return true; default: - std::cout << "ERROR: fieldDecode: UNKNOWN: ChanFieldType not yet " - "implemented" - << std::endl; + logger().error( + "ERROR: fieldDecode: UNKNOWN:" + "ChanFieldType not yet " + "implemented"); return true; } return true; // ERROR @@ -751,29 +787,36 @@ bool fieldDecodeMulti(LidarScan& lidar_scan, const ScanData& scan_data, auto err = fieldDecode(lidar_scan, scan_data, scan_idxs[i], field_types[i], px_offset); if (err) { - std::cerr << "ERROR: fieldDecodeMulti: Can't decode field [" - << sensor::to_string(field_types[i]) << "]" << std::endl; + logger().error( + "ERROR: fieldDecodeMulti: " + "Can't decode field [{}]", + field_types[i].first); } res_err = res_err || err; } return res_err; } #ifdef OUSTER_OSF_NO_THREADING -bool scanDecodeFieldsSingleThread(LidarScan& lidar_scan, - const ScanData& scan_data, - const std::vector& px_offset) { - size_t fields_cnt = std::distance(lidar_scan.begin(), lidar_scan.end()); +bool scanDecodeFieldsSingleThread( + LidarScan& lidar_scan, const ScanData& scan_data, + const std::vector& px_offset, + const ouster::LidarScanFieldTypes& field_types) { + size_t fields_cnt = lidar_scan.fields().size(); if (scan_data.size() != fields_cnt) { - std::cerr << "ERROR: lidar_scan data contains # of channels: " - << scan_data.size() << ", expected: " << fields_cnt - << " for OSF_EUDP" << std::endl; + logger().error( + "ERROR: lidar_scan data contains # of channels: {}" + ", expected: {} for OSF_EUDP", + scan_data.size(), fields_cnt); return true; } size_t next_idx = 0; - for (auto f : lidar_scan) { - if (fieldDecode(lidar_scan, scan_data, next_idx, f, px_offset)) { - std::cout << "ERROR: scanDecodeFields: Failed to decode field" - << std::endl; + for (auto ft : field_types) { + auto& f = lidar_scan.field(ft.name); + if (fieldDecode(lidar_scan, scan_data, next_idx, + {ft.name, ft.element_type}, px_offset)) { + logger().error( + "ERROR: scanDecodeFields:" + "Failed to decode field"); return true; } ++next_idx; @@ -784,13 +827,14 @@ bool scanDecodeFieldsSingleThread(LidarScan& lidar_scan, // TWS 20240301 TODO: determine if we can deduplicate this code (see // scanEncodeFields) bool scanDecodeFields(LidarScan& lidar_scan, const ScanData& scan_data, - const std::vector& px_offset) { - LidarScanFieldTypes field_types(lidar_scan.begin(), lidar_scan.end()); + const std::vector& px_offset, + const ouster::LidarScanFieldTypes& field_types) { size_t fields_num = field_types.size(); if (scan_data.size() != fields_num) { - std::cerr << "ERROR: lidar_scan data contains # of channels: " - << scan_data.size() << ", expected: " << fields_num - << " for OSF EUDP" << std::endl; + logger().error( + "ERROR: lidar_scan data contains # of channels: " + "{}, expected: {} for OSF EUDP", + scan_data.size(), fields_num); return true; } @@ -813,7 +857,8 @@ bool scanDecodeFields(LidarScan& lidar_scan, const ScanData& scan_data, std::vector thread_idxs{}; for (size_t i = 0; i < per_thread_num && i + start_idx < fields_num; ++i) { - thread_fields.push_back(field_types[start_idx + i]); + thread_fields.push_back({field_types[start_idx + i].name, + field_types[start_idx + i].element_type}); thread_idxs.push_back(scan_idx); scan_idx += 1; // for UINT64 can be 2 (NOT IMPLEMENTED YET) } @@ -899,24 +944,19 @@ bool decode24bitImage(Eigen::Ref> img, // Sanity checks for encoded PNG size if (width != static_cast(img.cols()) || height != static_cast(img.rows())) { - std::cout << "ERROR: img contains data of incompatible size: " << width - << "x" << height << ", expected: " << img.cols() << "x" - << img.rows() << std::endl; + print_incompatable_image_size(width, height, + static_cast(img.cols()), + static_cast(img.rows())); return true; } if (sample_depth != 8) { - std::cout << "ERROR: encoded img contains data with incompatible " - "sample_depth: " - << sample_depth << ", expected: 8" << std::endl; + print_bad_sample_depth(sample_depth, 8); return true; } if (color_type != PNG_COLOR_TYPE_RGB) { - std::cout << "ERROR: encoded img contains data with incompatible " - "color type: " - << color_type << ", expected: " << PNG_COLOR_TYPE_RGB - << std::endl; + print_bad_color_type(color_type, PNG_COLOR_TYPE_RGB); return true; } @@ -971,8 +1011,7 @@ template bool decode32bitImage(Eigen::Ref> img, const ScanChannelData& channel_buf) { if (sizeof(T) < 4) { - std::cerr << "WARNING: Attempt to decode image of bigger pixel size" - << std::endl; + print_bad_pixel_size(); } // libpng main structs png_structp png_ptr; @@ -1008,24 +1047,19 @@ bool decode32bitImage(Eigen::Ref> img, // Sanity checks for encoded PNG size if (width != static_cast(img.cols()) || height != static_cast(img.rows())) { - std::cout << "ERROR: img contains data of incompatible size: " << width - << "x" << height << ", expected: " << img.cols() << "x" - << img.rows() << std::endl; + print_incompatable_image_size(width, height, + static_cast(img.cols()), + static_cast(img.rows())); return true; } if (sample_depth != 8) { - std::cout << "ERROR: encoded img contains data with incompatible " - "sample_depth: " - << sample_depth << ", expected: 8" << std::endl; + print_bad_sample_depth(sample_depth, 8); return true; } if (color_type != PNG_COLOR_TYPE_RGB_ALPHA) { - std::cout << "ERROR: encoded img contains data with incompatible " - "color type: " - << color_type << ", expected: " << PNG_COLOR_TYPE_RGB_ALPHA - << std::endl; + print_bad_color_type(color_type, PNG_COLOR_TYPE_RGB_ALPHA); return true; } @@ -1081,8 +1115,7 @@ template bool decode64bitImage(Eigen::Ref> img, const ScanChannelData& channel_buf) { if (sizeof(T) < 8) { - std::cerr << "WARNING: Attempt to decode image of bigger pixel size" - << std::endl; + print_bad_pixel_size(); } // libpng main structs png_structp png_ptr; @@ -1118,24 +1151,21 @@ bool decode64bitImage(Eigen::Ref> img, // Sanity checks for encoded PNG size if (width != static_cast(img.cols()) || height != static_cast(img.rows())) { - std::cout << "ERROR: img contains data of incompatible size: " << width - << "x" << height << ", expected: " << img.cols() << "x" - << img.rows() << std::endl; + print_incompatable_image_size(width, height, + static_cast(img.cols()), + static_cast(img.rows())); + return true; } if (sample_depth != 16) { - std::cout << "ERROR: encoded img contains data with incompatible " - "sample_depth: " - << sample_depth << ", expected: 16" << std::endl; + print_bad_sample_depth(sample_depth, 16); return true; } if (color_type != PNG_COLOR_TYPE_RGB_ALPHA) { - std::cout << "ERROR: encoded img contains data with incompatible " - "color type: " - << color_type << ", expected: " << PNG_COLOR_TYPE_RGB_ALPHA - << std::endl; + print_bad_color_type(color_type, PNG_COLOR_TYPE_RGB_ALPHA); + return true; } @@ -1197,8 +1227,7 @@ template bool decode16bitImage(Eigen::Ref> img, const ScanChannelData& channel_buf) { if (sizeof(T) < 2) { - std::cerr << "WARNING: Attempt to decode image of bigger pixel size" - << std::endl; + print_bad_pixel_size(); } // libpng main structs png_structp png_ptr; @@ -1234,24 +1263,19 @@ bool decode16bitImage(Eigen::Ref> img, // Sanity checks for encoded PNG size if (width != static_cast(img.cols()) || height != static_cast(img.rows())) { - std::cout << "ERROR: img contains data of incompatible size: " << width - << "x" << height << ", expected: " << img.cols() << "x" - << img.rows() << std::endl; + print_incompatable_image_size(width, height, + static_cast(img.cols()), + static_cast(img.rows())); return true; } if (sample_depth != 16) { - std::cout << "ERROR: encoded img contains data with incompatible " - "sample_depth: " - << sample_depth << ", expected: 16" << std::endl; + print_bad_sample_depth(sample_depth, 16); return true; } if (color_type != PNG_COLOR_TYPE_GRAY) { - std::cout << "ERROR: encoded img contains data with incompatible " - "color type: " - << color_type << ", expected: " << PNG_COLOR_TYPE_GRAY - << std::endl; + print_bad_color_type(color_type, PNG_COLOR_TYPE_GRAY); return true; } @@ -1338,24 +1362,19 @@ bool decode8bitImage(Eigen::Ref> img, // Sanity checks for encoded PNG size if (width != static_cast(img.cols()) || height != static_cast(img.rows())) { - std::cout << "ERROR: img contains data of incompatible size: " << width - << "x" << height << ", expected: " << img.cols() << "x" - << img.rows() << std::endl; + print_incompatable_image_size(width, height, + static_cast(img.cols()), + static_cast(img.rows())); return true; } if (sample_depth != 8) { - std::cout << "ERROR: encoded img contains data with incompatible " - "sample_depth: " - << sample_depth << ", expected: 16" << std::endl; + print_bad_sample_depth(sample_depth, 8); return true; } if (color_type != PNG_COLOR_TYPE_GRAY) { - std::cout << "ERROR: encoded img contains data with incompatible " - "color type: " - << color_type << ", expected: " << PNG_COLOR_TYPE_GRAY - << std::endl; + print_bad_color_type(color_type, PNG_COLOR_TYPE_GRAY); return true; } @@ -1380,5 +1399,86 @@ template bool decode8bitImage(Eigen::Ref>, template bool decode8bitImage(Eigen::Ref>, const ScanChannelData&); +ScanChannelData encodeField(const ouster::Field& field) { + ScanChannelData buffer; + + // do not compress, flat fields "compressed" size is greater than original + if (field.shape().size() == 1) { + buffer.resize(field.bytes()); + std::memcpy(buffer.data(), field, field.bytes()); + return buffer; + } + + FieldView view = uint_view(field); + // collapse shape + if (view.shape().size() > 2) { + size_t rows = view.shape()[0]; + size_t cols = view.size() / rows; + view = view.reshape(rows, cols); + } + + bool res = true; + switch (view.tag()) { + case sensor::ChanFieldType::UINT8: + res = encode8bitImage(buffer, view); + break; + case sensor::ChanFieldType::UINT16: + res = encode16bitImage(buffer, view); + break; + case sensor::ChanFieldType::UINT32: + res = encode32bitImage(buffer, view); + break; + case sensor::ChanFieldType::UINT64: + res = encode64bitImage(buffer, view); + break; + default: + break; + } + + if (res) { + throw std::runtime_error("encodeField: could not encode field"); + } + + return buffer; +} + +void decodeField(ouster::Field& field, const ScanChannelData& buffer) { + // 1d case, uncompressed + if (field.shape().size() == 1) { + std::memcpy(field, buffer.data(), buffer.size()); + return; + } + + FieldView view = uint_view(field); + // collapse shape + if (view.shape().size() > 2) { + size_t rows = view.shape()[0]; + size_t cols = view.size() / rows; + view = view.reshape(rows, cols); + } + + bool res = true; + switch (view.tag()) { + case sensor::ChanFieldType::UINT8: + res = decode8bitImage(view, buffer); + break; + case sensor::ChanFieldType::UINT16: + res = decode16bitImage(view, buffer); + break; + case sensor::ChanFieldType::UINT32: + res = decode32bitImage(view, buffer); + break; + case sensor::ChanFieldType::UINT64: + res = decode64bitImage(view, buffer); + break; + default: + break; + } + + if (res) { + throw std::runtime_error("decodeField: could not decode field"); + } +} + } // namespace osf } // namespace ouster diff --git a/ouster_osf/src/png_tools.h b/ouster_osf/src/png_tools.h index a45896d0..7773c661 100644 --- a/ouster_osf/src/png_tools.h +++ b/ouster_osf/src/png_tools.h @@ -8,6 +8,7 @@ #include #include +#include "os_sensor/common_generated.h" #include "os_sensor/lidar_scan_stream_generated.h" #include "ouster/lidar_scan.h" @@ -22,7 +23,7 @@ using ScanData = std::vector; // FieldTypes container using LidarScanFieldTypes = - std::vector>; + std::vector>; /** * libpng only versions for Encode/Decode LidarScan to PNG buffers @@ -39,21 +40,25 @@ using LidarScanFieldTypes = * @param[in] scan_data PNG buffers to decode. * @param[in] px_offset Pixel shift per row used to reconstruct staggered range * image form. + * @param[in] fields Fields to deserialize in the correct order. * @return false (0) if operation is successful true (1) if error occured */ bool scanDecode(LidarScan& lidar_scan, const ScanData& scan_data, - const std::vector& px_offset); + const std::vector& px_offset, + const ouster::LidarScanFieldTypes& fields); #ifdef OUSTER_OSF_NO_THREADING /// Decoding eUDP LidarScan // TODO[pb]: Make decoding of just some fields from scan data?? Not now ... bool scanDecodeFieldsSingleThread(LidarScan& lidar_scan, const ScanData& scan_data, - const std::vector& px_offset); + const std::vector& px_offset, + const ouster::LidarScanFieldTypes& fields); #else /// Decoding eUDP LidarScan, multithreaded version bool scanDecodeFields(LidarScan& lidar_scan, const ScanData& scan_data, - const std::vector& px_offset); + const std::vector& px_offset, + const ouster::LidarScanFieldTypes& fields); #endif /** @@ -71,7 +76,7 @@ bool scanDecodeFields(LidarScan& lidar_scan, const ScanData& scan_data, */ bool fieldDecode( LidarScan& lidar_scan, const ScanData& scan_data, size_t scan_idx, - const std::pair field_type, + const std::pair& field_type, const std::vector& px_offset); /** @@ -243,10 +248,12 @@ bool decode64bitImage(Eigen::Ref> img, * @param[in] lidar_scan The LidarScan object to encode. * @param[in] px_offset Pixel shift per row used to * destaggered LidarScan data. + * @param[in] field_types the list of fields to encode. * @return encoded PNG buffers, empty() if error occured. */ ScanData scanEncode(const LidarScan& lidar_scan, - const std::vector& px_offset); + const std::vector& px_offset, + const LidarScanFieldTypes& field_types); #ifdef OUSTER_OSF_NO_THREADING /** @@ -256,6 +263,7 @@ ScanData scanEncode(const LidarScan& lidar_scan, * @param[in] lidar_scan A lidar scan object to encode. * @param[in] px_offset Pixel shift per row used to construct de-staggered range * image form. + * @param[in] field_types the list of fields to encode. * @return Encoded PNGs in ScanData in order of field_types. */ ScanData scanEncodeFieldsSingleThread(const LidarScan& lidar_scan, @@ -291,7 +299,7 @@ ScanData scanEncodeFields(const LidarScan& lidar_scan, */ bool fieldEncode( const LidarScan& lidar_scan, - const std::pair field_type, + const std::pair& field_type, const std::vector& px_offset, ScanData& scan_data, size_t scan_idx); /** @@ -446,5 +454,24 @@ template bool encode64bitImage(ScanChannelData& res_buf, const Eigen::Ref>& img); +/** + * Encode Field into a data buffer. + * May use png compression, depending on field dimensionality. + * + * @param[in] field field to encode + * + * @return output buffer + */ +ScanChannelData encodeField(const ouster::Field& field); + +/** + * Decode Field from a data buffer. + * May use png compression, depending on field dimensionality. + * + * @param[inout] field field to store result in + * @param[in] buffer buffer to decode + */ +void decodeField(ouster::Field& field, const ScanChannelData& buffer); + } // namespace osf } // namespace ouster diff --git a/ouster_osf/src/reader.cpp b/ouster_osf/src/reader.cpp index 11e399bf..91ba97eb 100644 --- a/ouster_osf/src/reader.cpp +++ b/ouster_osf/src/reader.cpp @@ -9,6 +9,7 @@ #include #include "fb_utils.h" +#include "ouster/impl/logging.h" #include "ouster/osf/basics.h" #include "ouster/osf/crc32.h" #include "ouster/osf/file.h" @@ -19,6 +20,8 @@ using StreamChunksMap = std::unordered_map>>; +using namespace ouster::sensor; + namespace ouster { namespace osf { @@ -387,9 +390,11 @@ ChunksRange Reader::chunks() { Reader::Reader(const std::string& file) : file_{file} { if (!file_.valid()) { - std::cerr << "ERROR: While openning OSF file. Expected valid() but " - "got file_ = " - << file_.to_string() << std::endl; + logger().error( + "ERROR: While openning OSF file. " + "Expected valid() but " + "got file_ = {}", + file_.to_string()); throw std::logic_error("provided OSF file is not a valid OSF file."); } diff --git a/ouster_osf/src/stream_lidar_scan.cpp b/ouster_osf/src/stream_lidar_scan.cpp index ef47e815..e515e566 100644 --- a/ouster_osf/src/stream_lidar_scan.cpp +++ b/ouster_osf/src/stream_lidar_scan.cpp @@ -8,26 +8,21 @@ #include #include +#include "ouster/impl/logging.h" #include "ouster/lidar_scan.h" #include "ouster/osf/basics.h" +#include "ouster/strings.h" #include "ouster/types.h" #include "png_tools.h" +using namespace ouster::sensor; +using namespace ouster::strings; + namespace ouster { namespace osf { namespace { -gen::CHAN_FIELD to_osf_enum(sensor::ChanField f) { - // TODO[pb]: When we start diverging add a better mapping. - return static_cast(f); -} - -sensor::ChanField from_osf_enum(gen::CHAN_FIELD f) { - // TODO[pb]: When we start diverging add a better mapping. - return static_cast(f); -} - gen::CHAN_FIELD_TYPE to_osf_enum(sensor::ChanFieldType f) { // TODO[pb]: When we start diverging add a better mapping. return static_cast(f); @@ -38,16 +33,28 @@ sensor::ChanFieldType from_osf_enum(gen::CHAN_FIELD_TYPE ft) { return static_cast(ft); } +gen::FIELD_CLASS to_osf_enum(ouster::FieldClass ff) { + return static_cast(ff); +} + +ouster::FieldClass from_osf_enum(gen::FIELD_CLASS ff) { + return static_cast(ff); +} + } // namespace bool poses_present(const LidarScan& ls) { - return std::find_if_not(ls.pose().begin(), ls.pose().end(), - [](const mat4d& m) { return m.isIdentity(); }) != - ls.pose().end(); + auto&& pose = ls.pose(); + ouster::mat4d mat; + for (size_t i = 0, end = pose.shape()[0]; i < end; ++i) { + std::memcpy(mat.data(), pose.subview(i).get(), sizeof(double) * 16); + if (!mat.isIdentity()) return true; + } + return false; } LidarScan slice_with_cast(const LidarScan& ls_src, - const LidarScanFieldTypes& field_types) { + const ouster::LidarScanFieldTypes& field_types) { LidarScan ls_dest{static_cast(ls_src.w), static_cast(ls_src.h), field_types.begin(), field_types.end()}; @@ -58,18 +65,17 @@ LidarScan slice_with_cast(const LidarScan& ls_src, ls_dest.timestamp() = ls_src.timestamp(); ls_dest.measurement_id() = ls_src.measurement_id(); ls_dest.status() = ls_src.status(); - ls_dest.pose() = ls_src.pose(); ls_dest.packet_timestamp() = ls_src.packet_timestamp(); + ls_dest.pose() = ls_src.pose(); // Copy fields for (const auto& ft : field_types) { - if (ls_src.field_type(ft.first)) { - ouster::impl::visit_field(ls_dest, ft.first, + if (ls_src.has_field(ft.name)) { + ouster::impl::visit_field(ls_dest, ft.name, ouster::impl::copy_and_cast(), ls_src, - ft.first); + ft.name); } else { - throw std::invalid_argument("Required field '" + - sensor::to_string(ft.first) + + throw std::invalid_argument("Required field '" + ft.name + "' is missing from scan."); } } @@ -105,27 +111,157 @@ flatbuffers::Offset> CreateVectorOfStructs( return res_off; } +flatbuffers::Offset create_osf_field( + flatbuffers::FlatBufferBuilder& fbb, const std::string& name, + const Field& f) { + ScanChannelData data = encodeField(f); + std::vector shape{f.shape().begin(), f.shape().end()}; + return gen::CreateFieldDirect(fbb, name.c_str(), to_osf_enum(f.tag()), + &shape, to_osf_enum(f.field_class()), &data, + f.bytes()); +} + +using nonstd::make_optional; +using nonstd::nullopt; +using nonstd::optional; + +namespace impl { +template +using Table = std::array, N>; +} + +template +static optional lookup(const impl::Table table, const K& k) { + auto end = table.end(); + auto res = std::find_if(table.begin(), end, [&](const std::pair& p) { + return p.first == k; + }); + + return res == end ? nullopt : make_optional(res->second); +} + +template +static optional rlookup(const impl::Table table, + const char* v) { + auto end = table.end(); + auto res = std::find_if( + table.begin(), end, [&](const std::pair& p) { + return p.second && std::strcmp(p.second, v) == 0; + }); + + return res == end ? nullopt : make_optional(res->first); +} + +// mapping of channel name to osf ChanField +static impl::Table + chanfield_strings{{ + {ouster::osf::gen::CHAN_FIELD::RANGE, ChanField::RANGE}, + {ouster::osf::gen::CHAN_FIELD::RANGE2, ChanField::RANGE2}, + {ouster::osf::gen::CHAN_FIELD::SIGNAL, ChanField::SIGNAL}, + {ouster::osf::gen::CHAN_FIELD::SIGNAL2, ChanField::SIGNAL2}, + {ouster::osf::gen::CHAN_FIELD::REFLECTIVITY, ChanField::REFLECTIVITY}, + {ouster::osf::gen::CHAN_FIELD::REFLECTIVITY2, ChanField::REFLECTIVITY2}, + {ouster::osf::gen::CHAN_FIELD::NEAR_IR, ChanField::NEAR_IR}, + {ouster::osf::gen::CHAN_FIELD::FLAGS, ChanField::FLAGS}, + {ouster::osf::gen::CHAN_FIELD::FLAGS2, ChanField::FLAGS2}, + {ouster::osf::gen::CHAN_FIELD::RAW_HEADERS, ChanField::RAW_HEADERS}, + {ouster::osf::gen::CHAN_FIELD::CUSTOM0, "CUSTOM0"}, + {ouster::osf::gen::CHAN_FIELD::CUSTOM1, "CUSTOM1"}, + {ouster::osf::gen::CHAN_FIELD::CUSTOM2, "CUSTOM2"}, + {ouster::osf::gen::CHAN_FIELD::CUSTOM3, "CUSTOM3"}, + {ouster::osf::gen::CHAN_FIELD::CUSTOM4, "CUSTOM4"}, + {ouster::osf::gen::CHAN_FIELD::CUSTOM5, "CUSTOM5"}, + {ouster::osf::gen::CHAN_FIELD::CUSTOM6, "CUSTOM6"}, + {ouster::osf::gen::CHAN_FIELD::CUSTOM7, "CUSTOM7"}, + {ouster::osf::gen::CHAN_FIELD::CUSTOM8, "CUSTOM8"}, + {ouster::osf::gen::CHAN_FIELD::CUSTOM9, "CUSTOM9"}, + {ouster::osf::gen::CHAN_FIELD::RAW32_WORD1, ChanField::RAW32_WORD1}, + {ouster::osf::gen::CHAN_FIELD::RAW32_WORD2, ChanField::RAW32_WORD2}, + {ouster::osf::gen::CHAN_FIELD::RAW32_WORD3, ChanField::RAW32_WORD3}, + {ouster::osf::gen::CHAN_FIELD::RAW32_WORD4, ChanField::RAW32_WORD4}, + {ouster::osf::gen::CHAN_FIELD::RAW32_WORD5, ChanField::RAW32_WORD5}, + {ouster::osf::gen::CHAN_FIELD::RAW32_WORD6, ChanField::RAW32_WORD6}, + {ouster::osf::gen::CHAN_FIELD::RAW32_WORD7, ChanField::RAW32_WORD7}, + {ouster::osf::gen::CHAN_FIELD::RAW32_WORD8, ChanField::RAW32_WORD8}, + {ouster::osf::gen::CHAN_FIELD::RAW32_WORD9, ChanField::RAW32_WORD9}, + }}; + +nonstd::optional to_osf_enum(const std::string& f) { + // TODO[pb]: When we start diverging add a better mapping. + return rlookup(chanfield_strings, f.c_str()); +} + +std::string from_osf_enum(gen::CHAN_FIELD f) { + // TODO[pb]: When we start diverging add a better mapping. + return lookup(chanfield_strings, f).value(); +} + flatbuffers::Offset create_lidar_scan_msg( flatbuffers::FlatBufferBuilder& fbb, const LidarScan& lidar_scan, const ouster::sensor::sensor_info& info, - const LidarScanFieldTypes meta_field_types) { - auto ls = lidar_scan; - if (!meta_field_types.empty()) { - // Make a reduced field LidarScan (erroring if we are missing anything) - ls = slice_with_cast(lidar_scan, meta_field_types); + const ouster::LidarScanFieldTypes meta_field_types) { + const auto& ls = lidar_scan; + + // Prepare field_types for LidarScanMsg + std::vector> custom_fields; + std::vector> + standard_fields_to_sort; + for (const auto& f : ls.fields()) { + // if we have meta_field_types ignore fields not in it + if (meta_field_types.size()) { + bool found = false; + for (const auto& mft : meta_field_types) { + if (mft.name == f.first) { + found = true; + break; + } + } + if (!found) { + continue; + } + } + // if the field is custom, add it as custom + auto enum_value = to_osf_enum(f.first); // optional + if (!enum_value) { + custom_fields.push_back(create_osf_field(fbb, f.first, f.second)); + } else { + // otherwise add it to field types and then scan encode those + standard_fields_to_sort.emplace_back(enum_value.value(), f.first); + } + } + + // because older OSF readers are broken, we need to sort the standard fields + // by value... + std::sort(standard_fields_to_sort.begin(), standard_fields_to_sort.end()); + + // now actually build our arrays from the sorted one + std::vector field_types; + std::vector> + standard_fields; + for (const auto& f : standard_fields_to_sort) { + const auto& field = ls.field(f.second); + field_types.push_back( + ouster::osf::gen::ChannelField(f.first, to_osf_enum(field.tag()))); + standard_fields.push_back({f.second, field.tag()}); + } + + // error if we are missing a required field + for (const auto& mft : meta_field_types) { + if (ls.fields().find(mft.name) == ls.fields().end()) { + throw std::invalid_argument("Required field '" + mft.name + + "' is missing from scan."); + } } + // Encode LidarScan to PNG buffers - ScanData scan_data = scanEncode(ls, info.format.pixel_shift_by_row); + ScanData scan_data = + scanEncode(ls, info.format.pixel_shift_by_row, standard_fields); // Prepare PNG encoded channels for LidarScanMsg.channels vector std::vector> channels; for (const auto& channel_data : scan_data) { channels.emplace_back(gen::CreateChannelDataDirect(fbb, &channel_data)); } - // Prepare field_types for LidarScanMsg - std::vector field_types; - for (const auto& f : ls) { - field_types.emplace_back(to_osf_enum(f.first), to_osf_enum(f.second)); - } + auto channels_off = fbb.CreateVector<::flatbuffers::Offset>(channels); auto field_types_off = osf::CreateVectorOfStructs( @@ -137,14 +273,23 @@ flatbuffers::Offset create_lidar_scan_msg( auto status_off = fbb.CreateVector(ls.status().data(), ls.w); flatbuffers::Offset> pose_off = 0; if (poses_present(ls)) { - pose_off = fbb.CreateVector(ls.pose().data()->data(), - ls.pose().size() * 16); + pose_off = fbb.CreateVector(ls.pose().get(), + ls.pose().bytes() / sizeof(double)); } auto packet_timestamp_id_off = fbb.CreateVector( ls.packet_timestamp().data(), ls.packet_timestamp().size()); - return gen::CreateLidarScanMsg( - fbb, channels_off, field_types_off, timestamp_off, measurement_id_off, - status_off, ls.frame_id, pose_off, packet_timestamp_id_off); + + flatbuffers::Offset>> + custom_fields_off = 0; + if (custom_fields.size()) { + custom_fields_off = + fbb.CreateVector>(custom_fields); + } + + return gen::CreateLidarScanMsg(fbb, channels_off, field_types_off, + timestamp_off, measurement_id_off, + status_off, ls.frame_id, pose_off, + packet_timestamp_id_off, custom_fields_off); } std::unique_ptr restore_lidar_scan( @@ -157,13 +302,15 @@ std::unique_ptr restore_lidar_scan( uint32_t height = info.format.pixels_per_column; // read field_types - LidarScanFieldTypes field_types; + ouster::LidarScanFieldTypes field_types; if (ls_msg->field_types() && ls_msg->field_types()->size()) { std::transform( ls_msg->field_types()->begin(), ls_msg->field_types()->end(), std::back_inserter(field_types), [](const gen::ChannelField* p) { - return std::make_pair(from_osf_enum(p->chan_field()), - from_osf_enum(p->chan_field_type())); + return FieldType{from_osf_enum(p->chan_field()), + from_osf_enum(p->chan_field_type()), + {}, + FieldClass::PIXEL_FIELD}; }); } @@ -181,9 +328,11 @@ std::unique_ptr restore_lidar_scan( ls->timestamp()[i] = msg_ts_vec->Get(i); } } else if (msg_ts_vec->size() != 0) { - std::cout << "ERROR: LidarScanMsg has header_timestamp of length: " - << msg_ts_vec->size() << ", expected: " << ls->w - << std::endl; + logger().error( + "ERROR: LidarScanMsg has " + "header_timestamp of length: " + "{}, expected: {}", + msg_ts_vec->size(), ls->w); return nullptr; } } @@ -196,9 +345,11 @@ std::unique_ptr restore_lidar_scan( ls->measurement_id()[i] = msg_mid_vec->Get(i); } } else if (msg_mid_vec->size() != 0) { - std::cout - << "ERROR: LidarScanMsg has header_measurement_id of length: " - << msg_mid_vec->size() << ", expected: " << ls->w << std::endl; + logger().error( + "ERROR: LidarScanMsg has " + "header_measurement_id of length: " + "{}, expected: {}", + msg_mid_vec->size(), ls->w); return nullptr; } } @@ -211,9 +362,11 @@ std::unique_ptr restore_lidar_scan( ls->status()[i] = msg_status_vec->Get(i); } } else if (msg_status_vec->size() != 0) { - std::cout << "ERROR: LidarScanMsg has header_status of length: " - << msg_status_vec->size() << ", expected: " << ls->w - << std::endl; + logger().error( + "ERROR: LidarScanMsg has " + "header_status of length: " + "{}, expected: {}", + msg_status_vec->size(), ls->w); return nullptr; } } @@ -221,18 +374,15 @@ std::unique_ptr restore_lidar_scan( // Set poses per column auto pose_vec = ls_msg->pose(); if (pose_vec) { - if (static_cast(ls->pose().size() * 16) == pose_vec->size()) { - for (uint32_t i = 0; i < static_cast(ls->pose().size()); - ++i) { - for (uint32_t el = 0; el < 16; ++el) { - *(ls->pose()[i].data() + el) = pose_vec->Get(i * 16 + el); - } - } + auto& pose = ls->pose(); + if (static_cast(pose.size()) == pose_vec->size()) { + std::memcpy(pose.get(), pose_vec->Data(), pose.bytes()); } else if (pose_vec->size() != 0) { - std::cout << "ERROR: LidarScanMsg has pose of length: " - << pose_vec->size() - << ", expected: " << (ls->pose().size() * 16) - << std::endl; + logger().error( + "ERROR: LidarScanMsg has " + "pose of length: " + "{}, expected: {}", + pose_vec->size(), pose.size()); return nullptr; } } @@ -246,10 +396,11 @@ std::unique_ptr restore_lidar_scan( ls->packet_timestamp()[i] = packet_ts_vec->Get(i); } } else if (packet_ts_vec->size() != 0) { - std::cout << "ERROR: LidarScanMsg has packet_timestamp of length: " - << packet_ts_vec->size() - << ", expected: " << ls->packet_timestamp().size() - << std::endl; + logger().error( + "ERROR: LidarScanMsg has " + "packet_timestamp of length: " + "{}, expected: {}", + packet_ts_vec->size(), ls->packet_timestamp().size()); return nullptr; } } @@ -257,8 +408,9 @@ std::unique_ptr restore_lidar_scan( // Fill Scan Data with scan channels auto msg_scan_vec = ls_msg->channels(); if (!msg_scan_vec || !msg_scan_vec->size()) { - std::cout - << "ERROR: lidar_scan msg doesn't have scan field or it's empty.\n"; + logger().error( + "ERROR: lidar_scan msg doesn't " + "have scan field or it's empty."); return nullptr; } ScanData scan_data; @@ -268,31 +420,53 @@ std::unique_ptr restore_lidar_scan( } // Decode PNGs data to LidarScan - if (scanDecode(*ls, scan_data, info.format.pixel_shift_by_row)) { + if (scanDecode(*ls, scan_data, info.format.pixel_shift_by_row, + field_types)) { return nullptr; } + auto msg_custom_fields = ls_msg->custom_fields(); + if (msg_custom_fields && msg_custom_fields->size()) { + for (uint32_t i = 0; i < msg_custom_fields->size(); ++i) { + auto custom_field = msg_custom_fields->Get(i); + + std::string name{custom_field->name()->c_str()}; + ChanFieldType tag = from_osf_enum(custom_field->tag()); + std::vector shape{custom_field->shape()->begin(), + custom_field->shape()->end()}; + auto desc = FieldDescriptor::array(tag, shape); + ouster::FieldClass field_class = + from_osf_enum(custom_field->field_class()); + auto& field = ls->add_field(name, desc, field_class); + + std::vector encoded{custom_field->data()->begin(), + custom_field->data()->end()}; + decodeField(field, encoded); + } + } + return ls; } -LidarScanStreamMeta::LidarScanStreamMeta(const uint32_t sensor_meta_id, - const LidarScanFieldTypes field_types) +LidarScanStreamMeta::LidarScanStreamMeta( + const uint32_t sensor_meta_id, + const ouster::LidarScanFieldTypes field_types) : sensor_meta_id_{sensor_meta_id}, field_types_{field_types.begin(), field_types.end()} {} uint32_t LidarScanStreamMeta::sensor_meta_id() const { return sensor_meta_id_; } -const LidarScanFieldTypes& LidarScanStreamMeta::field_types() const { - return field_types_; -} - std::vector LidarScanStreamMeta::buffer() const { flatbuffers::FlatBufferBuilder fbb = flatbuffers::FlatBufferBuilder(512); // Make and store field_types with details for LidarScanStream std::vector field_types; for (const auto& ft : field_types_) { - field_types.emplace_back(to_osf_enum(ft.first), to_osf_enum(ft.second)); + auto field_enum = to_osf_enum(ft.name); + if (field_enum) { + field_types.emplace_back(field_enum.value(), + to_osf_enum(ft.element_type)); + } } auto field_types_off = osf::CreateVectorOfStructs( @@ -314,13 +488,15 @@ std::unique_ptr LidarScanStreamMeta::from_buffer( auto sensor_meta_id = lidar_scan_stream->sensor_id(); auto field_types_vec = lidar_scan_stream->field_types(); - LidarScanFieldTypes field_types; + ouster::LidarScanFieldTypes field_types; if (field_types_vec && field_types_vec->size()) { std::transform( field_types_vec->begin(), field_types_vec->end(), std::back_inserter(field_types), [](const gen::ChannelField* p) { - return std::make_pair(from_osf_enum(p->chan_field()), - from_osf_enum(p->chan_field_type())); + return ouster::FieldType{from_osf_enum(p->chan_field()), + from_osf_enum(p->chan_field_type()), + {}, + FieldClass::PIXEL_FIELD}; }); } @@ -335,8 +511,7 @@ std::string LidarScanStreamMeta::repr() const { bool first = true; for (const auto& f : field_types_) { if (!first) ss << ", "; - ss << sensor::to_string(f.first) << ":" - << ouster::sensor::to_string(f.second); + ss << f.name << ":" << ouster::sensor::to_string(f.element_type); first = false; } ss << "}"; @@ -347,10 +522,11 @@ std::string LidarScanStreamMeta::repr() const { LidarScanStream::LidarScanStream(Token /*key*/, Writer& writer, const uint32_t sensor_meta_id, - const LidarScanFieldTypes& field_types) + const ouster::LidarScanFieldTypes& field_types) : writer_{writer}, meta_(sensor_meta_id, field_types), - sensor_meta_id_(sensor_meta_id) { + sensor_meta_id_(sensor_meta_id), + field_types_(field_types) { // Note key is ignored and just used to gatekeep. // Check sensor and get sensor_info auto sensor_meta_entry = writer.get_metadata(sensor_meta_id_); @@ -375,8 +551,8 @@ void LidarScanStream::save(const ouster::osf::ts_t ts, std::vector LidarScanStream::make_msg(const LidarScan& lidar_scan) { flatbuffers::FlatBufferBuilder fbb = flatbuffers::FlatBufferBuilder(32768); - auto ls_msg_offset = create_lidar_scan_msg(fbb, lidar_scan, sensor_info_, - meta_.field_types()); + auto ls_msg_offset = + create_lidar_scan_msg(fbb, lidar_scan, sensor_info_, field_types_); fbb.FinishSizePrefixed(ls_msg_offset); const uint8_t* buf = fbb.GetBufferPointer(); const size_t size = fbb.GetSize(); diff --git a/ouster_osf/src/writer.cpp b/ouster_osf/src/writer.cpp index 4f6e5934..c523efd1 100644 --- a/ouster_osf/src/writer.cpp +++ b/ouster_osf/src/writer.cpp @@ -8,11 +8,14 @@ #include #include "fb_utils.h" +#include "ouster/impl/logging.h" #include "ouster/osf/basics.h" #include "ouster/osf/crc32.h" #include "ouster/osf/layout_streaming.h" #include "ouster/osf/stream_lidar_scan.h" +using namespace ouster::sensor; + constexpr size_t MAX_CHUNK_SIZE = 500 * 1024 * 1024; namespace ouster { @@ -42,18 +45,21 @@ Writer::Writer(const std::string& filename, uint32_t chunk_size) Writer::Writer(const std::string& filename, const ouster::sensor::sensor_info& info, - const LidarScanFieldTypes& field_types, uint32_t chunk_size) + const std::vector& desired_fields, + uint32_t chunk_size) : Writer(filename, std::vector{info}, - field_types, chunk_size) {} + desired_fields, chunk_size) {} Writer::Writer(const std::string& filename, const std::vector& info, - const LidarScanFieldTypes& field_types, uint32_t chunk_size) + const std::vector& desired_fields, + uint32_t chunk_size) : Writer(filename, chunk_size) { sensor_info_ = info; for (uint32_t i = 0; i < info.size(); i++) { lidar_meta_id_[i] = add_metadata(ouster::osf::LidarSensor(info[i])); - field_types_.push_back(field_types); + field_types_.push_back({}); + desired_fields_.push_back(desired_fields); } } @@ -68,10 +74,11 @@ const ouster::sensor::sensor_info Writer::sensor_info(int stream_index) const { uint32_t Writer::sensor_info_count() const { return sensor_info_.size(); } uint32_t Writer::add_sensor(const ouster::sensor::sensor_info& info, - const LidarScanFieldTypes& field_types) { + const std::vector& desired_fields) { lidar_meta_id_[lidar_meta_id_.size()] = add_metadata(ouster::osf::LidarSensor(info)); - field_types_.push_back(field_types); + field_types_.push_back({}); + desired_fields_.push_back(desired_fields); sensor_info_.push_back(info); return lidar_meta_id_.size() - 1; } @@ -81,14 +88,67 @@ void Writer::_save(uint32_t stream_index, const LidarScan& scan, if (stream_index < lidar_meta_id_.size()) { auto item = lidar_streams_.find(stream_index); if (item == lidar_streams_.end()) { - const auto& fields = field_types_[stream_index].size() - ? field_types_[stream_index] - : get_field_types(scan); + // build list of field types from provided or the first scan + std::vector field_types; + if (desired_fields_[stream_index].size() == 0) { + field_types = scan.field_types(); + } else { + for (const auto& desired : desired_fields_[stream_index]) { + if (!scan.has_field(desired)) { + throw std::invalid_argument("Required field '" + + desired + + "' is missing from scan."); + } + field_types.push_back(scan.field_type(desired)); + } + } + field_types_[stream_index] = field_types; + lidar_streams_[stream_index] = std::make_unique( LidarScanStream::Token(), *this, - lidar_meta_id_[stream_index], fields); + lidar_meta_id_[stream_index], field_types); } + + // enforce that this scan meets our expected field types and that + // dimensions didnt change when required to be the same + for (const auto& ft : field_types_[stream_index]) { + const auto& field = scan.fields().find(ft.name); + if (field == scan.fields().end()) { + throw std::invalid_argument("Required field '" + ft.name + + "' is missing from scan."); + } + + if (ft.element_type != field->second.tag()) { + throw std::invalid_argument( + "Field '" + ft.name + "' has changed from '" + + sensor::to_string(ft.element_type) + "' to '" + + sensor::to_string(field->second.tag()) + + "'. Field types cannot change between saved scans from the " + "same sensor."); + } + + if (ft.field_class != field->second.field_class()) { + throw std::invalid_argument( + "Field '" + ft.name + "' has changed from '" + + to_string(ft.field_class) + "' to '" + + to_string(field->second.field_class()) + + "'. Field class cannot change between saved scans from the " + "same sensor."); + } + + // Dimensions should not change for pixel fields between scans + if (ft.field_class == FieldClass::PIXEL_FIELD) { + if (ft != scan.field_type(ft.name)) { + throw std::invalid_argument( + "Field '" + ft.name + + "' dimensions have changed. Pixel field dimensions " + "cannot change for between saved scans from the same " + "sensor."); + } + } + } + lidar_streams_[stream_index]->save(time, scan); } else { throw std::logic_error("ERROR: Bad Stream ID"); @@ -147,7 +207,7 @@ uint64_t Writer::append(const uint8_t* buf, const uint64_t size) { throw std::logic_error("ERROR: Hmm, Writer is finished."); } if (size == 0) { - std::cout << "nothing to append!!!\n"; + logger().info("Writer::append has nothing to append"); return 0; } uint64_t saved_bytes = buffer_to_file(buf, size, file_name_, true); @@ -247,15 +307,15 @@ void Writer::close() { header_size_) { finished_ = true; } else { - std::cerr << "ERROR: Can't finish OSF file!" - "Recorded header of " - "different sizes ..." - << std::endl; + logger().error( + "ERROR: Can't finish OSF file!" + "Recorded header of " + "different sizes ..."); } } else { - std::cerr << "ERROR: Oh, why we are here and " - "didn't finish correctly?" - << std::endl; + logger().error( + "ERROR: Oh, why we are here and " + "didn't finish correctly?"); } } @@ -268,9 +328,9 @@ Writer::~Writer() { close(); } void ChunkBuilder::save_message(const uint32_t stream_id, const ts_t ts, const std::vector& msg_buf) { if (finished_) { - std::cerr - << "ERROR: ChunkBuilder is finished and can't accept new messages!" - << std::endl; + logger().error( + "ERROR: ChunkBuilder is finished " + "and can't accept new messages!"); return; } diff --git a/ouster_osf/tests/CMakeLists.txt b/ouster_osf/tests/CMakeLists.txt index 156e2304..adfcef48 100644 --- a/ouster_osf/tests/CMakeLists.txt +++ b/ouster_osf/tests/CMakeLists.txt @@ -13,7 +13,6 @@ set(OSF_TESTS_SOURCES png_tools_test.cpp file_ops_test.cpp reader_test.cpp operations_test.cpp - pcap_source_test.cpp basics_test.cpp meta_streaming_info_test.cpp ) diff --git a/ouster_osf/tests/common.h b/ouster_osf/tests/common.h index 78546c12..24ba886a 100644 --- a/ouster_osf/tests/common.h +++ b/ouster_osf/tests/common.h @@ -98,35 +98,59 @@ std::array normal_arr(const double& m, const double& s) { return arr; } -// set field to random values, with mask_bits specifienging the number of +template +void set_random(Eigen::Ref> field_dest, size_t mask_bits = 0) { + field_dest = field_dest.unaryExpr([=](T) { + double sr = static_cast(std::rand()) / RAND_MAX; + return static_cast( + sr * static_cast(std::numeric_limits::max())); + }); + if (mask_bits && sizeof(T) * 8 > mask_bits) { + field_dest = field_dest.unaryExpr( + [=](T a) { return static_cast(a & ((1LL << mask_bits) - 1)); }); + } +} + +template +void set_random(Eigen::Ref> field_dest, size_t /*mask_bits*/ = 0) { + field_dest = field_dest.unaryExpr([=](float) { + double sr = static_cast(std::rand()) / RAND_MAX; + return static_cast( + sr * static_cast(std::numeric_limits::max())); + }); + // mask bits arent supported for floats +} + +template +void set_random(Eigen::Ref> field_dest, + size_t /*mask_bits*/ = 0) { + field_dest = field_dest.unaryExpr([=](double) { + double sr = static_cast(std::rand()) / RAND_MAX; + return static_cast( + sr * static_cast(std::numeric_limits::max())); + }); + // mask bits arent supported for doubles +} + +// set field to random values, with mask_bits specifying the number of // bits to mask struct set_to_random { template void operator()(Eigen::Ref> field_dest, size_t mask_bits = 0) { - field_dest = field_dest.unaryExpr([=](T) { - double sr = static_cast(std::rand()) / RAND_MAX; - return static_cast( - sr * static_cast(std::numeric_limits::max())); - }); - if (mask_bits && sizeof(T) * 8 > mask_bits) { - field_dest = field_dest.unaryExpr([=](T a) { - return static_cast(a & ((1LL << mask_bits) - 1)); - }); - } + set_random(field_dest, mask_bits); } }; inline void random_lidar_scan_data(LidarScan& ls) { - using sensor::ChanField; using sensor::ChanFieldType; - for (auto f : ls) { - if (f.first == sensor::ChanField::RANGE || - f.first == sensor::ChanField::RANGE2) { + for (auto f : ls.field_types()) { + if (f.name == sensor::ChanField::RANGE || + f.name == sensor::ChanField::RANGE2) { // Closer to reality that RANGE is just 20bits and not all 32 - ouster::impl::visit_field(ls, f.first, set_to_random(), 20); + ouster::impl::visit_field(ls, f.name, set_to_random(), 20); } else { - ouster::impl::visit_field(ls, f.first, set_to_random()); + ouster::impl::visit_field(ls, f.name, set_to_random()); } } @@ -137,7 +161,7 @@ inline void random_lidar_scan_data(LidarScan& ls) { const int64_t t_start_p = 100000000000; const int64_t dt = 100 * 1000 / (ls.w - 1); const int64_t dt_p = 100 * 1000 / (ls.packet_timestamp().size() - 1); - for (ptrdiff_t i = 0; i < ls.w; ++i) { + for (size_t i = 0; i < ls.w; ++i) { if (i == 0) ls.timestamp()[i] = t_start; else @@ -146,7 +170,9 @@ inline void random_lidar_scan_data(LidarScan& ls) { (std::numeric_limits::max() / ls.w) * i); ls.measurement_id()[i] = static_cast( (std::numeric_limits::max() / ls.w) * i); - ls.pose()[i] = ouster::mat4d::Random(); + + Eigen::Ref> pose = ls.pose().subview(i); + pose = ouster::mat4d::Random(); const int32_t pi = i / columns_per_packet; if (pi == 0) @@ -165,9 +191,9 @@ inline LidarScan get_random_lidar_scan( return ls; } -inline LidarScan get_random_lidar_scan(const size_t w = 1024, - const size_t h = 64, - LidarScanFieldTypes field_types = {}) { +inline LidarScan get_random_lidar_scan( + const size_t w = 1024, const size_t h = 64, + ouster::LidarScanFieldTypes field_types = {}) { LidarScan ls{w, h, field_types.begin(), field_types.end()}; random_lidar_scan_data(ls); return ls; @@ -179,5 +205,21 @@ inline LidarScan get_random_lidar_scan(const sensor::sensor_info& si) { si.format.udp_profile_lidar); } +template +void randomize_field(ouster::Field& field, GEN& gen, DISTRIBUTION& d) { + ArrayView1 flat_view = field.reshape(field.size()); + for (T& v : flat_view) { + v = d(gen); + } +} + +template +ouster::Field randomized_field(GEN& gen, DISTRIBUTION& d, + std::vector shape) { + auto field = Field{FieldDescriptor::array(shape)}; + randomize_field(field, gen, d); + return field; +} + } // namespace osf } // namespace ouster diff --git a/ouster_osf/tests/operations_test.cpp b/ouster_osf/tests/operations_test.cpp index c17846c0..8e1a713c 100644 --- a/ouster_osf/tests/operations_test.cpp +++ b/ouster_osf/tests/operations_test.cpp @@ -203,11 +203,10 @@ bool _parse_json(const std::string& json, Json::Value& root) { ouster::sensor::sensor_info _gen_new_metadata(int start_number) { ouster::sensor::sensor_info new_metadata; - new_metadata.name = "Foobar"; new_metadata.sn = "DEADBEEF"; new_metadata.fw_rev = "sqrt(-1) friends"; - new_metadata.mode = ouster::sensor::MODE_512x10; - new_metadata.prod_line = "LEEROY JENKINS"; + new_metadata.config.lidar_mode = ouster::sensor::MODE_512x10; + new_metadata.prod_line = "OS-1-128"; new_metadata.format.pixels_per_column = 5; new_metadata.format.columns_per_packet = 2 + start_number; @@ -231,8 +230,8 @@ ouster::sensor::sensor_info _gen_new_metadata(int start_number) { new_metadata.lidar_origin_to_beam_origin_mm = 22 + start_number; new_metadata.init_id = 23 + start_number; - new_metadata.udp_port_lidar = 24 + start_number; - new_metadata.udp_port_imu = 25 + start_number; + new_metadata.config.udp_port_lidar = 24 + start_number; + new_metadata.config.udp_port_imu = 25 + start_number; new_metadata.build_date = "Made in SAN FRANCISCO"; new_metadata.image_rev = "IDK, ask someone else"; @@ -302,7 +301,7 @@ TEST_F(OperationsTest, MetadataRewriteTestSimple) { EXPECT_NE(test_root, output_root); Json::Value new_root{}; - EXPECT_TRUE(_parse_json(new_metadata.updated_metadata_string(), new_root)); + EXPECT_TRUE(_parse_json(new_metadata.to_json_string(), new_root)); EXPECT_EQ(new_root, output_root["metadata"]["entries"][0]["buffer"]["sensor_info"]); @@ -333,9 +332,9 @@ TEST_F(OperationsTest, MetadataRewriteTestMulti) { EXPECT_NE(test_root, output_root); Json::Value new_root{}; - EXPECT_TRUE(_parse_json(new_metadata.updated_metadata_string(), new_root)); + EXPECT_TRUE(_parse_json(new_metadata.to_json_string(), new_root)); Json::Value new_root2{}; - auto temp_string = new_metadata2.updated_metadata_string(); + auto temp_string = new_metadata2.to_json_string(); EXPECT_TRUE(_parse_json(temp_string, new_root2)); EXPECT_EQ(new_root, @@ -375,7 +374,7 @@ TEST_F(OperationsTest, MetadataRewriteTestPreExisting) { EXPECT_NE(test_root, output_root); Json::Value new_root{}; - EXPECT_TRUE(_parse_json(new_metadata.updated_metadata_string(), new_root)); + EXPECT_TRUE(_parse_json(new_metadata.to_json_string(), new_root)); EXPECT_EQ(output_root["metadata"]["entries"][0]["buffer"], "LidarScanStreamMeta: sensor_id = 12345678, field_types = {}"); diff --git a/ouster_osf/tests/pcap_source_test.cpp b/ouster_osf/tests/pcap_source_test.cpp deleted file mode 100644 index f869beb4..00000000 --- a/ouster_osf/tests/pcap_source_test.cpp +++ /dev/null @@ -1,45 +0,0 @@ -/** - * Copyright(c) 2021, Ouster, Inc. - * All rights reserved. - */ - -#include "ouster/osf/pcap_source.h" - -#include - -#include "osf_test.h" -#include "ouster/osf/pcap_source.h" -#include "ouster/types.h" - -namespace ouster { -namespace osf { -namespace { - -class OsfPcapSourceTest : public OsfTestWithData {}; - -// TODO[pb]: Remove this test and PcapRawSource since it's not matching of what -// we have in the Python -TEST_F(OsfPcapSourceTest, ReadLidarScansAndImus) { - std::string pcap_file = path_concat( - test_data_dir(), "pcaps/OS-1-128_v2.3.0_1024x10_lb_n3.pcap"); - std::string meta_file = - path_concat(test_data_dir(), "pcaps/OS-1-128_v2.3.0_1024x10.json"); - - PcapRawSource pcap_source{pcap_file}; - - auto info = sensor::metadata_from_json(meta_file); - - int ls_cnt = 0; - - pcap_source.addLidarDataHandler( - 7502, info, [&ls_cnt](const osf::ts_t, const LidarScan&) { ls_cnt++; }); - - pcap_source.runAll(); - - EXPECT_EQ(2, ls_cnt); -} - -} // namespace - -} // namespace osf -} // namespace ouster diff --git a/ouster_osf/tests/png_tools_test.cpp b/ouster_osf/tests/png_tools_test.cpp index 30b9b442..dd1ec45d 100644 --- a/ouster_osf/tests/png_tools_test.cpp +++ b/ouster_osf/tests/png_tools_test.cpp @@ -7,6 +7,8 @@ #include #include +#include +#include #include #include @@ -17,6 +19,7 @@ #include "common.h" #include "osf_test.h" +#include "ouster/impl/logging.h" #include "ouster/lidar_scan.h" #include "ouster/osf/basics.h" #include "ouster/types.h" @@ -35,8 +38,8 @@ class OsfPngToolsTest : public OsfTestWithDataAndFiles {}; using ouster::sensor::lidar_mode; using ouster::sensor::sensor_info; -size_t field_size(LidarScan& ls, sensor::ChanField f) { - switch (ls.field_type(f)) { +size_t field_size(LidarScan& ls, const std::string& f) { + switch (ls.field_type(f).element_type) { case sensor::ChanFieldType::UINT8: return ls.field(f).size(); break; @@ -66,8 +69,8 @@ TEST_F(OsfPngToolsTest, MakesLidarScan) { EXPECT_EQ(ls.w, si.format.columns_per_frame); EXPECT_EQ(ls.h, si.format.pixels_per_column); - for (const auto& f : ls) { - EXPECT_EQ(field_size(ls, f.first), n); + for (const auto& f : ls.field_types()) { + EXPECT_EQ(field_size(ls, f.name), n); } EXPECT_EQ(ls.status().size(), si.format.columns_per_frame); } @@ -292,8 +295,12 @@ TEST_F(OsfPngToolsTest, InternalsTest) { bool error_caught = false; std::stringstream output_stream; - std::streambuf* old_output_stream = std::cout.rdbuf(); - std::cout.rdbuf(output_stream.rdbuf()); + auto ostream_sink = std::make_shared< + spdlog::sinks::ostream_sink>( + output_stream); + ouster::sensor::impl::Logger::instance().configure_generic_sink( + ostream_sink, "info"); + if (setjmp(png_jmpbuf(foo))) { error_caught = true; } else { @@ -301,10 +308,14 @@ TEST_F(OsfPngToolsTest, InternalsTest) { foo, "Also Checkout Porcupine Tree - Arriving Somewhere But Not Here"); } - std::cout.rdbuf(old_output_stream); + + std::string output_error = output_stream.str(); + auto error_loc = output_error.find("[error]"); + EXPECT_NE(error_loc, std::string::npos); + output_error = output_error.substr(error_loc); EXPECT_TRUE(error_caught); - EXPECT_EQ(output_stream.str(), - "ERROR libpng osf: Also Checkout Porcupine Tree" + EXPECT_EQ(output_error, + "[error] ERROR libpng osf: Also Checkout Porcupine Tree" " - Arriving Somewhere But Not Here\n"); } @@ -315,12 +326,16 @@ TEST_F(OsfPngToolsTest, scanDecodeFields) { int w = 32; int h = 32; auto scan = ouster::LidarScan(w, h); - LidarScanFieldTypes field_types(scan.begin(), scan.end()); + auto field_types = scan.field_types(); + std::vector> fields; + for (const auto& field : field_types) { + fields.push_back({field.name, field.element_type}); + } std::vector shift_by_row; EXPECT_THROW( { try { - scanEncodeFields(scan, shift_by_row, field_types); + scanEncodeFields(scan, shift_by_row, fields); } catch (const std::invalid_argument& e) { ASSERT_STREQ(e.what(), "image height does not match shifts size"); @@ -331,6 +346,40 @@ TEST_F(OsfPngToolsTest, scanDecodeFields) { } #endif +TEST(OsfFieldEncodeTest, field_encode_decode_test) { + auto test_field_encoding = [](const ouster::Field& f) { + ScanChannelData compressed; + EXPECT_NO_THROW({ compressed = encodeField(f); }); + Field decoded(f.desc()); + EXPECT_NO_THROW({ decodeField(decoded, compressed); }); + EXPECT_EQ(f, decoded); + }; + + std::random_device rd; + std::mt19937 gen{rd()}; + + std::normal_distribution nd_f{100.f, 10.f}; + test_field_encoding(randomized_field(gen, nd_f, {128, 1024, 3})); + test_field_encoding(randomized_field(gen, nd_f, {128, 1024})); + test_field_encoding(randomized_field(gen, nd_f, {4096})); + + std::normal_distribution nd_d{0.0, 1000.0}; + test_field_encoding(randomized_field(gen, nd_d, {128, 1024, 3})); + test_field_encoding(randomized_field(gen, nd_d, {128, 1024})); + test_field_encoding(randomized_field(gen, nd_d, {4096})); + + std::uniform_int_distribution ud_u32{0, 4096}; + test_field_encoding( + randomized_field(gen, ud_u32, {128, 1024, 3})); + test_field_encoding(randomized_field(gen, ud_u32, {128, 1024})); + test_field_encoding(randomized_field(gen, ud_u32, {4096})); + + std::uniform_int_distribution ud_i64{0, 1024 * 1024}; + test_field_encoding(randomized_field(gen, ud_i64, {128, 1024, 3})); + test_field_encoding(randomized_field(gen, ud_i64, {128, 1024})); + test_field_encoding(randomized_field(gen, ud_i64, {4096})); +} + } // namespace } // namespace osf } // namespace ouster diff --git a/ouster_osf/tests/reader_test.cpp b/ouster_osf/tests/reader_test.cpp index 1dbe5428..56b100ff 100644 --- a/ouster_osf/tests/reader_test.cpp +++ b/ouster_osf/tests/reader_test.cpp @@ -6,9 +6,12 @@ #include "ouster/osf/reader.h" #include +#include +#include #include "common.h" #include "osf_test.h" +#include "ouster/impl/logging.h" #include "ouster/osf/meta_lidar_sensor.h" #include "ouster/osf/meta_streaming_info.h" #include "ouster/osf/stream_lidar_scan.h" @@ -307,14 +310,21 @@ TEST_F(ReaderTest, MetadataFromBufferTest) { std::vector buf; std::stringstream output_stream; - std::streambuf* old_output_stream = std::cout.rdbuf(); + auto ostream_sink = std::make_shared< + spdlog::sinks::ostream_sink>( + output_stream); + ouster::sensor::impl::Logger::instance().configure_generic_sink( + ostream_sink, "info"); - std::cout.rdbuf(output_stream.rdbuf()); auto result = sensor->from_buffer(buf, "NON EXISTENT"); - std::cout.rdbuf(old_output_stream); - EXPECT_EQ(output_stream.str(), "UNKNOWN TYPE: NON EXISTENT\n"); + + std::string output_error = output_stream.str(); + auto error_loc = output_error.find("[error]"); + EXPECT_NE(error_loc, std::string::npos); + + output_error = output_error.substr(error_loc); + EXPECT_EQ(output_error, "[error] UNKNOWN TYPE: NON EXISTENT\n"); EXPECT_EQ(result, nullptr); - std::cout.rdbuf(old_output_stream); result = sensor->from_buffer(sensor->buffer(), "ouster/v1/os_sensor/LidarSensor"); diff --git a/ouster_osf/tests/writer_test.cpp b/ouster_osf/tests/writer_test.cpp index 80bdd83d..ef3b6ed2 100644 --- a/ouster_osf/tests/writer_test.cpp +++ b/ouster_osf/tests/writer_test.cpp @@ -48,14 +48,14 @@ TEST_F(WriterTest, WriteSingleLidarScan) { std::string output_osf_filename = tmp_file("writer_simple.osf"); - std::string sinfo_str = sinfo.updated_metadata_string(); + std::string sinfo_str = sinfo.to_json_string(); // Writing LidarScan Writer writer(output_osf_filename); writer.set_metadata_id("test_session"); EXPECT_EQ(writer.chunks_layout(), ChunksLayout::LAYOUT_STREAMING); - writer.add_sensor(sinfo, get_field_types(sinfo)); + writer.add_sensor(sinfo); writer.save(0, ls, ts_t{123}); writer.close(); @@ -81,7 +81,7 @@ TEST_F(WriterTest, WriteSingleLidarScan) { // Use first sensor and get its sensor_info auto sinfo_recovered = sensors.begin()->second->info(); - EXPECT_EQ(sinfo_recovered.original_string(), sinfo_str); + EXPECT_EQ(sinfo_recovered.to_json_string(), sinfo_str); auto metadata_recovered = sensors.begin()->second->metadata(); EXPECT_EQ(metadata_recovered, sinfo_str); @@ -94,7 +94,7 @@ TEST_F(WriterTest, WriteLidarSensorWithExtrinsics) { std::string output_osf_filename = tmp_file("writer_lidar_sensor_extrinsics.osf"); - std::string sinfo_str = sinfo.original_string(); + std::string sinfo_str = sinfo.to_json_string(); sinfo.extrinsic(0, 3) = 10.0; sinfo.extrinsic(0, 1) = 0.756; @@ -143,14 +143,14 @@ TEST_F(WriterTest, WriteSingleLidarScanStreamingLayout) { std::string output_osf_filename = tmp_file("writer_simple_streaming.osf"); - std::string sinfo_str = sinfo.updated_metadata_string(); + std::string sinfo_str = sinfo.to_json_string(); // Writing LidarScan Writer writer(output_osf_filename); writer.set_metadata_id("test_session"); EXPECT_EQ(writer.chunks_layout(), ChunksLayout::LAYOUT_STREAMING); - writer.add_sensor(sinfo, get_field_types(sinfo)); + writer.add_sensor(sinfo); writer.save(0, ls, ts_t{123}); writer.close(); @@ -187,7 +187,7 @@ TEST_F(WriterTest, WriteSingleLidarScanStreamingLayout) { // Use first sensor and get its sensor_info auto sinfo_recovered = sensors.begin()->second->info(); - EXPECT_EQ(sinfo_recovered.original_string(), sinfo_str); + EXPECT_EQ(sinfo_recovered.to_json_string(), sinfo.to_json_string()); auto metadata_recovered = sensors.begin()->second->metadata(); EXPECT_EQ(metadata_recovered, sinfo_str); @@ -199,23 +199,21 @@ TEST_F(WriterTest, WriteSlicedLidarScan) { LidarScan ls = get_random_lidar_scan(sinfo); // Subset of fields to leave in LidarScan - LidarScanFieldTypes field_types; - field_types.emplace_back(sensor::ChanField::RANGE, - ls.field_type(sensor::ChanField::RANGE)); - field_types.emplace_back(sensor::ChanField::REFLECTIVITY, - ls.field_type(sensor::ChanField::REFLECTIVITY)); + ouster::LidarScanFieldTypes field_types; + field_types.emplace_back(ls.field_type(sensor::ChanField::RANGE)); + field_types.emplace_back(ls.field_type(sensor::ChanField::REFLECTIVITY)); // Make a reduced field LidarScan ls = slice_with_cast(ls, field_types); - EXPECT_EQ(field_types.size(), std::distance(ls.begin(), ls.end())); + EXPECT_EQ(field_types.size(), ls.field_types().size()); std::string output_osf_filename = tmp_file("writer_sliced.osf"); - std::string sinfo_str = sinfo.updated_metadata_string(); + std::string sinfo_str = sinfo.to_json_string(); // Writing LidarScan - Writer writer(output_osf_filename, sinfo, field_types); + Writer writer(output_osf_filename, sinfo, {"RANGE", "REFLECTIVITY"}); writer.set_metadata_id("test_session"); writer.save(0, ls, ts_t{123}); writer.close(); @@ -231,8 +229,7 @@ TEST_F(WriterTest, WriteSlicedLidarScan) { auto ls_recovered = msg_it->decode_msg(); - EXPECT_EQ(field_types.size(), - std::distance(ls_recovered->begin(), ls_recovered->end())); + EXPECT_EQ(field_types.size(), ls_recovered->field_types().size()); EXPECT_TRUE(ls_recovered); EXPECT_EQ(*ls_recovered, ls); @@ -245,7 +242,7 @@ TEST_F(WriterTest, WriteSlicedLidarScan) { // Use first sensor and get its sensor_info auto sinfo_recovered = sensors.begin()->second->info(); - EXPECT_EQ(sinfo_recovered.original_string(), sinfo_str); + EXPECT_EQ(sinfo_recovered.to_json_string(), sinfo.to_json_string()); auto metadata_recovered = sensors.begin()->second->metadata(); EXPECT_EQ(metadata_recovered, sinfo_str); @@ -263,7 +260,7 @@ TEST_F(WriterTest, WriteSlicedLegacyLidarScan) { field_types.emplace_back(sensor::ChanField::SIGNAL, sensor::ChanFieldType::UINT16); field_types.emplace_back(sensor::ChanField::REFLECTIVITY, - sensor::ChanFieldType::UINT8); + sensor::ChanFieldType::UINT16); std::cout << "LidarScan field_types: " << ouster::to_string(field_types) << std::endl; @@ -272,15 +269,15 @@ TEST_F(WriterTest, WriteSlicedLegacyLidarScan) { // that will be compared with a recovered LidarScan from OSF auto ls_reference = slice_with_cast(ls_orig, field_types); - EXPECT_EQ(field_types.size(), - std::distance(ls_reference.begin(), ls_reference.end())); + EXPECT_EQ(field_types.size(), ls_reference.field_types().size()); std::string output_osf_filename = tmp_file("writer_sliced_legacy.osf"); - std::string sinfo_str = sinfo.updated_metadata_string(); + std::string sinfo_str = sinfo.to_json_string(); // Writing LidarScan with custom field types - Writer writer(output_osf_filename, sinfo, field_types); + Writer writer(output_osf_filename, sinfo, + {"RANGE", "SIGNAL", "REFLECTIVITY"}); writer.set_metadata_id("test_session"); writer.save(0, ls_orig, ts_t{123}); @@ -297,11 +294,10 @@ TEST_F(WriterTest, WriteSlicedLegacyLidarScan) { auto ls_recovered = msg_it->decode_msg(); - EXPECT_EQ(field_types.size(), - std::distance(ls_recovered->begin(), ls_recovered->end())); - EXPECT_TRUE(ls_recovered); + EXPECT_EQ(field_types.size(), ls_recovered->field_types().size()); + EXPECT_EQ(*ls_recovered, ls_reference); EXPECT_EQ(++msg_it, reader.messages().end()); @@ -312,7 +308,7 @@ TEST_F(WriterTest, WriteSlicedLegacyLidarScan) { // Use first sensor and get its sensor_info auto sinfo_recovered = sensors.begin()->second->info(); - EXPECT_EQ(sinfo_recovered.original_string(), sinfo_str); + EXPECT_EQ(sinfo_recovered.to_json_string(), sinfo.to_json_string()); auto metadata_recovered = sensors.begin()->second->metadata(); EXPECT_EQ(metadata_recovered, sinfo_str); @@ -339,9 +335,9 @@ TEST_F(WriterTest, WriteCustomLidarScanWithFlags) { sensor::ChanFieldType::UINT8); field_types_with_flags.emplace_back(sensor::ChanField::FLAGS2, sensor::ChanFieldType::UINT8); - field_types_with_flags.emplace_back(sensor::ChanField::CUSTOM0, + field_types_with_flags.emplace_back("CUSTOM0", sensor::ChanFieldType::UINT64); - field_types_with_flags.emplace_back(sensor::ChanField::CUSTOM7, + field_types_with_flags.emplace_back("CUSTOM7", sensor::ChanFieldType::UINT16); LidarScan ls = get_random_lidar_scan(sinfo.format.columns_per_frame, @@ -363,19 +359,15 @@ TEST_F(WriterTest, WriteCustomLidarScanWithFlags) { // Check that we have non zero CUSTOM7 img_t custom{ls.h, ls.w}; - impl::visit_field(ls, sensor::ChanField::CUSTOM7, - ouster::impl::read_and_cast(), custom); + impl::visit_field(ls, "CUSTOM7", ouster::impl::read_and_cast(), custom); EXPECT_FALSE((custom == 0).all()); - EXPECT_EQ(field_types_with_flags.size(), - std::distance(ls.begin(), ls.end())); + EXPECT_EQ(field_types_with_flags.size(), ls.field_types().size()); std::string output_osf_filename = tmp_file("writer_with_flags.osf"); - std::string sinfo_str = sinfo.original_string(); - // Writing LidarScan - Writer writer(output_osf_filename, sinfo, get_field_types(ls)); + Writer writer(output_osf_filename, sinfo); writer.set_metadata_id("test_session"); writer.save(0, ls, ts_t{123}); writer.close(); @@ -394,7 +386,7 @@ TEST_F(WriterTest, WriteCustomLidarScanWithFlags) { EXPECT_TRUE(ls_recovered); EXPECT_EQ(field_types_with_flags.size(), - std::distance(ls_recovered->begin(), ls_recovered->end())); + ls_recovered->field_types().size()); EXPECT_EQ(*ls_recovered, ls); @@ -525,6 +517,49 @@ TEST_F(WriterTest, FileNameOnlyWriterTest) { EXPECT_EQ(writer.metadata_id(), "ouster_sdk"); } +TEST_F(WriterTest, WriteCustomFieldsTest) { + const sensor_info sinfo = sensor::metadata_from_json( + path_concat(test_data_dir(), "pcaps/OS-1-128_v2.3.0_1024x10.json")); + LidarScan ls = get_random_lidar_scan(sinfo); + auto& f1 = + ls.add_field("custom_field_1", fd_array(85, 129, 344), {}); + auto& f2 = ls.add_field("custom_field_2", fd_array(111, 333), {}); + + std::random_device rd; + std::mt19937 gen{rd()}; + std::normal_distribution nd_d{100.0, 10.0}; + std::uniform_int_distribution ud_u8{0, 150}; + randomize_field(f1, gen, nd_d); + randomize_field(f2, gen, ud_u8); + + std::string output_osf_filename = tmp_file("writer_simple.osf"); + + std::string sinfo_str = sinfo.to_json_string(); + + // Writing LidarScan + Writer writer(output_osf_filename); + writer.set_metadata_id("test_session"); + EXPECT_EQ(writer.chunks_layout(), ChunksLayout::LAYOUT_STREAMING); + + writer.add_sensor(sinfo); + writer.save(0, ls, ts_t{123}); + writer.close(); + + OsfFile osf_file(output_osf_filename); + EXPECT_TRUE(osf_file.good()); + + Reader reader(osf_file); + EXPECT_EQ(reader.metadata_id(), "test_session"); + + auto msg_it = reader.messages().begin(); + EXPECT_NE(msg_it, reader.messages().end()); + + auto ls_recovered = msg_it->decode_msg(); + + EXPECT_TRUE(ls_recovered); + EXPECT_EQ(*ls_recovered, ls); +} + } // namespace } // namespace osf } // namespace ouster diff --git a/ouster_osf/tests/writerv2_test.cpp b/ouster_osf/tests/writerv2_test.cpp index ee62a89e..69764e8a 100644 --- a/ouster_osf/tests/writerv2_test.cpp +++ b/ouster_osf/tests/writerv2_test.cpp @@ -37,8 +37,7 @@ TEST_F(WriterV2Test, WriterV2AccessorTest) { path_concat(test_data_dir(), "pcaps/OS-0-128-U1_v2.3.0_1024x10.json")); { std::vector info_compare = {info}; - Writer writer(output_osf_filename, info, LidarScanFieldTypes(), - chunk_size); + Writer writer(output_osf_filename, info, {}, chunk_size); EXPECT_EQ(writer.chunk_size(), chunk_size); EXPECT_EQ(writer.sensor_info_count(), 1); EXPECT_EQ(writer.filename(), output_osf_filename); @@ -47,8 +46,7 @@ TEST_F(WriterV2Test, WriterV2AccessorTest) { } { std::vector info_compare = {info, info2}; - Writer writer(output_osf_filename, info_compare, LidarScanFieldTypes(), - chunk_size); + Writer writer(output_osf_filename, info_compare, {}, chunk_size); EXPECT_EQ(writer.sensor_info_count(), 2); EXPECT_EQ(writer.sensor_info(), info_compare); @@ -62,11 +60,11 @@ TEST_F(WriterV2Test, WriterV2BoundingTest) { std::string output_osf_filename = tmp_file("WriterV2BoundingTest.osf"); const sensor::sensor_info info = sensor::metadata_from_json( path_concat(test_data_dir(), "pcaps/OS-1-128_v2.3.0_1024x10.json")); - Writer writer(output_osf_filename, info, LidarScanFieldTypes(), chunk_size); + Writer writer(output_osf_filename, info, {}, chunk_size); bool caught = false; try { - LidarScan one; + LidarScan one = get_random_lidar_scan(info); writer.save(1, one); } catch (const std::logic_error& e) { EXPECT_EQ(std::string(e.what()), "ERROR: Bad Stream ID"); @@ -77,8 +75,8 @@ TEST_F(WriterV2Test, WriterV2BoundingTest) { EXPECT_TRUE(caught); caught = false; try { - LidarScan one; - LidarScan two; + LidarScan one = get_random_lidar_scan(info); + LidarScan two = get_random_lidar_scan(info); writer.save({one, two}); } catch (const std::logic_error& e) { EXPECT_EQ(std::string(e.what()), diff --git a/ouster_pcap/CMakeLists.txt b/ouster_pcap/CMakeLists.txt index 611d55f0..31678675 100644 --- a/ouster_pcap/CMakeLists.txt +++ b/ouster_pcap/CMakeLists.txt @@ -5,7 +5,7 @@ find_package(libtins REQUIRED) include(Coverage) # ==== Libraries ==== -add_library(ouster_pcap src/pcap.cpp src/os_pcap.cpp src/indexed_pcap_reader.cpp) +add_library(ouster_pcap src/pcap.cpp src/os_pcap.cpp src/indexed_pcap_reader.cpp src/ip_reassembler.cpp) target_include_directories(ouster_pcap SYSTEM PRIVATE ${PCAP_INCLUDE_DIR}) target_include_directories(ouster_pcap PUBLIC diff --git a/ouster_pcap/src/indexed_pcap_reader.cpp b/ouster_pcap/src/indexed_pcap_reader.cpp index 960cd748..50344f25 100644 --- a/ouster_pcap/src/indexed_pcap_reader.cpp +++ b/ouster_pcap/src/indexed_pcap_reader.cpp @@ -27,7 +27,7 @@ nonstd::optional IndexedPcapReader::sensor_idx_for_current_packet() const { const auto& pkt_info = current_info(); for (size_t i = 0; i < sensor_infos_.size(); i++) { - if (pkt_info.dst_port == sensor_infos_[i].udp_port_lidar) { + if (pkt_info.dst_port == sensor_infos_[i].config.udp_port_lidar) { // TODO use the packet format and match serial number if it's // available this will allow us to have multiple sensors on the same // port diff --git a/ouster_pcap/src/ip_reassembler.cpp b/ouster_pcap/src/ip_reassembler.cpp new file mode 100644 index 00000000..ce386479 --- /dev/null +++ b/ouster_pcap/src/ip_reassembler.cpp @@ -0,0 +1,190 @@ +/* + * Copyright (c) 2017, Matias Fontanini + * All rights reserved. + * + * 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. + * + * 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 + * OWNER 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. + * + */ + +#include "ip_reassembler.h" + +#include +#include +#include + +using std::make_pair; + +namespace Tins { +namespace Internals { + +static const std::chrono::microseconds fragment_timeout(2000000); + +IPv4Stream2::IPv4Stream2() + : received_size_(), total_size_(), received_end_(false) {} + +void IPv4Stream2::add_fragment(const std::chrono::microseconds& timestamp, + IP* ip) { + // handle timeout + if (fragments_.size()) { + if ((timestamp - last_timestamp_) > fragment_timeout) { + // if we timed out, clear out all old packets + received_size_ = 0; + total_size_ = 0; + received_end_ = false; + fragments_.clear(); + first_fragment_ = IP(); + } + } + last_timestamp_ = timestamp; + + const uint16_t offset = extract_offset(ip); + fragments_type::iterator it = fragments_.begin(); + while (it != fragments_.end() && offset > it->offset()) { + ++it; + } + // Replace duplicates + if (it != fragments_.end() && it->offset() == offset) { + it = fragments_.erase(it); + fragments_.insert(it, IPv4Fragment2(ip->inner_pdu(), offset)); + } else { + fragments_.insert(it, IPv4Fragment2(ip->inner_pdu(), offset)); + received_size_ += ip->inner_pdu()->size(); + } + // If the MF flag is not set, this is the end of the packet + if ((ip->flags() & IP::MORE_FRAGMENTS) == 0) { + total_size_ = offset + ip->inner_pdu()->size(); + received_end_ = true; + } + if (offset == 0) { + // Release the inner PDU, store this first fragment and restore the + // inner PDU + PDU* inner_pdu = ip->release_inner_pdu(); + first_fragment_ = *ip; + ip->inner_pdu(inner_pdu); + } +} + +bool IPv4Stream2::is_complete() const { + // If we haven't received the last chunk of we haven't received all the + // data, then we're not complete + if (!received_end_ || received_size_ != total_size_) { + return false; + } + // Make sure the first fragment has offset 0 + return fragments_.begin()->offset() == 0; +} + +PDU* IPv4Stream2::allocate_pdu() const { + PDU::serialization_type buffer; + buffer.reserve(total_size_); + // Check if we actually have all the data we need. Otherwise return nullptr; + size_t expected = 0; + for (fragments_type::const_iterator it = fragments_.begin(); + it != fragments_.end(); ++it) { + if (expected != it->offset()) { + return 0; + } + expected = it->offset() + it->payload().size(); + buffer.insert(buffer.end(), it->payload().begin(), it->payload().end()); + } + return Internals::pdu_from_flag( + static_cast(first_fragment_.protocol()), + buffer.empty() ? 0 : &buffer[0], static_cast(buffer.size())); +} + +const IP& IPv4Stream2::first_fragment() const { return first_fragment_; } + +uint16_t IPv4Stream2::extract_offset(const IP* ip) { + return ip->fragment_offset() * 8; +} + +} // namespace Internals + +IPv4Reassembler2::IPv4Reassembler2() : technique_(NONE) {} + +IPv4Reassembler2::IPv4Reassembler2(OverlappingTechnique technique) + : technique_(technique) {} + +IPv4Reassembler2::PacketStatus IPv4Reassembler2::process( + const std::chrono::microseconds& timestamp, PDU& pdu) { + IP* ip = pdu.find_pdu(); + if (ip && ip->inner_pdu()) { + // There's fragmentation + if (ip->is_fragmented()) { + key_type key = make_key(ip); + // Delete old streams if too many build up, we only expect one per + // lidar in practice We just want to keep from building up too much + // memory If we didnt do this we could end up with 99 MB or so in + // junk worst case + if (streams_.size() > 100) { + for (auto it = streams_.cbegin(); it != streams_.cend();) { + auto age = timestamp - it->second.last_timestamp_; + if (age > Tins::Internals::fragment_timeout) { + it = streams_.erase(it); + } else { + ++it; + } + } + } + // Create it or look it up, it's the same + Internals::IPv4Stream2& stream = streams_[key]; + stream.add_fragment(timestamp, ip); + if (stream.is_complete()) { + PDU* pdu = stream.allocate_pdu(); + // Use all field values from the first fragment + *ip = stream.first_fragment(); + + // Erase this stream, since it's already assembled + streams_.erase(key); + // The packet is corrupt + if (!pdu) { + return FRAGMENTED; + } + ip->inner_pdu(pdu); + ip->fragment_offset(0); + ip->flags(static_cast(0)); + return REASSEMBLED; + } else { + return FRAGMENTED; + } + } + } + return NOT_FRAGMENTED; +} + +IPv4Reassembler2::key_type IPv4Reassembler2::make_key(const IP* ip) const { + return make_pair(ip->id(), + make_address_pair(ip->src_addr(), ip->dst_addr())); +} + +IPv4Reassembler2::address_pair IPv4Reassembler2::make_address_pair( + IPv4Address addr1, IPv4Address addr2) const { + if (addr1 < addr2) { + return make_pair(addr1, addr2); + } else { + return make_pair(addr2, addr1); + } +} + +} // namespace Tins diff --git a/ouster_pcap/src/ip_reassembler.h b/ouster_pcap/src/ip_reassembler.h new file mode 100644 index 00000000..f3c99963 --- /dev/null +++ b/ouster_pcap/src/ip_reassembler.h @@ -0,0 +1,186 @@ +/* + * Copyright (c) 2017, Matias Fontanini + * All rights reserved. + * + * 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. + * + * 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 + * OWNER 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. + * + */ + +#ifndef TINS_IP_REASSEMBLER2_H +#define TINS_IP_REASSEMBLER2_H + +#include +#include +#include +#include + +#include +#include +#include + +namespace Tins { + +/** + * \cond + */ +namespace Internals { +class IPv4Fragment2 { + public: + typedef PDU::serialization_type payload_type; + + IPv4Fragment2() : offset_() {} + + template + IPv4Fragment2(T* pdu, uint16_t offset) + : payload_(pdu->serialize()), offset_(offset) {} + + const payload_type& payload() const { return payload_; } + + uint16_t offset() const { return offset_; } + + private: + payload_type payload_; + uint16_t offset_; +}; + +class TINS_API IPv4Stream2 { + public: + IPv4Stream2(); + + std::chrono::microseconds last_timestamp_; + + void add_fragment(const std::chrono::microseconds& timestamp, IP* ip); + bool is_complete() const; + PDU* allocate_pdu() const; + const IP& first_fragment() const; + + private: + typedef std::vector fragments_type; + + uint16_t extract_offset(const IP* ip); + bool extract_more_frag(const IP* ip); + + fragments_type fragments_; + size_t received_size_; + size_t total_size_; + IP first_fragment_; + bool received_end_; +}; +} // namespace Internals + +/** + * \endcond + */ + +/** + * \brief Reassembles fragmented IP packets. + * + * This class is fairly simple: just feed packets into it using + * IPv4Reassembler::process. If the return value is IPv4Reassembler::FRAGMENTED, + * then the packet is fragmented and we haven't yet seen the missing fragments, + * hence we can't reassemble it. If the function returns either + * IPv4Reassembler::NOT_FRAGMENTED (meaning the packet wasn't fragmented) or + * IPv4Reassembler::REASSEMBLED (meaning the packet was fragmented but it's now + * reassembled), then you can process the packet normally. + * + * Simple example: + * + * \code + * IPv4Reassembler reassembler; + * Sniffer sniffer = ...; + * sniffer.sniff_loop([&](PDU& pdu) { + * // Process it in any case, unless it's fragmented (and can't be + * reassembled yet) if (reassembler.process(pdu) != IPv4Reassembler::FRAGMENTED) + * { + * // Now actually process the packet + * process_packet(pdu); + * } + * }); + * \endcode + */ +class TINS_API IPv4Reassembler2 { + public: + /** + * The status of each processed packet. + */ + enum PacketStatus { + NOT_FRAGMENTED, ///< The given packet is not fragmented + FRAGMENTED, ///< The given packet is fragmented and can't be + ///< reassembled yet + REASSEMBLED ///< The given packet was fragmented but is now reassembled + }; + + TINS_DEPRECATED(typedef PacketStatus packet_status); + + /** + * The type used to represent the overlapped segment reassembly + * technique to be used. + */ + enum OverlappingTechnique { NONE, REPLACE }; + + /** + * Default constructor + */ + IPv4Reassembler2(); + + /** + * Constructs an IPV4Reassembler. + * + * \param technique The technique to be used for reassembling + * overlapped fragments. + */ + IPv4Reassembler2(OverlappingTechnique technique); + + /** + * \brief Processes a PDU and tries to reassemble it. + * + * This method tries to reassemble the provided packet. If + * the packet is successfully reassembled using previously + * processed packets, its contents will be modified so that + * it contains the whole payload and not just a fragment. + * + * \param timestamp The timestamp of the packet. Used for timeouts. + * \param pdu The PDU to process. + * \return NOT_FRAGMENTED if the PDU does not contain an IP + * layer or is not fragmented, FRAGMENTED if the packet is + * fragmented or REASSEMBLED if the packet was fragmented + * but has now been reassembled. + */ + PacketStatus process(const std::chrono::microseconds& timestamp, PDU& pdu); + + private: + typedef std::pair address_pair; + typedef std::pair key_type; + typedef std::map streams_type; + + key_type make_key(const IP* ip) const; + address_pair make_address_pair(IPv4Address addr1, IPv4Address addr2) const; + + streams_type streams_; + OverlappingTechnique technique_; +}; + +} // namespace Tins + +#endif // TINS_IP_REASSEMBLER2_H diff --git a/ouster_pcap/src/pcap.cpp b/ouster_pcap/src/pcap.cpp index ffcf2477..53b2b5f5 100644 --- a/ouster_pcap/src/pcap.cpp +++ b/ouster_pcap/src/pcap.cpp @@ -41,6 +41,8 @@ #include #include +#include "ip_reassembler.h" + using us = std::chrono::microseconds; using timepoint = std::chrono::system_clock::time_point; using namespace Tins; @@ -57,8 +59,7 @@ struct pcap_impl { pcap_reader; ///< Object that holds the unified pcap reader FILE* pcap_reader_internals; Tins::Packet packet_cache; - Tins::IPv4Reassembler - reassembler; ///< The reassembler mainly for lidar packets + IPv4Reassembler2 reassembler; ///< The reassembler mainly for lidar packets bool have_new_packet; int encap_proto; }; @@ -133,8 +134,9 @@ size_t PcapReader::next_packet() { if ((ip && ip->protocol() == PROTOCOL_UDP) || (ipv6 && ipv6->next_header() == PROTOCOL_UDP)) { // reassm is also used in the while loop - reassm = (impl->reassembler.process(*pdu) != - IPv4Reassembler::FRAGMENTED); + reassm = (impl->reassembler.process( + impl->packet_cache.timestamp(), *pdu) != + IPv4Reassembler2::FRAGMENTED); if (reassm) { info.fragments_in_packet = reassm_packets; info.encapsulation_protocol = impl->encap_proto; diff --git a/python/.flake8 b/python/.flake8 new file mode 100644 index 00000000..c4fee9ff --- /dev/null +++ b/python/.flake8 @@ -0,0 +1,20 @@ +[flake8] +max-line-length = 120 +per-file-ignores = + tests/*: D + docs/*: D +ignore = + # E125, E126, E128 continuation line indentation, yapf doesn't fix this + E125, + E126, + E128, + # E251 newlines around equals in keywords, yapf again + E251, + # E731 assigning a lambda expression + E731, + # W503 line break before binary operator, yapf again + W503, + # W504 line break before binary operator, yapf again + W504, + # E741 "l" and "I" as variables names; not a problem with some static analysis + E741 \ No newline at end of file diff --git a/python/Dockerfile b/python/Dockerfile index 42db8209..c7d35967 100644 --- a/python/Dockerfile +++ b/python/Dockerfile @@ -31,8 +31,9 @@ RUN set -xe \ python3-pip \ python3-venv \ ccache \ + libxml2-utils \ # Install any additional available cpython versions for testing - 'python3.(7|8|9|10)-dev' \ + 'python3.(8|9|10)-dev' \ wget \ && rm -rf /var/lib/apt/lists @@ -49,32 +50,8 @@ ENV PATH="${PATH}:${BUILD_HOME}/.local/bin" \ OUSTER_SDK_PATH="/opt/ouster_example" WORKDIR ${BUILD_HOME} -RUN set -xe \ -# use oldest available, supported python as tox default -&& PYTHON=$(which python3.8 python3.9 python3.10 | head -1) \ -&& $PYTHON -m pip install --no-cache-dir --user -U pip tox pybind11 - # Populate source dir -COPY . ${OUSTER_SDK_PATH} +COPY python/docker_install_reqs.sh python/setup.py ${OUSTER_SDK_PATH}/python/ +COPY python/src/ouster/sdk/mapping/requirements.json ${OUSTER_SDK_PATH}/python/src/ouster/sdk/mapping/ -# Entrypoint for running tox: -# -# Usage: from OUSTER_SDK root, run -# docker build . -f python/Dockerfile -t ouster-sdk-tox -# docker run --rm -it [-e VAR=VAL ..] ouster-sdk-tox [TOX ARGS ..] -# -# Without any arguments: run unit tests with all available Python versions. See -# the tox.ini for other commands. -# -# The following environment variables can be set, which may be useful when -# running with additional host bind mounts: -# ARTIFACT_DIR: where to put test output. Defaults to ${BUILD_HOME}/artifacts -# WHEELS_DIR: where to look for wheels for running tests against wheels -# OUSTER_SDK_PATH: path of SDK source -# -ENTRYPOINT ["sh", "-c", "set -e \ -&& rm -rf ./src && cp -a ${OUSTER_SDK_PATH} ./src \ -&& export ARTIFACT_DIR=${ARTIFACT_DIR:-$BUILD_HOME/artifacts} \ -&& . /etc/os-release && export ID VERSION_ID \ -&& exec tox -c ./src/python --workdir ${HOME}/tox \"$@\" \ -", "tox-entrypoint"] +RUN ${OUSTER_SDK_PATH}/python/docker_install_reqs.sh diff --git a/python/README.rst b/python/README.rst index 3779a1a1..0c10dca9 100644 --- a/python/README.rst +++ b/python/README.rst @@ -35,7 +35,7 @@ Supported Platforms Pre-built binaries are provided on `PyPI`_ for the following platforms: - Most glibc-based Linux distributions on x86_64 and ARM64 platforms (``manylinux2010_x86_64``, - ``manylinux2014_aarch64``) + ``manylinux_2_28_aarch64``) - macOS >= 10.15 on x86_64 platforms (``macosx_10_15_x86_64``) - macOS >= 11.0 on Apple M1 for Python >= 3.8 (``macosx_11_0_arm64``) - Windows 10 on x86_64 platforms (``win_amd64``) diff --git a/python/docker_install_reqs.sh b/python/docker_install_reqs.sh new file mode 100755 index 00000000..aa1c9d88 --- /dev/null +++ b/python/docker_install_reqs.sh @@ -0,0 +1,22 @@ +#! /bin/bash + +PYTHON_=$(which python3.8 python3.9 python3.10 | head -1) +PYTHON="${DOCKER_PYTHON:-$PYTHON_}" + +$PYTHON -m pip install --no-cache-dir --user -U pip pybind11 mypy flake8 flake8-formatter-junit-xml wheel pytest + +SCRIPT_DIR=$(cd $(dirname $0) && pwd) +pushd $SCRIPT_DIR +DEPS=$(python3 -- <=2.0, <3', 'more-itertools >=8.6', 'numpy >=1.19, <2, !=1.19.4', - # scipy is not supported on Mac M1 with Mac OS < 12.0 - 'scipy >=1.7, <2;platform_system != "Darwin" or platform_machine != "arm64" or platform_version >= "21.0.0"', + 'laspy >=2.5.0, <3', 'typing-extensions >=3.7.4.3', + 'threadpoolctl >=3.5.0', 'Pillow >=9.2', 'packaging', - 'setuptools; python_version >= "3.12"', + 'setuptools; python_version >= "3.12"' ] - env = os.environ.copy() - skip_mapping = env.get('OUSTER_SDK_SKIP_MAPPING') - if not skip_mapping: - install_requires.append('ouster-mapping==0.2.1; python_version >= "3.8" and python_version <= "3.12"') + # PUT ALL MAPPING REQUIREMENTS INSIDE OF THE requirements.json file + with open(os.path.join(SRC_PATH, + "src", + "ouster", + "sdk", + "mapping", + "requirements.json")) as f: + mapping_reqs = json.loads(f.read()) + for item in mapping_reqs: + temp = f"{item} {mapping_reqs[item]['version']}; {mapping_reqs[item]['marker']}" + install_requires.append(temp) return install_requires - -setup( - name='ouster-sdk', - url='https://github.com/ouster-lidar/ouster_example', - # read from top-level sdk CMakeLists.txt - version=parse_version(), - package_dir={'': 'src'}, - packages=find_namespace_packages(where='src', include='ouster.*'), - package_data={ - 'ouster.sdk.client': ['py.typed', '_client.pyi'], - 'ouster.sdk.pcap': ['py.typed', '_pcap.pyi'], - 'ouster.sdk.osf': ['py.typed', '_osf.pyi'], - 'ouster.sdk.viz': ['py.typed', '_viz.pyi'], - 'ouster.sdk.bag': ['py.typed'] - }, - author='Ouster Sensor SDK Developers', - author_email='oss@ouster.io', - description='Ouster Sensor SDK', - license='BSD 3-Clause License', - ext_modules=[ - CMakeExtension('ouster.*'), - ], - cmdclass={ - 'build_ext': CMakeBuild, - 'sdist': sdk_sdist, - 'bdist_wheel': sdk_bdist_wheel, - }, - zip_safe=False, - python_requires='>=3.8, <4', - install_requires=install_requires(), - extras_require={ - 'test': [ - 'pytest >=7.0, <8', - 'pytest-asyncio', - 'flask==2.2.5' - ], - 'dev': ['flake8', 'mypy', 'pylsp-mypy', 'python-lsp-server', 'yapf'], - 'docs': [ - 'Sphinx >=3.5', - 'sphinx-autodoc-typehints ==1.17.0', - 'sphinx-rtd-theme ==1.0.0', - 'sphinx-copybutton ==0.5.0', - 'docutils <0.18', - 'sphinx-tabs ==3.3.1', - 'breathe ==4.33.1', - 'sphinx-rtd-size' +if __name__ == "__main__": + setup( + name='ouster-sdk', + url='https://github.com/ouster-lidar/ouster_example', + # read from top-level sdk CMakeLists.txt + version=parse_version(), + package_dir={'': 'src'}, + packages=find_namespace_packages(where='src', include='ouster.*'), + package_data={ + 'ouster.sdk.client': ['py.typed', '_client.pyi'], + 'ouster.sdk.pcap': ['py.typed', '_pcap.pyi'], + 'ouster.sdk.osf': ['py.typed', '_osf.pyi'], + 'ouster.sdk.viz': ['py.typed', '_viz.pyi'], + 'ouster.sdk.mapping': ['py.typed', 'requirements.json'], + 'ouster.sdk.bag': ['py.typed'] + }, + author='Ouster Sensor SDK Developers', + author_email='oss@ouster.io', + description='Ouster Sensor SDK', + license='BSD 3-Clause License', + ext_modules=[ + CMakeExtension('ouster.*'), ], - 'examples': [ - 'matplotlib', - 'opencv-python', - 'laspy', - 'PyQt5; platform_system=="Windows"', - ], - }, - entry_points={'console_scripts': - [ - 'simple-viz=ouster.sdk.simple_viz:main', # TODO[UN]: do we need to keep? - 'convert-meta-to-legacy=ouster.sdk.convert_to_legacy:main', # TODO[UN]: do we need to keep? - 'ouster-cli=ouster.cli.core:run' - ] - } -) + cmdclass={ + 'build_ext': CMakeBuild, + 'sdist': sdk_sdist, + 'bdist_wheel': sdk_bdist_wheel, + }, + zip_safe=False, + python_requires='>=3.8, <4', + install_requires=install_requires(), + extras_require={ + 'test': [ + 'pytest >=7.0, <8', + 'pytest-asyncio', + 'flask==2.2.5' + ], + 'dev': ['flake8', 'mypy', 'pylsp-mypy', 'python-lsp-server', 'yapf'], + 'docs': [ + 'Sphinx >=3.5', + 'sphinx-autodoc-typehints ==1.17.0', + 'sphinx-rtd-theme ==1.0.0', + 'sphinx-copybutton ==0.5.0', + 'docutils <0.18', + 'sphinx-tabs ==3.3.1', + 'breathe ==4.33.1', + 'sphinx-rtd-size' + ], + 'examples': [ + 'matplotlib', + 'opencv-python', + 'laspy', + 'PyQt5; platform_system=="Windows"', + ], + }, + entry_points={'console_scripts': + [ + 'simple-viz=ouster.sdk.simple_viz:main', # TODO[UN]: do we need to keep? + 'ouster-cli=ouster.cli.core:run' + ] + } + ) diff --git a/python/src/cpp/_client.cpp b/python/src/cpp/_client.cpp index e04edf01..c39b30e0 100644 --- a/python/src/cpp/_client.cpp +++ b/python/src/cpp/_client.cpp @@ -14,16 +14,10 @@ #include #include #include - -// spdlog includes -// TODO: revise once ubuntu 18.04 is deprecated to drop dependence on -// spdlog with version < 1 #include #include #include -#if (SPDLOG_VER_MAJOR < 1) -#include -#elif ((SPDLOG_VER_MAJOR == 1) && (SPDLOG_VER_MINOR <= 5)) +#if ((SPDLOG_VER_MAJOR == 1) && (SPDLOG_VER_MINOR <= 5)) #include #else #include @@ -46,6 +40,7 @@ #include "ouster/impl/packet_writer.h" #include "ouster/impl/profile_extension.h" #include "ouster/lidar_scan.h" +#include "ouster/sensor_http.h" #include "ouster/types.h" #include "ouster/udp_packet_source.h" @@ -54,12 +49,12 @@ namespace chrono = std::chrono; using spdlog::sinks::base_sink; using ouster::sensor::calibration_status; -using ouster::sensor::ChanField; using ouster::sensor::data_format; using ouster::sensor::ImuPacket; using ouster::sensor::LidarPacket; using ouster::sensor::Packet; using ouster::sensor::packet_format; +using ouster::sensor::product_info; using ouster::sensor::sensor_config; using ouster::sensor::sensor_info; using ouster::sensor::impl::BufferedUDPSource; @@ -67,16 +62,63 @@ using ouster::sensor::impl::Event; using ouster::sensor::impl::packet_writer; using ouster::sensor::impl::Producer; using ouster::sensor::impl::UDPPacketSource; +using ouster::sensor::util::SensorHttp; using namespace ouster; using client_shared_ptr = std::shared_ptr; PYBIND11_MAKE_OPAQUE(client_shared_ptr); +inline py::object from_json(const Json::Value& j) { + switch (j.type()) { + case Json::nullValue: + return py::none(); + case Json::intValue: + case Json::uintValue: + return py::int_(j.asInt64()); + case Json::realValue: + return py::float_(j.asDouble()); + case Json::stringValue: + return py::str(j.asString()); + case Json::booleanValue: + return py::bool_(j.asBool()); + case Json::arrayValue: { + py::list obj(j.size()); + for (std::size_t i = 0; i < j.size(); i++) { + obj[i] = from_json(j[(int)i]); + } + return obj; + } + case Json::objectValue: { + py::dict obj; + for (const auto& member : j.getMemberNames()) { + obj[py::str(member)] = from_json(j[member]); + } + return obj; + } + } + throw std::runtime_error("Unhandled Json Type"); +} + namespace pybind11 { namespace detail { template struct type_caster> : optional_caster> { }; +template <> +struct type_caster { + public: + PYBIND11_TYPE_CASTER(Json::Value, _("Value")); + + bool load(handle /*src*/, bool) { + throw std::runtime_error("Unimplemented"); + } + + static handle cast(Json::Value src, return_value_policy /* policy */, + handle /* parent */) { + object obj = from_json(src); + return obj.release(); + } +}; } // namespace detail } // namespace pybind11 @@ -97,7 +139,6 @@ extern const Table multipurpose_io_mode_strings; extern const Table polarity_strings; extern const Table nmea_baud_rate_strings; -extern Table chanfield_strings; extern Table udp_profile_lidar_strings; extern Table udp_profile_imu_strings; @@ -106,6 +147,7 @@ extern Table thermal_shutdown_status_strings; extern Table full_scale_range_strings; extern Table return_order_strings; +extern const Table field_class_strings; } // namespace impl } // namespace sensor @@ -216,6 +258,18 @@ static sensor::ChanFieldType field_type_of_dtype(const py::dtype& dt) { return sensor::ChanFieldType::UINT32; else if (dt.is(py::dtype::of())) return sensor::ChanFieldType::UINT64; + else if (dt.is(py::dtype::of())) + return sensor::ChanFieldType::INT8; + else if (dt.is(py::dtype::of())) + return sensor::ChanFieldType::INT16; + else if (dt.is(py::dtype::of())) + return sensor::ChanFieldType::INT32; + else if (dt.is(py::dtype::of())) + return sensor::ChanFieldType::INT64; + else if (dt.is(py::dtype::of())) + return sensor::ChanFieldType::FLOAT32; + else if (dt.is(py::dtype::of())) + return sensor::ChanFieldType::FLOAT64; else throw std::invalid_argument("Invalid dtype for a channel field"); } @@ -233,13 +287,39 @@ static py::dtype dtype_of_field_type(const sensor::ChanFieldType& ftype) { return py::dtype::of(); case sensor::ChanFieldType::UINT64: return py::dtype::of(); + case sensor::ChanFieldType::INT8: + return py::dtype::of(); + case sensor::ChanFieldType::INT16: + return py::dtype::of(); + case sensor::ChanFieldType::INT32: + return py::dtype::of(); + case sensor::ChanFieldType::INT64: + return py::dtype::of(); + case sensor::ChanFieldType::FLOAT32: + return py::dtype::of(); + case sensor::ChanFieldType::FLOAT64: + return py::dtype::of(); default: throw std::invalid_argument( - "Invalid field_type for convertion to dtype"); + "Invalid field_type for conversion to dtype"); } return py::dtype(); // unreachable ... } +/** + * Retrieves numpy shape from field descriptor + */ +py::array::ShapeContainer shape_of_descriptor(const FieldDescriptor& rd) { + /** + * Throws if shape is absent i.e. field is not declared as array; we are + * currently not handling those cases, change if/when the usecase arises + */ + if (rd.shape.size() == 0) { + throw std::invalid_argument("Field is not an array"); + } + return py::array::ShapeContainer(rd.shape.begin(), rd.shape.end()); +} + template struct lambda_iter { Fn lambda; @@ -267,8 +347,8 @@ template struct set_field { using Field = py::array_t; - void operator()(const packet_writer& self, LidarPacket& p, ChanField i, - Field field) { + void operator()(const packet_writer& self, LidarPacket& p, + const std::string& i, Field field) { if (field.ndim() != 2 || field.shape(0) != self.pixels_per_column || field.shape(1) != self.columns_per_packet) throw std::invalid_argument("field dimension mismatch"); @@ -309,57 +389,6 @@ struct set_field { } }; -#if (SPDLOG_VER_MAJOR >= 1) // don't include for spdlog < 1.x.x - -/* - * Forward spdlog to python logging module - */ -class PySink : public base_sink { - protected: - void sink_it_(const spdlog::details::log_msg& msg) override { - // https://spdlog.docsforge.com/v1.x/4.sinks/#implementing-your-own-sink - // Probably need #if here for spdlog 0.x on Ubuntu 18.04 - // fmt::memory_buffer formatted; - spdlog::memory_buf_t formatted; - base_sink::formatter_->format(msg, formatted); - - py::gil_scoped_acquire acquire; - // map spdlog levels to python logging levels - int py_level = logger_.attr("NOTSET").cast(); - switch (msg.level) { - case spdlog::level::trace: - case spdlog::level::debug: - py_level = logger_.attr("DEBUG").cast(); - break; - case spdlog::level::info: - py_level = logger_.attr("INFO").cast(); - break; - case spdlog::level::warn: - py_level = logger_.attr("WARNING").cast(); - break; - case spdlog::level::err: - py_level = logger_.attr("ERROR").cast(); - break; - case spdlog::level::critical: - py_level = logger_.attr("CRITICAL").cast(); - break; - default: - break; - } - logger_.attr("log")(py_level, fmt::to_string(formatted)); - } - - // noop for python logger - void flush_() override {} - - py::object logger_; - - public: - PySink(py::object logger) : logger_{logger} {} -}; - -#endif // (SPDLOG_VER_MAJOR >= 1) - PYBIND11_MODULE(_client, m) { m.doc() = R"( Sensor client bindings generated by pybind11. @@ -402,6 +431,19 @@ PYBIND11_MODULE(_client, m) { // Packet Format py::class_(m, "PacketFormat") + .def(py::init([](const sensor_info& info) { + return new packet_format(info); + })) + .def(py::init([](sensor::UDPProfileLidar profile, + size_t pixels_per_column, + size_t columns_per_packet) { + return new packet_format(profile, pixels_per_column, + columns_per_packet); + })) + .def_static("from_metadata", + [](const sensor_info& info) -> const packet_format& { + return sensor::get_format(info); + }, py::return_value_policy::reference) .def_static("from_info", [](const sensor_info& info) -> const packet_format& { return sensor::get_format(info); @@ -466,7 +508,7 @@ PYBIND11_MODULE(_client, m) { }, py::keep_alive<0, 1>()), "Return an iterator of available channel fields.") - .def("packet_field", [](packet_format& pf, sensor::ChanField f, py::buffer buf) -> py::array { + .def("packet_field", [](packet_format& pf, const std::string& f, py::buffer buf) -> py::array { auto buf_ptr = getptr(pf.lidar_packet_size, buf); auto packet_field = [&](auto& res) -> void { @@ -479,7 +521,7 @@ PYBIND11_MODULE(_client, m) { std::vector dims{static_cast(pf.pixels_per_column), static_cast(pf.columns_per_packet)}; - + switch (pf.field_type(f)) { case sensor::ChanFieldType::UINT8: { py::array_t res(dims); @@ -502,7 +544,7 @@ PYBIND11_MODULE(_client, m) { return std::move(res); } default: - throw py::key_error("Invalid field for PacketFormat"); + throw py::key_error("Invalid type for PacketFormat"); } }) @@ -590,24 +632,20 @@ PYBIND11_MODULE(_client, m) { .def("set_field", set_field{}) .def("set_field", set_field{}) .def("set_field", set_field{}) - .def("set_field", set_field{}); + .def("set_field", set_field{}) + .def("set_field", set_field{}) + .def("set_field", set_field{}) + .def("set_field", set_field{}) + .def("set_field", set_field{}) + .def("set_field", set_field{}) + .def("set_field", set_field{}); m.def("scan_to_packets", - [](const LidarScan& ls, const packet_writer& pw, uint32_t init_id, uint64_t prod_sn) -> py::list { - py::list packets; - py::object class_type = - py::module::import("ouster.sdk.client").attr("LidarPacket"); + [](const LidarScan& ls, const packet_writer& pw, uint32_t init_id, uint64_t prod_sn) { + std::vector packets; auto append_pypacket = [&](const LidarPacket& packet) { - py::object pypacket = class_type(py::arg("packet_format") = pw); - // next couple lines should not fail unless someone messes with - // ouster.sdk.client.LidarPacket implementation - LidarPacket* p_ptr = pypacket.cast(); - if (p_ptr->buf.size() != packet.buf.size()) - throw std::invalid_argument("packet sizes don't match"); - p_ptr->host_timestamp = packet.host_timestamp; - std::memcpy(p_ptr->buf.data(), packet.buf.data(), packet.buf.size()); - packets.append(pypacket); + packets.push_back(packet); }; auto iter = make_lambda_iter(append_pypacket); @@ -670,6 +708,36 @@ PYBIND11_MODULE(_client, m) { .def("__str__", [](const calibration_status& i) { return to_string(i); }) .def("__eq__", [](const calibration_status& i, const calibration_status& j) { return i == j; }); + // Product Line + py::class_(m, "ProductInfo") + .def_readonly("full_product_info", &product_info::full_product_info, R"( + The original full product info string. + + :type: string + )") + .def_readonly("form_factor", &product_info::form_factor, R"( + The form factor of the product. + + :type: string + )") + .def_readonly("short_range", &product_info::short_range, R"( + If the product is a short range make. + + :type: bool + )") + .def_readonly("beam_config", &product_info::beam_config, R"( + The beam configuration of the product.. + + :type: string + )") + .def_readonly("beam_count", &product_info::beam_count, R"( + Number of beams + + :type: int + )") + .def("__eq__", [](const product_info& i, const product_info& j) { return i == j; }) + .def("__str__", [](const product_info& i) {return to_string(i);}); + // Sensor Info py::class_(m, "SensorInfo", R"( Sensor Info required to interpret UDP data streams. @@ -677,17 +745,26 @@ PYBIND11_MODULE(_client, m) { See the sensor documentation for the meaning of each property. )") .def(py::init<>(), R"( + Construct an empty metadata. + )") + .def(py::init([](const std::string& s, bool skip_beam_validation) { + return new sensor_info(s, skip_beam_validation); + }), py::arg("s"), py::arg("skip_beam_validation") = false, R"( Args: - raw (str): json string to parse + s (str): json string to parse + skip_beam_validation (boolean): if true, skip validating beam angles )") - .def("__init__", [](sensor_info& self, const std::string& s, bool skip_beam_validation) { - new (&self) sensor_info{}; - self = sensor::parse_metadata(s, skip_beam_validation); - }, py::arg("s"), py::arg("skip_beam_validation") = false) - .def_readwrite("hostname", &sensor_info::name, "Sensor hostname.") .def_readwrite("sn", &sensor_info::sn, "Sensor serial number.") .def_readwrite("fw_rev", &sensor_info::fw_rev, "Sensor firmware revision.") - .def_readwrite("mode", &sensor_info::mode, "Lidar mode, e.g., 1024x10.") + .def_property("mode", [](const sensor_info& self) { + ouster::sensor::logger().warn("sensor_info.mode is deprecated, use sensor_info.config.lidar_mode instead."); + return self.config.lidar_mode; + }, + [](sensor_info& self, ouster::sensor::lidar_mode mode) { + ouster::sensor::logger().warn("sensor_info.mode is deprecated, use sensor_info.config.lidar_mode instead."); + self.config.lidar_mode = mode; + }) + //.def_readwrite("mode", &sensor_info::config::mode, "Lidar mode, e.g., 1024x10.") .def_readwrite("prod_line", &sensor_info::prod_line, "Product line, e.g., 'OS-1-128'.") .def_readwrite("format", &sensor_info::format, "Describes the structure of a lidar packet. See class DataFormat.") .def_readwrite("beam_azimuth_angles", &sensor_info::beam_azimuth_angles, "Beam azimuth angles, useful for XYZ projection.") @@ -698,30 +775,45 @@ PYBIND11_MODULE(_client, m) { .def_readwrite("beam_to_lidar_transform", &sensor_info::beam_to_lidar_transform, "Homogenous transformation matrix reprsenting Beam to Lidar Transform") .def_readwrite("extrinsic", &sensor_info::extrinsic, "Extrinsic Matrix.") .def_readwrite("init_id", &sensor_info::init_id, "Initialization id.") - .def_readwrite("udp_port_lidar", &sensor_info::udp_port_lidar, "Configured port for lidar data.") - .def_readwrite("udp_port_imu", &sensor_info::udp_port_imu, "Configured port for imu data.") + .def_property("udp_port_lidar", [](const sensor_info& self) { + ouster::sensor::logger().warn("sensor_info.udp_port_lidar is deprecated, use sensor_info.config.udp_port_lidar instead."); + return self.config.udp_port_lidar; + }, + [](sensor_info& self, int port) { + ouster::sensor::logger().warn("sensor_info.udp_port_lidar is deprecated, use sensor_info.config.udp_port_lidar instead."); + self.config.udp_port_lidar = port; + }) + .def_property("udp_port_imu", [](const sensor_info& self) { + ouster::sensor::logger().warn("sensor_info.udp_port_imu is deprecated, use sensor_info.config.udp_port_imu instead."); + return self.config.udp_port_imu; + }, + [](sensor_info& self, int port) { + ouster::sensor::logger().warn("sensor_info.udp_port_imu is deprecated, use sensor_info.config.udp_port_imu instead."); + self.config.udp_port_imu = port; + }) .def_readwrite("build_date", &sensor_info::build_date, "Build date") .def_readwrite("image_rev", &sensor_info::image_rev, "Image rev") .def_readwrite("prod_pn", &sensor_info::prod_pn, "Prod pn") .def_readwrite("status", &sensor_info::status, "sensor status") .def_readwrite("cal", &sensor_info::cal, "sensor calibration") .def_readwrite("config", &sensor_info::config, "sensor config") + .def_readwrite("user_data", &sensor_info::user_data, "sensor user data") .def_static("from_default", &sensor::default_sensor_info, R"( Create gen-1 OS-1-64 SensorInfo populated with design values. )") - .def("original_string", &sensor_info::original_string, R"( Return original string that initialized sensor_info" - )") - .def("updated_metadata_string", &sensor_info::updated_metadata_string, R"( Return metadata string made from updated entries" + .def("to_json_string", &sensor_info::to_json_string, R"( Return metadata string made from current entries" )") .def("has_fields_equal", &sensor_info::has_fields_equal, R"(Compare public fields")") // only uncomment for debugging purposes!! story for general use and output is not filled //.def("__str__", [](const sensor_info& i) { return to_string(i); }) .def("__eq__", [](const sensor_info& i, const sensor_info& j) { return i == j; }) .def("__repr__", [](const sensor_info& self) { - const auto mode = self.mode ? to_string(self.mode) : std::to_string(self.format.fps) + "fps"; + const auto mode = self.config.lidar_mode ? to_string(self.config.lidar_mode.value()) : std::to_string(self.format.fps) + "fps"; return ""; }) + .def("get_version", [](const sensor_info& self) { return self.get_version(); }, R"(Get parsed sensor version)") + .def("get_product_info", [](const sensor_info& self) { return self.get_product_info(); }, R"(Get parsed product info)") .def("__copy__", [](const sensor_info& self) { return sensor_info{self}; }) .def("__deepcopy__", [](const sensor_info& self, py::dict) { return sensor_info{self}; }); @@ -733,12 +825,6 @@ PYBIND11_MODULE(_client, m) { Determines to horizontal and vertical resolution rates of sensor. See sensor documentation for details.)"); def_enum(lidar_mode, sensor::impl::lidar_mode_strings, "MODE_"); - lidar_mode.def_property_readonly("cols", [](const sensor::lidar_mode& self) { - return sensor::n_cols_of_lidar_mode(self); }, - "Returns columns of lidar mode, e.g., 1024 for LidarMode 1024x10."); - lidar_mode.def_property_readonly("frequency", [](const sensor::lidar_mode& self) { - return sensor::frequency_of_lidar_mode(self); }, - "Returns frequency of lidar mode, e.g. 10 for LidarMode 512x10."); // TODO: this is wacky lidar_mode.value("MODE_UNSPEC", sensor::lidar_mode::MODE_UNSPEC); lidar_mode.attr("from_string") = py::cpp_function([](const std::string& s) { @@ -787,11 +873,6 @@ PYBIND11_MODULE(_client, m) { Expected baud rate sensor attempts to decode for NMEA UART input $GPRMC messages.)"); def_enum(NMEABaudRate, sensor::impl::nmea_baud_rate_strings); - auto ChanField = py::enum_(m, "ChanField", R"( - Channel data block fields - )"); - def_enum(ChanField, sensor::impl::chanfield_strings); - auto UDPProfileLidar = py::enum_(m, "UDPProfileLidar", "UDP lidar profile."); def_enum(UDPProfileLidar, sensor::impl::udp_profile_lidar_strings, "PROFILE_LIDAR_"); UDPProfileLidar.attr("from_string") = py::cpp_function( @@ -820,23 +901,27 @@ PYBIND11_MODULE(_client, m) { auto ThermalShutdownStatus = py::enum_(m, "ThermalShutdownStatus", "Thermal Shutdown Status."); def_enum(ThermalShutdownStatus, sensor::impl::thermal_shutdown_status_strings); + auto FieldClass = py::enum_(m, "FieldClass", "LidarScan field classes", py::arithmetic()); + def_enum(FieldClass, sensor::impl::field_class_strings); + // Sensor Config py::class_(m, "SensorConfig", R"( Corresponds to sensor config parameters. Please see sensor documentation for the meaning of each property. )") - .def(py::init<>(), R"( + .def(py::init<>(), "Construct an empty SensorConfig.") + .def(py::init([](const std::string& s) { + auto config = new sensor_config{}; + *config = sensor::parse_config(s); + return config; + }), py::arg("s"), R"(Construct a SensorConfig from a json string. Args: - raw (str): json string to parse + s (str): json string to parse )") - .def("__init__", [](sensor_config& self, const std::string &s) { - new (&self) sensor_config{}; - self = sensor::parse_config(s); - }) .def_readwrite("udp_dest", &sensor_config::udp_dest, "Destination to which sensor sends UDP traffic.") .def_readwrite("udp_port_lidar", &sensor_config::udp_port_lidar, "Port on UDP destination to which lidar data will be sent.") .def_readwrite("udp_port_imu", &sensor_config::udp_port_imu, "Port on UDP destination to which IMU data will be sent.") - .def_readwrite("timestamp_mode", &sensor_config::ts_mode, "Timestamp mode of sensor. See class TimestampMode.") - .def_readwrite("lidar_mode", &sensor_config::ld_mode, "Horizontal and Vertical Resolution rate of sensor as mode, e.g., 1024x10. See class LidarMode.") + .def_readwrite("timestamp_mode", &sensor_config::timestamp_mode, "Timestamp mode of sensor. See class TimestampMode.") + .def_readwrite("lidar_mode", &sensor_config::lidar_mode, "Horizontal and Vertical Resolution rate of sensor as mode, e.g., 1024x10. See class LidarMode.") .def_readwrite("operating_mode", &sensor_config::operating_mode, "Operating Mode of sensor. See class OperatingMode.") .def_readwrite("multipurpose_io_mode", &sensor_config::multipurpose_io_mode, "Mode of MULTIPURPOSE_IO pin. See class MultipurposeIOMode.") .def_readwrite("azimuth_window", &sensor_config::azimuth_window, "Tuple representing the visible region of interest of the sensor in millidegrees, .e.g., (0, 360000) for full visibility.") @@ -921,16 +1006,6 @@ PYBIND11_MODULE(_client, m) { )", py::arg("hostname"), py::arg("config"), py::arg("persist") = false, py::arg("udp_dest_auto") = false, py::arg("force_reinit") = false); - m.def("convert_to_legacy", &ouster::sensor::convert_to_legacy , R"( - Convert a non-legacy metadata in string format to legacy - - Args: - metadata: non-legacy metadata string - - Returns: - returns legacy string representation of metadata. - )", py::arg("metadata")); - m.def("get_config", [](const std::string& hostname, bool active) { sensor::sensor_config config; if (!sensor::get_config(hostname, config, active)) { @@ -951,10 +1026,13 @@ PYBIND11_MODULE(_client, m) { .def("__eq__", [](const util::version& u, const util::version& v) { return u == v; }) .def("__lt__", [](const util::version& u, const util::version& v) { return u < v; }) .def("__le__", [](const util::version& u, const util::version& v) { return u <= v; }) - .def("__str__", [](const util::version& u) { return to_string(u); }) .def_readwrite("major", &util::version::major) .def_readwrite("minor", &util::version::minor) .def_readwrite("patch", &util::version::patch) + .def_readwrite("stage", &util::version::stage) + .def_readwrite("machine", &util::version::machine) + .def_readwrite("prerelease", &util::version::prerelease) + .def_readwrite("build", &util::version::build) .def_static("from_string", &util::version_from_string); m.attr("invalid_version") = util::invalid_version; @@ -1019,12 +1097,10 @@ PYBIND11_MODULE(_client, m) { }) .def( "get_metadata", - [](client_shared_ptr& self, int timeout_sec, - bool legacy_format) -> std::string { - return sensor::get_metadata(*self, timeout_sec, legacy_format); + [](client_shared_ptr& self, int timeout_sec) -> std::string { + return sensor::get_metadata(*self, timeout_sec); }, - py::arg("timeout_sec") = DEFAULT_HTTP_REQUEST_TIMEOUT_SECONDS, - py::arg("legacy") = false) + py::arg("timeout_sec") = DEFAULT_HTTP_REQUEST_TIMEOUT_SECONDS) .def("shutdown", [](client_shared_ptr& self) { self.reset(); }); // Client Handle @@ -1156,6 +1232,74 @@ PYBIND11_MODULE(_client, m) { return self.capacity(); }); + py::class_(m, "FieldType", R"( + Describes a field. + )") + .def(py::init( + [](const std::string& name, py::type dtype, + py::tuple extra_dims, + size_t flags = (size_t)ouster::FieldClass::PIXEL_FIELD) { + return new FieldType( + name, field_type_of_dtype(py::dtype::from_args(dtype)), + extra_dims.cast>(), + static_cast(flags)); + }), + R"( + Construct a FieldType. + + Args: + name: name of the field + dt: data type of the field, e.g. np.uint8. + extra_dims: a tuple representing the number of elements. + in the dimensions beyond width and height, + for fields with three or more dimensions. + field_class: indicates whether the field has an entry + per-packet, per-column, per-scan or per-pixel. + )", + py::arg("name"), py::arg("dtype"), + py::arg("extra_dims") = py::tuple(), + py::arg("field_class") = ouster::FieldClass::PIXEL_FIELD) + .def_readwrite("name", &FieldType::name, "The name of the field.") + .def_readwrite( + "field_class", &FieldType::field_class, + R"("field_class - an enum that indicates whether the field has an entry + per-packet, per-column, per-scan or per-pixel.)") + .def_property( + "extra_dims", + [](FieldType& self) -> py::tuple { + py::tuple extra_dims_tuple(self.extra_dims.size()); + for (size_t i = 0; i < self.extra_dims.size(); i++) { + extra_dims_tuple[i] = self.extra_dims[i]; + } + return extra_dims_tuple; + }, + [](FieldType& self, py::tuple extra_dims) { + self.extra_dims = extra_dims.cast>(); + }, + R"(A tuple representing the size of extra dimensions + (if the field is greater than 2 dimensions.))") + .def_property( + "element_type", + [](FieldType& self) -> py::dtype { + return dtype_of_field_type(self.element_type); + }, + [](FieldType& self, py::dtype dtype) { + self.element_type = field_type_of_dtype(dtype); + }, + "The data type (as a numpy dtype) of the field.") + .def("__lt__", + [](const FieldType& u, const FieldType& v) { return u < v; }) + .def("__repr__", + [](const FieldType& self) { + std::stringstream ss; + ss << ""; + return ss.str(); + }) + .def("__eq__", + [](const FieldType& l, const FieldType& r) { return l == r; }) + .def("__str__", [](const FieldType& self) { return to_string(self); }); + // Scans py::class_(m, "LidarScan", R"( Represents a single "scan" or "frame" of lidar data. @@ -1166,12 +1310,8 @@ PYBIND11_MODULE(_client, m) { column of data in each field. )") // TODO: Python and C++ API differ in h/w order for some reason - .def( - "__init__", - [](LidarScan& self, size_t h, size_t w) { - new (&self) LidarScan(w, h); - }, - R"( + .def(py::init([](size_t h, size_t w) { return new LidarScan(w, h); }), + R"( Default constructor creates a 0 x 0 scan @@ -1183,14 +1323,12 @@ PYBIND11_MODULE(_client, m) { New LidarScan of 0x0 expecting fields of the LEGACY profile )", - py::arg("h"), py::arg("w")) - .def( - "__init__", - [](LidarScan& self, size_t h, size_t w, - sensor::UDPProfileLidar profile, size_t columns_per_packet) { - new (&self) LidarScan(w, h, profile, columns_per_packet); - }, - R"( + py::arg("h"), py::arg("w")) + .def(py::init([](size_t h, size_t w, sensor::UDPProfileLidar profile, + size_t columns_per_packet) { + return new LidarScan(w, h, profile, columns_per_packet); + }), + R"( Initialize a scan with the default fields for a particular udp profile @@ -1203,67 +1341,49 @@ PYBIND11_MODULE(_client, m) { New LidarScan of specified dimensions expecting fields of specified profile )", - py::arg("h"), py::arg("w"), py::arg("profile"), - py::arg("columns_per_packet") = DEFAULT_COLUMNS_PER_PACKET) - .def( - "__init__", - [](LidarScan& self, size_t h, size_t w, - const std::map& field_types, - size_t columns_per_packet) { - std::map ft; - for (const auto& kv : field_types) { - auto dt = py::dtype::from_args(kv.second); - ft[kv.first] = field_type_of_dtype(dt); - } - new (&self) - LidarScan(w, h, ft.begin(), ft.end(), columns_per_packet); - }, - R"( + py::arg("h"), py::arg("w"), py::arg("profile"), + py::arg("columns_per_packet") = DEFAULT_COLUMNS_PER_PACKET) + .def(py::init([](size_t h, size_t w, + const std::vector& field_types, + size_t columns_per_packet) { + return new LidarScan(w, h, field_types.begin(), + field_types.end(), columns_per_packet); + }), + R"( Initialize a scan with a custom set of fields Args: height: height of LidarScan, i.e., number of channels width: width of LidarScan - fields_dict: dict where keys are ChanFields and values are type, e.g., {client.ChanField.SIGNAL: np.uint32} + field_types: list of FieldType that specifies which fields should be present in the scan Returns: New LidarScan of specified dimensions expecting fields specified by dict )", - py::arg("w"), py::arg("h"), py::arg("field_types"), - py::arg("columns_per_packet") = DEFAULT_COLUMNS_PER_PACKET) - .def( - "__init__", - [](LidarScan& self, const LidarScan& source, - const std::map& field_types) { - LidarScanFieldTypes ft{}; - for (const auto& f : field_types) { - auto dtype = py::dtype::from_args(f.second); - ft.push_back( - std::make_pair(f.first, field_type_of_dtype(dtype))); - } - new (&self) LidarScan(source, ft); - }, - R"( + py::arg("w"), py::arg("h"), py::arg("field_types"), + py::arg("columns_per_packet") = DEFAULT_COLUMNS_PER_PACKET) + .def(py::init([](const LidarScan& source, + const std::vector& field_types) { + return new LidarScan(source, field_types); + }), + R"( Initialize a lidar scan from another with only the indicated fields. Casts, zero pads or removes fields from the original scan if necessary. Args: source: LidarScan to copy data from - fields_dict: dict of fields to have in the new scan where keys are ChanFields - and values are type, e.g., {client.ChanField.SIGNAL: np.uint32} + field_types: list of fields to have in the new scan where keys are ChanFields + and values are type, e.g., FieldType(client.ChanField.SIGNAL, np.uint32) Returns: New LidarScan with selected data copied over or zero padded )", - py::arg("source"), py::arg("field_types")) - .def( - "__init__", - [](LidarScan& self, const LidarScan& source) { - new (&self) LidarScan(source); - }, - R"( + py::arg("source"), py::arg("field_types")) + .def(py::init( + [](const LidarScan& source) { return new LidarScan(source); }), + R"( Initialize a lidar scan with a copy of the data from another. Args: @@ -1273,7 +1393,7 @@ PYBIND11_MODULE(_client, m) { New LidarScan with data copied over from provided scan. )", - py::arg("source")) + py::arg("source")) .def_readonly("w", &LidarScan::w, "Width or horizontal resolution of the scan.") .def_readonly("h", &LidarScan::h, @@ -1298,26 +1418,142 @@ PYBIND11_MODULE(_client, m) { nonstd::nullopt)) .def( "field", - [](LidarScan& self, sensor::ChanField f) { - std::vector dims{static_cast(self.h), - static_cast(self.w)}; - py::array res; - impl::visit_field(self, f, [&](auto field) { - auto dtype = - py::dtype::of(); - res = py::array(dtype, dims, field.data(), py::cast(self)); - }); - return res; + [](LidarScan& self, const std::string& name) { + auto&& field = self.field(name); + auto dt = dtype_of_field_type(field.tag()); + auto shape = shape_of_descriptor(field.desc()); + return py::array(dt, shape, field.get(), py::cast(self)); }, R"( Return a view of the specified channel field. Args: - field: The channel field to return + name: name of the field to return Returns: The specified field as a numpy array )") + .def( + "add_field", + [](LidarScan& self, const std::string& name, py::type dtype, + py::tuple shape, size_t field_class) { + auto ft = field_type_of_dtype(py::dtype::from_args(dtype)); + Field& ptr = self.add_field( + FieldType(name, ft, shape.cast>(), + static_cast(field_class))); + + auto dt = dtype_of_field_type(ptr.tag()); + auto sh = shape_of_descriptor(ptr.desc()); + return py::array(dt, sh, ptr.get(), py::cast(self)); + }, + R"( + Adds a new field under specified name + + Args: + name: name of the field to add + shape: tuple of ints, shape of the field to add + dtype: dtype of field to add, e.g. np.uint32 + field_class: class of the field to add, see field_class + + Returns: + The field as a numpy array + )", + py::arg("name"), py::arg("dtype"), py::arg("shape") = py::tuple(), + py::arg("field_class") = ouster::FieldClass::PIXEL_FIELD) + .def( + "add_field", + [](LidarScan& self, const FieldType& type) { + Field& ptr = self.add_field(type); + + auto dt = dtype_of_field_type(ptr.tag()); + auto sh = shape_of_descriptor(ptr.desc()); + return py::array(dt, sh, ptr.get(), py::cast(self)); + }, + R"( + Adds a new field under specified name and type + + Args: + type: FieldType of the field to add + + Returns: + The field as a numpy array + )", + py::arg("type")) + .def( + "add_field", + [](LidarScan& self, const std::string& name, const py::array& data, + size_t field_class) -> py::array { + if (!(data.flags() & py::array::c_style)) { + throw std::invalid_argument( + "add_field: submitted ndarray is not c-contiguous"); + } + + auto dt = data.dtype(); + std::vector shape; + for (int i = 0; i < data.ndim(); i++) { + shape.push_back(data.shape(i)); + } + auto ft = field_type_of_dtype(dt); + Field& ptr = self.add_field( + name, FieldDescriptor::array(ft, shape), + static_cast(field_class)); + auto sh = shape_of_descriptor(ptr.desc()); + py::buffer_info info = data.request(); + memcpy(ptr.get(), info.ptr, ptr.bytes()); + return py::array(dt, sh, ptr.get(), py::cast(self)); + }, + R"( + Adds a new field under the specified name, with the given contents. + IMPORTANT: this will deep copy the supplied data. + + Args: + name: the name of the new field + data: the contents of the new field + field_class: class of the field to add, see field_class + + Returns: + The field as a numpy array. + )", + py::arg("name"), py::arg("data"), + py::arg("field_class") = ouster::FieldClass::PIXEL_FIELD) + .def( + "del_field", + [](LidarScan& self, const std::string& name) -> py::array { + // need a heap allocated Field to pass to python + Field* field = new Field(std::move(self.del_field(name))); + auto dt = dtype_of_field_type(field->tag()); + auto shape = shape_of_descriptor(field->desc()); + + py::capsule cleanup{field, [](void* p) { + Field* ptr = + reinterpret_cast(p); + delete ptr; + }}; + return py::array(dt, shape, field->get(), cleanup); + }, + R"( + Release a field from the LidarScan and return it to the user + + Args: + name: name of the field to drop + + Returns: + The specified field as a numpy array + )") + .def( + "field_class", + [](LidarScan& self, const std::string& name) -> ouster::FieldClass { + return self.field(name).field_class(); + }, + R"( + Retrieve FieldClass of field + + Args: + name: name of the field + + Returns: + FieldClass of the field + )") .def("shot_limiting", &LidarScan::shot_limiting, "The frame shot limiting status.") .def("thermal_shutdown", &LidarScan::thermal_shutdown, @@ -1360,22 +1596,27 @@ PYBIND11_MODULE(_client, m) { .def_property_readonly( "pose", [](LidarScan& self) { - return py::array( - py::dtype::of(), - std::vector{ - static_cast(self.timestamp().size()), 4, 4}, - self.pose().data()->data(), py::cast(self)); + auto&& field = self.pose(); + return py::array(py::dtype::of(), + shape_of_descriptor(field.desc()), field.get(), + py::cast(self)); }, "The pose vector of 4x4 homogeneous matrices (per each timestamp).") .def_property_readonly( "fields", - // NOTE: keep_alive seems to be ignored without cpp_function wrapper - py::cpp_function( - [](LidarScan& self) { - return py::make_key_iterator(self.begin(), self.end()); - }, - py::keep_alive<0, 1>()), - "Return an iterator of available channel fields.") + [](const LidarScan& self) { + std::vector keys; + for (const auto& f : self.fields()) { + keys.push_back(f.first); + } + std::sort(keys.begin(), keys.end()); + return keys; + }, + "Return an list of available fields.") + .def_property_readonly( + "field_types", + [](const LidarScan& self) { return self.field_types(); }, + "Return an list of available fields.") .def("__eq__", [](const LidarScan& l, const LidarScan& r) { return l == r; }) .def("__copy__", [](const LidarScan& self) { return LidarScan{self}; }) @@ -1402,6 +1643,47 @@ PYBIND11_MODULE(_client, m) { m.def("destagger_float", &ouster::destagger); m.def("destagger_double", &ouster::destagger); + py::class_(m, "SensorHttp") + .def("metadata", &SensorHttp::metadata, py::arg("timeout_sec") = 1) + .def("sensor_info", &SensorHttp::sensor_info, + py::arg("timeout_sec") = 1) + .def("get_config_params", &SensorHttp::get_config_params, + py::arg("active"), py::arg("timeout_sec") = 1) + .def("set_config_param", &SensorHttp::set_config_param, py::arg("key"), + py::arg("value"), py::arg("timeout_sec") = 1) + .def("active_config_params", &SensorHttp::active_config_params, + py::arg("timeout_sec") = 1) + .def("staged_config_params", &SensorHttp::staged_config_params, + py::arg("timeout_sec") = 1) + .def("set_udp_dest_auto", &SensorHttp::set_udp_dest_auto, + py::arg("timeout_sec") = 1) + .def("beam_intrinsics", &SensorHttp::beam_intrinsics, + py::arg("timeout_sec") = 1) + .def("imu_intrinsics", &SensorHttp::imu_intrinsics, + py::arg("timeout_sec") = 1) + .def("lidar_intrinsics", &SensorHttp::lidar_intrinsics, + py::arg("timeout_sec") = 1) + .def("lidar_data_format", &SensorHttp::lidar_data_format, + py::arg("timeout_sec") = 1) + .def("reinitialize", &SensorHttp::reinitialize, + py::arg("timeout_sec") = 1) + .def("save_config_params", &SensorHttp::save_config_params, + py::arg("timeout_sec") = 1) + .def("get_user_data", &SensorHttp::get_user_data, + py::arg("timeout_sec") = 1) + // TODO: get_user_data_and_policy is hard to bind, bind later if needed + .def("set_user_data", &SensorHttp::set_user_data, py::arg("data"), + py::arg("keep_on_config_delete") = true, + py::arg("timeout_sec") = 1) + .def("delete_user_data", &SensorHttp::delete_user_data, + py::arg("timeout_sec") = 1) + .def_static( + "create", + [](const std::string& hostname, int timeout_sec) { + return SensorHttp::create(hostname, timeout_sec); + }, + py::arg("hostname"), py::arg("timeout_sec") = 40); + py::class_(m, "ScanBatcher") .def(py::init()) .def(py::init()) @@ -1421,31 +1703,30 @@ PYBIND11_MODULE(_client, m) { // XYZ Projection py::class_(m, "XYZLut") - .def( - "__init__", - [](XYZLut& self, const sensor_info& sensor, bool use_extrinsics) { - new (&self) XYZLut{}; - if (use_extrinsics) { - // apply extrinsics after lidar_to_sensor_transform so the - // resulting LUT will produce the coordinates in - // "extrinsics frame" instead of "sensor frame" - mat4d ext_transform = sensor.extrinsic; - ext_transform(0, 3) /= sensor::range_unit; - ext_transform(1, 3) /= sensor::range_unit; - ext_transform(2, 3) /= sensor::range_unit; - ext_transform = - ext_transform * sensor.lidar_to_sensor_transform; - self = make_xyz_lut( - sensor.format.columns_per_frame, - sensor.format.pixels_per_column, sensor::range_unit, - sensor.beam_to_lidar_transform, ext_transform, - sensor.beam_azimuth_angles, - sensor.beam_altitude_angles); - } else { - self = make_xyz_lut(sensor); - } - }, - py::arg("info"), py::arg("use_extrinsics") = false) + .def(py::init([](const sensor_info& sensor, bool use_extrinsics) { + auto self = new XYZLut{}; + if (use_extrinsics) { + // apply extrinsics after lidar_to_sensor_transform so the + // resulting LUT will produce the coordinates in + // "extrinsics frame" instead of "sensor frame" + mat4d ext_transform = sensor.extrinsic; + ext_transform(0, 3) /= sensor::range_unit; + ext_transform(1, 3) /= sensor::range_unit; + ext_transform(2, 3) /= sensor::range_unit; + ext_transform = + ext_transform * sensor.lidar_to_sensor_transform; + *self = make_xyz_lut( + sensor.format.columns_per_frame, + sensor.format.pixels_per_column, sensor::range_unit, + sensor.beam_to_lidar_transform, ext_transform, + sensor.beam_azimuth_angles, + sensor.beam_altitude_angles); + } else { + *self = make_xyz_lut(sensor); + } + return self; + }), + py::arg("info"), py::arg("use_extrinsics") = false) .def("__call__", [](const XYZLut& self, Eigen::Ref>& range) { return cartesian(range, self); @@ -1475,13 +1756,7 @@ PYBIND11_MODULE(_client, m) { m.def( "get_field_types", [](const sensor::sensor_info& info) { - auto field_types = ouster::get_field_types(info); - std::map field_types_res{}; - for (const auto& f : field_types) { - auto dtype = dtype_of_field_type(f.second); - field_types_res.emplace(f.first, dtype); - } - return field_types_res; + return ouster::get_field_types(info); }, R"( Extracts LidarScan fields with types for a given SensorInfo @@ -1494,38 +1769,10 @@ PYBIND11_MODULE(_client, m) { )", py::arg("info")); - m.def( - "get_field_types", - [](const LidarScan& ls) { - auto field_types = ouster::get_field_types(ls); - std::map field_types_res{}; - for (const auto& f : field_types) { - auto dtype = dtype_of_field_type(f.second); - field_types_res.emplace(f.first, dtype); - } - return field_types_res; - }, - R"( - Extracts LidarScan fields with types for a given lidar scan ``ls`` - - Args: - ls: lidar scan from which to get field types - - Returns: - returns field types - )", - py::arg("lidar_scan")); - m.def( "get_field_types", [](sensor::UDPProfileLidar profile) { - auto field_types = ouster::get_field_types(profile); - std::map field_types_res{}; - for (const auto& f : field_types) { - auto dtype = dtype_of_field_type(f.second); - field_types_res.emplace(f.first, dtype); - } - return field_types_res; + return ouster::get_field_types(profile); }, R"( Extracts LidarScan fields with types for a given ``udp_profile_lidar`` @@ -1538,14 +1785,17 @@ PYBIND11_MODULE(_client, m) { )", py::arg("udp_profile_lidar")); - py::class_(m, "_Packet") + py::class_(m, "Packet") .def(py::init(), py::arg("size") = 65536) // direct access to timestamp field - .def_readwrite("_host_timestamp", &Packet::host_timestamp) - // access via seconds + .def_readwrite("host_timestamp", &Packet::host_timestamp) + // access via seconds (deprecated) .def_property( "capture_timestamp", [](Packet& self) -> nonstd::optional { + ouster::sensor::logger().warn( + "Packet.capture_timestamp is deprecated, use " + "Packet.host_timestamp instead."); if (self.host_timestamp) { return self.host_timestamp / 1e9; } else { @@ -1553,6 +1803,9 @@ PYBIND11_MODULE(_client, m) { } }, [](Packet& self, nonstd::optional v) { + ouster::sensor::logger().warn( + "Packet.capture_timestamp is deprecated, use " + "Packet.host_timestamp instead."); if (v) { self.host_timestamp = static_cast(*v * 1e9); } else { @@ -1561,7 +1814,7 @@ PYBIND11_MODULE(_client, m) { }) // NOTE: returned array is writeable, but not reassignable .def_property_readonly( - "_data", + "buf", // we have to use cpp_function here because py::keep_alive // does not work with pybind11 def_property methods due to a bug: // https://github.com/pybind/pybind11/issues/4236 @@ -1572,21 +1825,34 @@ PYBIND11_MODULE(_client, m) { }, py::keep_alive<0, 1>())); - py::class_(m, "_LidarPacket") - .def(py::init(), py::arg("size") = 65536); + py::enum_(m, "PacketValidationFailure", + py::arithmetic()) + .value("NONE", sensor::PacketValidationFailure::NONE) + .value("PACKET_SIZE", sensor::PacketValidationFailure::PACKET_SIZE) + .value("ID", sensor::PacketValidationFailure::ID); - py::class_(m, "_ImuPacket") - .def(py::init(), py::arg("size") = 65536); + py::class_(m, "LidarPacket") + .def(py::init(), py::arg("size") = 65536) + .def("__copy__", + [](const LidarPacket& self) { return LidarPacket(self); }) + .def("__deepcopy__", [](const LidarPacket& self, + py::dict) { return LidarPacket(self); }) + .def("validate", &LidarPacket::validate); + + py::class_(m, "ImuPacket") + .def(py::init(), py::arg("size") = 65536) + .def("__copy__", [](const ImuPacket& self) { return ImuPacket(self); }) + .def("__deepcopy__", + [](const ImuPacket& self, py::dict) { return ImuPacket(self); }) + .def("validate", &ImuPacket::validate); using ouster::sensor::impl::FieldInfo; py::class_(m, "FieldInfo") - .def("__init__", - [](FieldInfo& self, py::object dt, size_t offset, uint64_t mask, - int shift) { - new (&self) - FieldInfo{field_type_of_dtype(py::dtype::from_args(dt)), - offset, mask, shift}; - }) + .def(py::init([](py::object dt, size_t offset, uint64_t mask, + int shift) { + return new FieldInfo{field_type_of_dtype(py::dtype::from_args(dt)), + offset, mask, shift}; + })) .def_property_readonly("ty_tag", [](const FieldInfo& self) { return dtype_of_field_type(self.ty_tag); diff --git a/python/src/cpp/_osf.cpp b/python/src/cpp/_osf.cpp index 1b8c6b3f..d69dd6d7 100644 --- a/python/src/cpp/_osf.cpp +++ b/python/src/cpp/_osf.cpp @@ -55,27 +55,6 @@ static sensor::ChanFieldType field_type_of_dtype(const py::dtype& dt) { throw std::invalid_argument("Invalid dtype for a channel field"); } -/* - * NOTE: Duped with _client.cpp - * Map a channel field type to a dtype - */ -static py::dtype dtype_of_field_type(const sensor::ChanFieldType& ftype) { - switch (ftype) { - case sensor::ChanFieldType::UINT8: - return py::dtype::of(); - case sensor::ChanFieldType::UINT16: - return py::dtype::of(); - case sensor::ChanFieldType::UINT32: - return py::dtype::of(); - case sensor::ChanFieldType::UINT64: - return py::dtype::of(); - default: - throw std::invalid_argument( - "Invalid field_type for convertion to dtype"); - } - return py::dtype(); // unreachable ... -} - PYBIND11_MODULE(_osf, m) { m.doc() = R"doc( Ouster OSF Python API generated by pybind11. @@ -520,16 +499,7 @@ to work with OSF files. return osf::metadata_type(); }) .def_property_readonly("sensor_meta_id", - &osf::LidarScanStreamMeta::sensor_meta_id) - .def_property_readonly( - "field_types", [](const osf::LidarScanStreamMeta& lsm) { - std::map field_types_res{}; - for (const auto& f : lsm.field_types()) { - auto dtype = dtype_of_field_type(f.second); - field_types_res.emplace(f.first, dtype); - } - return field_types_res; - }); + &osf::LidarScanStreamMeta::sensor_meta_id); // LidarScanStream py::class_(m, "LidarScanStream", R"( @@ -648,23 +618,17 @@ to work with OSF files. Default ``chunk_size`` is ``2 MB``. )") - .def( - py::init( - [](const std::string& filename, const sensor::sensor_info& info, - const std::map& field_types, - uint32_t chunk_size) { - LidarScanFieldTypes ft{}; - for (const auto& f : field_types) { - auto dtype = py::dtype::from_args(f.second); - ft.push_back(std::make_pair( - f.first, field_type_of_dtype(dtype))); - } - return new osf::Writer(filename, info, ft, chunk_size); - }), - py::arg("filename"), py::arg("info"), - py::arg("field_types") = std::map{}, - py::arg("chunk_size") = 0, - R"( + .def(py::init([](const std::string& filename, + const sensor::sensor_info& info, + const std::vector& fields_to_write, + uint32_t chunk_size) { + return new osf::Writer(filename, info, fields_to_write, + chunk_size); + }), + py::arg("filename"), py::arg("info"), + py::arg("fields_to_write") = std::vector{}, + py::arg("chunk_size") = 0, + R"( Creates a `Writer` with deafault ``STREAMING`` layout chunks writer. Using default ``chunk_size`` of ``2MB``. @@ -684,26 +648,20 @@ to work with OSF files. means more messages are indexed and a larger number of index entries. A more granular index allows for more precise seeking at the slight expense of a larger file. - field_types (Dict[ChanField, FieldDType]): The fields from scans to + fields_to_write (List[str]): The fields from scans to actually save into the OSF. If not provided uses the fields from the first saved lidar scan for each stream. This parameter is optional. )") - .def(py::init( - [](const std::string& filename, - const std::vector& info, - const std::map& field_types, - uint32_t chunk_size) { - LidarScanFieldTypes ft{}; - for (const auto& f : field_types) { - auto dtype = py::dtype::from_args(f.second); - ft.push_back(std::make_pair( - f.first, field_type_of_dtype(dtype))); - } - return new osf::Writer(filename, info, ft, chunk_size); - }), + .def(py::init([](const std::string& filename, + const std::vector& info, + const std::vector& fields_to_write, + uint32_t chunk_size) { + return new osf::Writer(filename, info, fields_to_write, + chunk_size); + }), py::arg("filename"), py::arg("info"), - py::arg("field_types") = std::map{}, + py::arg("fields_to_write") = std::vector{}, py::arg("chunk_size") = 0, R"( Creates a `Writer` with specified ``chunk_size``. @@ -714,7 +672,7 @@ to work with OSF files. filename (str): The filename to output to. info (List[sensor_info]): The sensor info vector to use for a multi stream OSF file. - field_types (Dict[ChanField, FieldDType]): The fields from scans to + fields_to_write (List[str]): The fields from scans to actually save into the OSF. If not provided uses the fields from the first saved lidar scan for each stream. This parameter is optional. chunk_size (int): The chunksize to use for the OSF file, this arg @@ -834,23 +792,17 @@ to work with OSF files. .def( "add_sensor", [](osf::Writer& writer, const sensor::sensor_info& info, - const std::map& field_types) { - LidarScanFieldTypes ft{}; - for (const auto& f : field_types) { - auto dtype = py::dtype::from_args(f.second); - ft.push_back( - std::make_pair(f.first, field_type_of_dtype(dtype))); - } - return writer.add_sensor(info, ft); + const std::vector& fields_to_write) { + return writer.add_sensor(info, fields_to_write); }, py::arg("info"), - py::arg("field_types") = std::map{}, + py::arg("fields_to_write") = std::vector{}, R"( Add a sensor to the OSF file. Args: info (sensor_info): Sensor to add. - field_types (Dict[ChanField, FieldDType]): The fields from scans to + fields_to_write (List[str]): The fields from scans to actually save into the OSF. If not provided uses the fields from the first saved lidar scan for each stream. This parameter is optional. @@ -959,12 +911,11 @@ to work with OSF files. m.def( "slice_and_cast", [](const LidarScan& ls, - const std::map& field_types) { - LidarScanFieldTypes ft{}; + const std::map& field_types) { + ouster::LidarScanFieldTypes ft{}; for (const auto& f : field_types) { auto dtype = py::dtype::from_args(f.second); - ft.push_back( - std::make_pair(f.first, field_type_of_dtype(dtype))); + ft.push_back(FieldType(f.first, field_type_of_dtype(dtype))); } return ouster::osf::slice_with_cast(ls, ft); }, diff --git a/python/src/cpp/_viz.cpp b/python/src/cpp/_viz.cpp index 95926773..90912ad4 100644 --- a/python/src/cpp/_viz.cpp +++ b/python/src/cpp/_viz.cpp @@ -337,18 +337,16 @@ PYBIND11_MODULE(_viz, m) { We also keep track of a per-cloud pose to efficiently transform the whole point cloud without having to update all ~2048 poses. )") - .def( - "__init__", - [](viz::Cloud& self, size_t n, pymatrixd extrinsics) { - // expect homogeneous 4x4 pose matrix in 2d or 1d shape - check_array(extrinsics, 16, 0, 'F'); - viz::mat4d extrinsica; - std::copy(extrinsics.data(), extrinsics.data() + 16, - extrinsica.data()); - new (&self) viz::Cloud(n, extrinsica); - }, - py::arg("n"), py::arg("extrinsics") = viz::identity4d, - R"( + .def(py::init([](size_t n, pymatrixd extrinsics) { + // expect homogeneous 4x4 pose matrix in 2d or 1d shape + check_array(extrinsics, 16, 0, 'F'); + viz::mat4d extrinsica; + std::copy(extrinsics.data(), extrinsics.data() + 16, + extrinsica.data()); + return new viz::Cloud(n, extrinsica); + }), + py::arg("n"), py::arg("extrinsics") = viz::identity4d, + R"( ``def __init__(self, n_points: int, extrinsics: np.ndarray) -> None:`` Unstructured point cloud for visualization. @@ -360,28 +358,26 @@ PYBIND11_MODULE(_viz, m) { extrinsics: sensor extrinsic calibration. 4x4 column-major homogeneous transformation matrix )") - .def( - "__init__", - [](viz::Cloud& self, const sensor::sensor_info& info) { - const auto xyz_lut = make_xyz_lut(info); - - // make_xyz_lut still outputs doubles - Eigen::Array direction = - xyz_lut.direction.cast(); - Eigen::Array offset = - xyz_lut.offset.cast(); - - viz::mat4d extrinsica; - std::copy(info.extrinsic.data(), info.extrinsic.data() + 16, - extrinsica.data()); - - new (&self) - viz::Cloud{info.format.columns_per_frame, - info.format.pixels_per_column, direction.data(), - offset.data(), extrinsica}; - }, - py::arg("metadata"), - R"( + .def(py::init([](const sensor::sensor_info& info) { + const auto xyz_lut = make_xyz_lut(info); + + // make_xyz_lut still outputs doubles + Eigen::Array direction = + xyz_lut.direction.cast(); + Eigen::Array offset = + xyz_lut.offset.cast(); + + viz::mat4d extrinsica; + std::copy(info.extrinsic.data(), info.extrinsic.data() + 16, + extrinsica.data()); + + return new viz::Cloud{info.format.columns_per_frame, + info.format.pixels_per_column, + direction.data(), offset.data(), + extrinsica}; + }), + py::arg("metadata"), + R"( ``def __init__(self, si: SensorInfo) -> None:`` Structured point cloud for visualization. @@ -691,20 +687,18 @@ PYBIND11_MODULE(_viz, m) { py::class_>( m, "Cuboid", "Manages the state of a single cuboid.") - .def( - "__init__", - [](viz::Cuboid& self, pymatrixd pose, py::tuple rgba) { - check_array(pose, 16, 2, 'F'); - viz::mat4d posea; - std::copy(pose.data(), pose.data() + 16, posea.data()); + .def(py::init([](pymatrixd pose, py::tuple rgba) { + check_array(pose, 16, 2, 'F'); + viz::mat4d posea; + std::copy(pose.data(), pose.data() + 16, posea.data()); - viz::vec4f ar{0.0, 0.0, 0.0, 1.0}; - tuple_to_float_array(ar, rgba); + viz::vec4f ar{0.0, 0.0, 0.0, 1.0}; + tuple_to_float_array(ar, rgba); - new (&self) viz::Cuboid{posea, ar}; - }, - py::arg("pose"), py::arg("rgba"), - R"( + return new viz::Cuboid{posea, ar}; + }), + py::arg("pose"), py::arg("rgba"), + R"( Creates cuboid. Args: @@ -746,11 +740,9 @@ PYBIND11_MODULE(_viz, m) { py::class_>( m, "Label", "Manages the state of a text label.") .def( - "__init__", - [](viz::Label& self, const std::string& text, double x, double y, - double z) { - new (&self) viz::Label{text, {x, y, z}}; - }, + py::init([](const std::string& text, double x, double y, double z) { + return new viz::Label{text, {x, y, z}}; + }), py::arg("text"), py::arg("x"), py::arg("y"), py::arg("z"), R"( ``def __init__(self, text: str, x: float, y: float, z: float) -> None:`` diff --git a/python/src/ouster/cli/core/__init__.py b/python/src/ouster/cli/core/__init__.py index 007024ca..2096a4da 100644 --- a/python/src/ouster/cli/core/__init__.py +++ b/python/src/ouster/cli/core/__init__.py @@ -18,12 +18,13 @@ from .util import util_group +from threadpoolctl import threadpool_limits + this_package_name = 'ouster-sdk' APP_NAME = 'ouster' TRACEBACK = False TRACEBACK_FLAG = '--traceback' - logger = logging.getLogger("cli-args-logger") @@ -173,6 +174,12 @@ def find_plugins(show_traceback: bool = False): def run(args=None) -> None: exit_code = None + # Set openblas and friends to only use 1 thread to avoid threadpool busywaiting issues + # The busywaiting in openblas was taking a viz from less than a half a core to 3 + # The small matrices/vectors we are using with BLAS are generally not conducive to + # multithreading anyways + threadpool_limits(limits=1, user_api='blas') + if platform.system() == "Windows": client_log_dir = os.getenv("LOCALAPPDATA") diff --git a/python/src/ouster/cli/core/util.py b/python/src/ouster/cli/core/util.py index ff1bf9b9..edd74a80 100644 --- a/python/src/ouster/cli/core/util.py +++ b/python/src/ouster/cli/core/util.py @@ -154,7 +154,7 @@ def md5file(path: str) -> str: Please verify that ROS Python modules are available. The best option is to try to install unofficial rospy packages that work -with Python 3.8 on Ubuntu 18.04/20.04 and Debian 10 without ROS: +with Python 3.8 on Ubuntu 20.04 and Debian 10 without ROS: pip install --extra-index-url https://rospypi.github.io/simple/ rospy rosbag tf2_ros @@ -328,28 +328,6 @@ def report_dur(key: str, dur: float): f.write(json.dumps(report, indent=4)) -@util_group.command() -@click.option('-o', 'output_path', required=False, help='Name of output file') -@click.argument('meta', required=True, type=click.Path(exists=True)) -def convert_metadata_to_legacy(meta: str, output_path: Optional[str]) -> None: - """Convert nonlegacy metadata to legacy format - - The supplied metadata should be of nonlegacy form. Output will print to screen. - Legacy metadata will error out by complaining it is an invalid non-legacy format. - - """ - - with open(meta) as json: - output = client.convert_to_legacy(json.read()) - if output_path is None: - click.echo(output) - else: - click.echo(f"Reading metadata from: {meta} and outputting converted " - f"legacy metadata to: {output_path}") - with open(output_path, "w") as outfile: - outfile.write(output) - - @util_group.command(name="benchmark-sensor") @click.argument('hostname', required=True, type=str) @click.option('-l', '--lidar-port', type=int, default=None, help="Lidar port") @@ -512,7 +490,7 @@ def benchmark_sensor(hostname: str, lidar_port: Optional[int], data_source = packet_source is_scan_source = False - frame_bound = [client.FrameBorder() for _ in data_source.metadata] + frame_bound = [client.FrameBorder(m) for m in data_source.metadata] now = time.monotonic() last_scan_ts = [now for _ in data_source.metadata] diff --git a/python/src/ouster/cli/plugins/source.py b/python/src/ouster/cli/plugins/source.py index 079b9dce..9f73a918 100644 --- a/python/src/ouster/cli/plugins/source.py +++ b/python/src/ouster/cli/plugins/source.py @@ -1,4 +1,5 @@ # type: ignore +import atexit import click import re import threading @@ -8,6 +9,7 @@ from ouster.cli.core.cli_args import CliArgs from ouster.cli.core.util import click_ro_file from ouster.sdk import open_source +from ouster.sdk.client import first_valid_packet_ts from ouster.sdk.client.core import ClientTimeout from ouster.sdk.io_type import (extension_from_io_type, io_type, OusterIoType) import ouster.cli.plugins.source_pcap as pcap_cli @@ -22,7 +24,6 @@ source_multicommand, _join_with_conjunction) - _source_arg_name: str = 'source' @@ -34,47 +35,51 @@ '--rate', default="1", help="Playback rate.", - type=click.Choice(["0.25", "0.5", "0.75", "1", "1.5", "2", "3"])) + type=click.Choice(["0.25", "0.5", "0.75", "1", "1.5", "2", "3", "max"])) @click.option("--pause-at", default=-1, help="Lidar Scan number to pause at") @click.option("--accum-num", - default=0, - help="Integer number of scans to accumulate") -@click.option("--accum-num", - default=0, - help="Integer number of scans to accumulate") + default=None, + type=int, + help="Accumulate up to this number of past scans for visualization. " + "Use <= 0 for unlimited. Defaults to 100 if --accum-every or --accum-every-m is set.") @click.option("--accum-every", default=None, - type=float, - help="Accumulate every Nth scan") + type=int, + help="Add a new scan to the accumulator every this number of scans.") @click.option("--accum-every-m", default=None, type=float, - help="Accumulate scan every M meters traveled") -@click.option("--accum-map", + help="Add a new scan to the accumulator after this many meters of travel.") +@click.option("--map", "_map", is_flag=True, - help="Enable the overall map accumulation mode") -@click.option("--accum-map-ratio", - default=0.001, - help="Ratio of random points of every scan to add to an overall map") + help="If set, add random points from every scan into an overall map for visualization. " + "Enabled if either --map-ratio or --map-size are set.") +@click.option("--map-ratio", + default=None, type=float, + help="Fraction of random points in every scan to add to overall map (0, 1]. [default: 0.01]") +@click.option("--map-size", + default=None, type=int, + help="Maximum number of points in overall map before discarding. [default: 1500000]") @click.pass_context @source_multicommand(type=SourceCommandType.CONSUMER) -def source_viz(ctx: SourceCommandContext, pause: bool, on_eof: str, pause_at: int, accum_num: int, +def source_viz(ctx: SourceCommandContext, pause: bool, on_eof: str, pause_at: int, accum_num: Optional[int], accum_every: Optional[int], accum_every_m: Optional[float], - accum_map: bool, accum_map_ratio: float, rate: str) -> SourceCommandCallback: + _map: bool, map_ratio: float, map_size: int, rate: str) -> SourceCommandCallback: """Visualize LidarScans in a 3D viewer.""" try: - from ouster.sdk.viz import SimpleViz, scans_accum_for_cli + from ouster.sdk.viz import SimpleViz, ScansAccumulator except ImportError as e: raise click.ClickException(str(e)) + source = ctx.scan_source + from ouster.sdk.client import ScanSourceAdapter + if isinstance(source, ScanSourceAdapter): + source = source._scan_source + # ugly workarounds ensue if on_eof == 'loop': - source = ctx.scan_source - from ouster.sdk.client import ScanSourceAdapter - if isinstance(source, ScanSourceAdapter): - source = source._scan_source # NOTE: setting it here instead of at open_source stage because we do not want to propagate # the flag up to `source` command source._cycle = True @@ -82,19 +87,62 @@ def source_viz(ctx: SourceCommandContext, pause: bool, on_eof: str, pause_at: in if pause and pause_at == -1: pause_at = 0 + # Determine how to set the rate + if rate == "max": + rate = 0.0 + else: + rate = float(rate) + if source.is_live: + if rate != 1.0: + raise click.exceptions.UsageError("Can only set a rate of 1 for live sources") + rate = None + ctx.scan_iter, scans = CoupledTee.tee(ctx.scan_iter, terminate=ctx.terminate_evt) metadata = ctx.scan_source.metadata - scans_accum = scans_accum_for_cli(metadata, - accum_num=accum_num, - accum_every=accum_every, - accum_every_m=accum_every_m, - accum_map=accum_map, - accum_map_ratio=accum_map_ratio) + + # build the accumulator + if accum_every_m is not None and accum_every is not None: + raise click.exceptions.UsageError("Can only provide one of --accum-every and --accum-every-m") + + if accum_num is not None and accum_num <= 0: + accum_num = 1000000 + + if accum_every_m is not None or accum_every is not None: + if accum_num is None: + accum_num = 100 + elif accum_num is None: + accum_num = 0 + + if accum_every_m is not None: + accum_every = 0 + elif accum_every is not None: + accum_every_m = 0.0 + else: + accum_every = 0 if accum_num is None else 1 + accum_every_m = 0.0 + + if map_ratio is not None or map_size is not None: + _map = True + map_ratio = 0.01 if map_ratio is None else map_ratio + map_size = 1500000 if map_size is None else map_size + + if map_ratio > 1.0 or map_ratio <= 0.0: + raise click.exceptions.UsageError("--map-ratio must be in the range (0, 1]") + if map_size <= 0: + raise click.exceptions.UsageError("--map-size must be greater than 0") + + scans_accum = ScansAccumulator(metadata, + accum_max_num=accum_num, + accum_min_dist_num=accum_every, + accum_min_dist_meters=accum_every_m, + map_enabled=_map, + map_select_ratio=map_ratio, + map_max_points=map_size) def viz_thread_fn(): sv = SimpleViz(metadata, scans_accum=scans_accum, - rate=float(rate), pause_at=pause_at, on_eof=on_eof) + rate=rate, pause_at=pause_at, on_eof=on_eof) sv.run(scans) ctx.terminate_evt.set() @@ -108,6 +156,21 @@ def extract_slice_indices(click_ctx: Optional[click.core.Context], param: Optional[click.core.Argument], value: str): """Validate and extract slice indices of the form [start]:[stop][:step].""" index_matches = re.findall(r"^(-?\d*):(-?\d*):?(-?\d*)$", value) # noqa: W605 + time_index_matches = re.findall(r"^(?:(\d+(?:\.\d+)?)(h|min|s|ms))?" + r":(?:(\d+(?:\.\d+)?)(h|min|s|ms))?(?::(-?\d*))?$", value) # noqa: W605 + + if time_index_matches and len(time_index_matches[0]) == 5: + # it was a time index, convert everything to float seconds and handle units + match = time_index_matches[0] + multipliers = {} + multipliers['h'] = 60.0 * 60.0 + multipliers['min'] = 60.0 + multipliers['s'] = 1.0 + multipliers['ms'] = 0.001 + start = float(match[0]) * multipliers[match[1]] if match[0] != "" else None + end = float(match[2]) * multipliers[match[3]] if match[2] != "" else None + skip = int(match[4]) if match[4] != "" else None + return start, end, skip if not index_matches or len(index_matches[0]) != 3: raise click.exceptions.BadParameter( @@ -136,9 +199,97 @@ def extract_slice_indices(click_ctx: Optional[click.core.Context], @source_multicommand(type=SourceCommandType.PROCESSOR) def source_slice(ctx: SourceCommandContext, indices: Tuple[Optional[int]]) -> SourceCommandCallback: - """Slice LidarScans streamed from SOURCE. Use the form [start]:[stop][:step].""" + """Slice LidarScans streamed from SOURCE. Use the form [start]:[stop][:step]. + Optionally can specify start and stop as times relative to the start of the file + using the units h (hours), min (minutes), s (seconds), or ms (milliseconds). + For example: 10s:20s:2""" start, stop, step = indices - ctx.scan_iter = islice(ctx.scan_iter, start, stop, step) + if type(start) is float: + scans = ctx.scan_iter + + def iter(): + start_time = None + counter = 0 + for scan in scans: + scan_time = first_valid_packet_ts(scan) + if scan_time == 0 or scan_time is None: + print("WARNING: Scan missing packet timestamps. Yielding scan in time slice anyways.") + yield scan + continue + scan_time = scan_time / 1e9 + if start_time is None: + start_time = scan_time + dt = scan_time - start_time + if dt >= start: + if not stop or dt <= stop: + if not step or counter % step == 0: + yield scan + counter = counter + 1 + else: + return + + ctx.scan_iter = iter() + else: + ctx.scan_iter = islice(ctx.scan_iter, start, stop, step) + + +@click.command() +@click.option("-v", "--verbose", is_flag=True, help="Print out additional stats info.") +@click.pass_context +@source_multicommand(type=SourceCommandType.CONSUMER) +def source_stats(ctx: SourceCommandContext, verbose: bool) -> SourceCommandCallback: + """Calculate and output various statistics about the scans at this point in the pipeline.""" + window = ctx.scan_source.metadata.format.column_window + scans = ctx.scan_iter + count = 0 + incomplete_count = 0 + start = None + end = None + incomplete_scans = [] + dimensions = {} + + def stats_iter(): + nonlocal count, incomplete_count, window, verbose, start, end, incomplete_scans, dimensions + ns_to_sec = 1.0 / 1000000000.0 + for scan in scans: + time = first_valid_packet_ts(scan) + if time != 0.0: + if start is None or time < start: + start = time + if end is None or time > end: + end = time + dimensions[(scan.w, scan.h)] = True + if not scan.complete(window): + incomplete_count = incomplete_count + 1 + if verbose: + incomplete_scans.append(f" #{count} at {time * ns_to_sec}") + count = count + 1 + yield scan + ctx.scan_iter = stats_iter() + + def exit_handler(): + nonlocal count, incomplete_count, start, end, incomplete_scans, dimensions + ns_to_sec = 1.0 / 1000000000.0 + print("Scan Statistics:") + print(f" Count: {count}") + dstring = "" + for k in dimensions: + dstring = dstring + f" {k[0]}x{k[1]}" + print(f" Sizes:{dstring}") + if start is None: + print(" First Time: No Valid Timestamps") + print(" Last Time: No Valid Timestamps") + print(" Duration: Unknown") + else: + print(f" First Time: {start * ns_to_sec}") + print(f" Last Time: {end * ns_to_sec}") + print(f" Duration: {(end - start) * ns_to_sec} seconds") + print(f" Incomplete Scans: {incomplete_count}") + if verbose: + for i in incomplete_scans: + print(i) + + atexit.register(exit_handler) class SourceMultiCommand(click.MultiCommand): @@ -157,12 +308,14 @@ def __init__(self, *args, **kwargs): 'ANY': { 'viz': source_viz, 'slice': source_slice, + 'stats': source_stats }, OusterIoType.SENSOR: { 'config': sensor_cli.sensor_config, 'metadata': sensor_cli.sensor_metadata, 'save': SourceSaveCommand('save', context_settings=dict(ignore_unknown_options=True, allow_extra_args=True)), + 'userdata': sensor_cli.sensor_userdata, }, OusterIoType.PCAP: { 'info': pcap_cli.pcap_info, diff --git a/python/src/ouster/cli/plugins/source_mapping.py b/python/src/ouster/cli/plugins/source_mapping.py new file mode 100644 index 00000000..9f2c8db5 --- /dev/null +++ b/python/src/ouster/cli/plugins/source_mapping.py @@ -0,0 +1,430 @@ +import os +import sys +import click +import logging +import laspy +import numpy as np +import ouster.sdk.util.pose_util as pu +from ouster.sdk.io_type import OusterIoType +from ouster.cli.plugins.source import source # type: ignore +from ouster.sdk.util import default_scan_fields +from ouster.cli.plugins.source_save import (SourceSaveCommand, + determine_filename, + create_directories_if_missing, + _file_exists_error) +from ouster.sdk.client import (LidarScan, + ChanField, + UDPProfileLidar, + XYZLut, + first_valid_column_ts, + first_valid_column_pose) +from ouster.cli.plugins.source_util import (source_multicommand, + SourceCommandType, + SourceCommandContext) + + +logging.basicConfig(level=logging.INFO) +logger = logging.getLogger('mapping') +_max_range = None +_min_range = None + + +@click.command +@click.option('--max-range', required=False, type=float, + default=None, help="Discard points further than this distance in meters") +@click.option('--min-range', required=False, type=float, + default=None, help="Discard points closer that this distance in meters") +@click.option('--percent-range', required=False, type=float, + default=None, help="Discards points with a range greater than this " + "percentile of ranges in the scan") +@click.pass_context +@source_multicommand(type=SourceCommandType.PROCESSOR) +def source_clip(ctx: SourceCommandContext, max_range: float, + min_range: float, percent_range, **kwargs) -> None: + """ + Clip points outside of a specified range. By default uses min/max ranges used + by the preceding slam command if present. + """ + + min_range = _min_range if min_range is None else min_range + max_range = _max_range if max_range is None else max_range + + if min_range is None and max_range is None and percent_range is None: + raise click.exceptions.UsageError("You must provide --max-range, --min-range, or" + "--range-percent to clip.") + + if percent_range is not None and (percent_range <= 0 or percent_range > 100): + raise click.exceptions.UsageError( + f"Expected 'percent_range' value to be between (0, 100], but received {percent_range}") + + low_range = min_range * 1000 if min_range else 0 + high_range = max_range * 1000 if max_range else float('inf') + + dual = ctx.scan_source.metadata.format.udp_profile_lidar in [ # type: ignore + UDPProfileLidar.PROFILE_LIDAR_RNG19_RFL8_SIG16_NIR16_DUAL, # type: ignore + UDPProfileLidar.PROFILE_LIDAR_FUSA_RNG15_RFL8_NIR8_DUAL] # type: ignore + + scans = ctx.scan_iter + + def clip_iter(): + for scan in scans: + copy = LidarScan(scan) + + range1 = copy.field(ChanField.RANGE) + + if percent_range and percent_range != 100: + nonlocal high_range + non_zero_range1 = range1[range1 != 0] # Filter out zeros + percentile_range = np.percentile(non_zero_range1, percent_range) + high_range = min(high_range, percentile_range) + + range1[(range1 < low_range) | (range1 > high_range)] = 0 + + if dual: + range2 = copy.field(ChanField.RANGE2) + range2[(range2 < low_range) | (range2 > high_range)] = 0 + + yield copy + + ctx.scan_iter = clip_iter() + + +@click.command +@click.option('--max-range', required=False, + default=150.0, help="Max valid range") +@click.option('--min-range', required=False, + default=1.0, help="Min valid range") +@click.option('-v', '--voxel-size', required=False, + type=float, help="Voxel map size") +@click.pass_context +@source_multicommand(type=SourceCommandType.PROCESSOR) +def source_slam(ctx: SourceCommandContext, max_range: float, min_range: float, + voxel_size: float, **kwargs) -> None: + """ + Run SLAM with a SOURCE.\n + + Example values for voxel_size:\n + Outdoor: 0.8 - 1.5\n + Large indoor: 0.4 - 0.8\n + Small indoor: 0.1 - 0.5\n + If voxel_size is not specifiied, the algorithm will use the first lidar scan to calculate it.\n + Small voxel size could give more accurate results but take more memory and + longer processing. For real-time slam, considing using a slightly larger voxel size + and use visualizer to monitor the SLAM process. + """ + + global _max_range, _min_range + _max_range = max_range + _min_range = min_range + + def make_kiss_slam(): + try: + from ouster.sdk.mapping.slam import KissBackend + except ImportError as e: + raise click.ClickException("kiss-icp, a package required for slam, is " + "unsupported on this platform. Error: " + str(e)) + + return KissBackend(info=ctx.scan_source.metadata, + max_range=max_range, + min_range=min_range, + voxel_size=voxel_size, + live_stream=ctx.scan_source.is_live) + + try: + slam_engine = make_kiss_slam() + except (ValueError, click.ClickException) as e: + logger.error(str(e)) + return + + def slam_iter(scan_source, slam_engine): + scan_start_ts = None + for scan in scan_source: + scan_ts = first_valid_column_ts(scan) + if scan_ts == scan_start_ts: + logger.info("SLAM restarts as scan iteration restarts") + slam_engine = make_kiss_slam() + if not scan_start_ts: + scan_start_ts = scan_ts + scan_slam = slam_engine.update(scan) + yield scan_slam + + ctx.scan_iter = slam_iter(ctx.scan_iter, slam_engine) + + +@click.command(context_settings=dict( + ignore_unknown_options=True, + allow_extra_args=True, +)) +@click.argument("filename", required=True) +@click.option('-p', '--prefix', default="", help="Output prefix.") +@click.option('-d', '--dir', default="", help="Output directory.") +@click.option('-s', '--voxel-size', default=0.1, help="Voxel map size for downsampling" + "This parameter is the same as the open3D voxel size. Default value is 0.1. " + "The bigger the value, the fewer points it outputs") +@click.option('--field', + required=False, + type=click.Choice(['SIGNAL', + 'NEAR_IR', + 'REFLECTIVITY'], + case_sensitive=False), + default="REFLECTIVITY", + help="Chanfield for output file key value. Choose between SIGNAL, NEAR_IR, " + "REFLECTIVITY. Default field is REFLECTIVITY") +@click.option('--decimate', required=False, type=bool, + default=True, help="Downsample the point cloud to output. Default is On") +@click.option('--verbose', is_flag=True, default=False, + help="Print point cloud status much frequently. Default is Off") +@click.option('--overwrite', is_flag=True, default=False, help="If true, overwrite " + "existing files with the same name") +@click.pass_context +@source_multicommand(type=SourceCommandType.CONSUMER) +def point_cloud_convert(ctx: SourceCommandContext, filename: str, prefix: str, + dir: str, voxel_size: float, field: str, decimate: bool, + overwrite: bool, verbose: bool, **kwargs) -> None: + + pcu_installed = False + if decimate: + try: + import point_cloud_utils as pcu # type: ignore + pcu_installed = True + except ImportError: + logger.warning("The point_cloud_utils library is not supported or installed," + "so the process will generate larger, non-downsampled files") + + scans = ctx.scan_iter + info = ctx.scan_source.metadata # type: ignore + + outfile_ext = "." + kwargs["format"] + output_file_path = determine_filename(prefix, dir, filename, + outfile_ext, info) + create_directories_if_missing(output_file_path) + + # files pre exist check step + file_wo_ext, outfile_ext = os.path.splitext(output_file_path) + may_existed_file = file_wo_ext + '-000' + outfile_ext + + if (os.path.isfile(output_file_path) or os.path.isfile(may_existed_file)) and not overwrite: + print(_file_exists_error(f'{output_file_path} or {may_existed_file}')) + exit(1) + + def convert_iter(): + points_to_process = np.empty(shape=[0, 3]) + keys_to_process = np.empty(shape=[0, 1]) + points_keys_total = np.empty(shape=[0, 4]) + + # hard-coded parameters and counters # + # affect the running time. smaller value mean longer running time. Too large + # may lead to crash + down_sample_steps = 100 + # affect per output file size. makes output file size ~1G + max_pnt_per_file = 10000000 + file_numb = 0 + + # variables for point cloud status printout + points_sum = 0 + points_saved = 0 + points_out_range = 0 + points_down_removed = 0 + + valid_fields = default_scan_fields( + info.format.udp_profile_lidar, flags=False) + + found = False + field_names = [] + for f in valid_fields: + if f.name == field: + found = True + field_names.append(f.name) + + if not found: + valid_fields_str = ", ".join(field_names) + + sys.exit(f"Exit! field {field} is not available in the low bandwidth mode\n" + f"use -f and choose a valid field from {valid_fields_str}") + + xyzlut = XYZLut(info, use_extrinsics=True) + + def process_points(points, keys): + nonlocal points_keys_total, points_down_removed, points_saved + + pts_size_before = points.shape[0] + + if pcu_installed and decimate: + # can be extended for RGBA values later + points, keys = pcu.downsample_point_cloud_on_voxel_grid( + voxel_size, points, keys) + keys = keys[:, np.newaxis] + + pts_size_after = points.shape[0] + + points_down_removed += pts_size_before - pts_size_after + points_saved += pts_size_after + + pts_keys_pair = np.append(points, keys, axis=1) + points_keys_total = np.append(points_keys_total, pts_keys_pair, axis=0) + + def save_file(file_wo_ext: str, outfile_ext: str): + nonlocal points_keys_total + logger.info(f"Output file: {file_wo_ext + outfile_ext}") + pc_status_print() + + if outfile_ext == ".ply": + # CloudCompare PLY color point cloud using the range 0-1 + with open(file_wo_ext + outfile_ext, 'w') as f: + # Write PLY header + f.write("ply\n") + f.write("format ascii 1.0\n") + f.write( + "element vertex {}\n".format( + points_keys_total.shape[0])) + f.write("property float x\n") + f.write("property float y\n") + f.write("property float z\n") + f.write(f"property float {field}\n") + f.write("end_header\n") + for i in range(points_keys_total.shape[0]): + f.write("{} {} {} {}\n".format( + points_keys_total[i, 0], points_keys_total[i, 1], + points_keys_total[i, 2], points_keys_total[i, 3] / 255)) + elif outfile_ext == ".las": + LAS_file = laspy.create() + LAS_file.x = points_keys_total[:, 0] + LAS_file.y = points_keys_total[:, 1] + LAS_file.z = points_keys_total[:, 2] + # LAS file only has intensity but we can use it for other field + # value + LAS_file.intensity = points_keys_total[:, 3] + LAS_file.write(file_wo_ext + outfile_ext) + logger.info(f"LAS format only supports the Intensity field. " + f"Saving {field} as the Intensity field.") + elif outfile_ext == ".pcd": + with open(file_wo_ext + outfile_ext, 'w') as f: + # Write PCD header + f.write(f"FIELDS x y z {field}\n") + f.write("SIZE 4 4 4 4\n") + f.write("TYPE F F F F\n") + f.write("COUNT 1 1 1 1\n") + f.write("WIDTH %d\n" % (points_keys_total.shape[0])) + f.write("HEIGHT 1\n") + f.write("POINTS %d\n" % (points_keys_total.shape[0])) + f.write("DATA ascii\n") + for i in range(points_keys_total.shape[0]): + f.write("{} {} {} {}\n".format( + points_keys_total[i, 0], points_keys_total[i, 1], + points_keys_total[i, 2], points_keys_total[i, 3])) + + points_keys_total = np.empty(shape=[0, 4]) + + def pc_status_print(): + nonlocal points_sum, points_down_removed, points_saved, points_out_range + down_removed_pct = (points_down_removed / points_sum) * 100 + out_range_pct = (points_out_range / points_sum) * 100 + save_pct = (points_saved / points_sum) * 100 + logger.info( + f"Point Cloud status info\n" + f"{points_sum} points accumulated during this period,\n{points_down_removed} " + f"down sampling points are removed [{down_removed_pct:.2f} %],\n{points_out_range} " + f"out range points are removed [{out_range_pct:.2f} %],\n{points_saved} points " + f"are saved [{save_pct:.2f} %].") + points_sum = 0 + points_out_range = 0 + points_saved = 0 + points_down_removed = 0 + + scan_start_ts = None + empty_pose = True + finish_saving = False + finish_saving_action = True + logger.info("Start processing...") + try: + for scan_idx, scan in enumerate(scans): + scan_ts = first_valid_column_ts(scan) + if scan_ts == scan_start_ts: + finish_saving = True + logger.info("Scan iteration restarts") + if finish_saving: + # Save point cloud and printout when scan iteration restarts + # This action only do once + if finish_saving_action: + process_points(points_to_process, keys_to_process) + save_file(f"{file_wo_ext}-{file_numb:03}", outfile_ext) + logger.info("Finished point cloud saving.") + finish_saving_action = False + yield scan + continue + if not scan_start_ts: + scan_start_ts = scan_ts + + # Pose attribute is per col global pose so we use identity for scan + # pose + column_poses = scan.pose + + if (empty_pose and column_poses.size > 0 + and not np.array_equal(first_valid_column_pose(scan), np.eye(4))): + empty_pose = False + + points = xyzlut(scan) + keys = scan.field(field) + + if scan_idx and scan_idx % 100 == 0: + logger.info(f"Processed {scan_idx} lidar scan") + + # to remove out range points + valid_row_index = scan.field(ChanField.RANGE) > 0 + out_range_row_index = scan.field(ChanField.RANGE) == 0 + dewarped_points = pu.dewarp(points, column_poses=column_poses) + filtered_points = dewarped_points[valid_row_index] + filtered_keys = keys[valid_row_index] + + curr_scan_points = dewarped_points.shape[0] * dewarped_points.shape[1] + points_sum += curr_scan_points + points_out_range += np.count_nonzero(out_range_row_index) + + # may not need below line + shaped_keys = filtered_keys.reshape(filtered_keys.shape[0], 1) + points_to_process = np.append(points_to_process, filtered_points, + axis=0) + keys_to_process = np.append(keys_to_process, shaped_keys, axis=0) + + # downsample the accumulated point clouds # + if scan_idx % down_sample_steps == 0: + process_points(points_to_process, keys_to_process) + points_to_process = np.empty(shape=[0, 3]) + keys_to_process = np.empty(shape=[0, 1]) + if verbose: + pc_status_print() + + # output a file to prevent crash due to oversize # + if points_keys_total.shape[0] >= max_pnt_per_file: + save_file(f"{file_wo_ext}-{file_numb:03}", outfile_ext) + file_numb += 1 + + yield scan + + except KeyboardInterrupt: + pass + + finally: + if empty_pose: + logger.info( + "Warning: Empty lidar scan pose in lidarscan stream!!!\n" + "Suggest: Append slam option to ouster-cli command or use a " + "SLAM output OSF file as an input.") + + # handle the last part of point cloud or the first part of point cloud if + # the size is less than down_sample_steps + if not finish_saving: + if keys_to_process.size > 0: + process_points(points_to_process, keys_to_process) + + save_file(f"{file_wo_ext}-{file_numb:03}", outfile_ext) + logger.info("Finished point cloud saving.") + + ctx.scan_iter = convert_iter() + + +source.commands['ANY']['slam'] = source_slam +source.commands['ANY']['clip'] = source_clip +SourceSaveCommand.implementations[OusterIoType.PCD] = point_cloud_convert +SourceSaveCommand.implementations[OusterIoType.LAS] = point_cloud_convert +SourceSaveCommand.implementations[OusterIoType.PLY] = point_cloud_convert diff --git a/python/src/ouster/cli/plugins/source_osf.py b/python/src/ouster/cli/plugins/source_osf.py index 47c50138..03dec853 100644 --- a/python/src/ouster/cli/plugins/source_osf.py +++ b/python/src/ouster/cli/plugins/source_osf.py @@ -69,7 +69,7 @@ def osf_metadata(ctx: SourceCommandContext, click_ctx: click.core.Context, n: in index = 0 for sensor_id, sensor_meta in msensors.items(): if index == n: - print(sensor_meta.info.original_string()) + print(sensor_meta.info.to_json_string()) return index = index + 1 @@ -96,7 +96,6 @@ def osf_info(ctx: SourceCommandContext, click_ctx: click.core.Context, except ImportError as e: raise click.ClickException("Error: " + str(e)) - from ouster.sdk.client._client import get_field_types from ouster.sdk.osf._osf import LidarScanStream import os @@ -153,15 +152,15 @@ def osf_info(ctx: SourceCommandContext, click_ctx: click.core.Context, obj["start"] = msg.ts obj["end"] = msg.ts obj["type"] = reader.meta_store[msg.id].type - obj["fields"] = get_field_types(ls) + obj["fields"] = ls.field_types obj["sensor"] = sensors[reader.meta_store[msg.id].sensor_meta_id] lidar_streams[msg.id] = obj else: obj = lidar_streams[msg.id] obj["count"] = obj["count"] + 1 obj["end"] = max(obj["end"], msg.ts) - if get_field_types(ls) != obj["fields"]: - print("WARNING: fields not equal!") + if obj["fields"] is not None and ls.field_types != obj["fields"]: + print("WARNING: fields changed for a sensor over the course of the file!") obj["fields"] = None print(f"Filename: {file}\nLayout: {orig_layout}") @@ -186,7 +185,7 @@ def osf_info(ctx: SourceCommandContext, click_ctx: click.core.Context, print(f" Duration: {end-start} seconds") print(f" Rate: {count/(end-start)} Hz") print(f" Product Line: {sensor.prod_line}") - print(f" Sensor Mode: {sensor.mode}") + print(f" Sensor Mode: {sensor.config.lidar_mode}") if verbose: print(f" Sensor SN: {sensor.sn}") print(f" Sensor FW Rev: {sensor.fw_rev}") @@ -195,7 +194,7 @@ def osf_info(ctx: SourceCommandContext, click_ctx: click.core.Context, print(" NO CONSISTENT FIELD TYPE") else: for f in stream["fields"]: - print(f" {f}:{stream['fields'][f]}") + print(f" {f}") for k in other_streams: stream = other_streams[k] @@ -244,7 +243,6 @@ def osf_parse(ctx: SourceCommandContext, click_ctx: click.core.Context, raise click.ClickException("Error: " + str(e)) # NOTE[pb]: Mypy quirks or some of our Python packages structure quirks, idk :( - from ouster.sdk.client._client import get_field_types from ouster.sdk.util.parsing import scan_to_packets, packets_to_scan, cut_raw32_words # type: ignore reader = osf.Reader(file) @@ -296,7 +294,7 @@ def proc_msgs(msgs: Iterator[osf.MessageRef]): packets = scan_to_packets(ls, sinfo) # recovered lidar scan - field_types = get_field_types(ls) + field_types = ls.field_types ls_rec = packets_to_scan( packets, sinfo, fields=field_types) diff --git a/python/src/ouster/cli/plugins/source_save.py b/python/src/ouster/cli/plugins/source_save.py index 46c84126..064a4906 100644 --- a/python/src/ouster/cli/plugins/source_save.py +++ b/python/src/ouster/cli/plugins/source_save.py @@ -67,7 +67,7 @@ def source_save_pcap(ctx: SourceCommandContext, prefix: str, dir: str, filename: # Save metadata as json with open(f"{filename}.json", 'w') as f: - f.write(info.updated_metadata_string()) + f.write(info.to_json_string()) if raw: scan_source = None @@ -89,10 +89,14 @@ def source_save_pcap(ctx: SourceCommandContext, prefix: str, dir: str, filename: f"{', '.join([c for c in ctx.invoked_command_names if c != 'save'])}.") # replace ScanSource's packetsource with RecordingPacketSource + if info.config.udp_port_imu is None or info.config.udp_port_lidar is None: + click.echo("Ports could not be found for the specified source.") + exit(1) + scan_source._source = RecordingPacketSource( scan_source._source, n_frames=None, prefix_path=filename, chunk_size=chunk_size, overwrite=overwrite, - lidar_port=info.udp_port_lidar, imu_port=info.udp_port_imu + lidar_port=info.config.udp_port_lidar, imu_port=info.config.udp_port_imu ) else: click.echo("Warning: Saving pcap without -r/--raw will not save LEGACY IMU packets.") @@ -110,10 +114,10 @@ def save_iter(): # [kk] TODO: implement chunk-size packets = scan_to_packets(scan, info) for packet in packets: - ts = packet.capture_timestamp or time.time() + ts = packet.host_timestamp / 1e9 if packet.host_timestamp else time.time() _pcap.record_packet(pcap_record_handle, "127.0.0.1", - "127.0.0.1", info.udp_port_lidar, - info.udp_port_lidar, packet._data, ts) + "127.0.0.1", info.config.udp_port_lidar, + info.config.udp_port_lidar, packet.buf, ts) yield scan except (KeyboardInterrupt, StopIteration): pass @@ -390,7 +394,8 @@ def determine_filename(prefix: str, dir: str, filename: str, extension: str, inf if filename != "": filename = str(outpath / f"{prefix}{filename}") else: - filename = str(outpath / f"{prefix}{info.prod_line}_{info.fw_rev}_{info.mode}_{time_str}{extension}") + filename = str(outpath / f"{prefix}{info.prod_line}_{info.fw_rev}_" + f"{info.config.lidar_mode}_{time_str}{extension}") return filename @@ -474,8 +479,8 @@ def save_iter(): for scan in scans: packets = scan_to_packets(scan, info) for packet in packets: - ts = rospy.Time.from_sec(packet.capture_timestamp) - msg = PacketMsg(buf=packet._data.tobytes()) + ts = rospy.Time.from_sec(packet.host_timestamp / 1e9) + msg = PacketMsg(buf=packet.buf.tobytes()) if isinstance(packet, LidarPacket): outbag.write(lidar_topic, msg, ts) elif isinstance(packet, ImuPacket): diff --git a/python/src/ouster/cli/plugins/source_sensor.py b/python/src/ouster/cli/plugins/source_sensor.py index 7edaf10b..9499dc9d 100644 --- a/python/src/ouster/cli/plugins/source_sensor.py +++ b/python/src/ouster/cli/plugins/source_sensor.py @@ -17,18 +17,31 @@ def sensor_group() -> None: @click.command -@click.option('--legacy/--non-legacy', - default=False, - help="Use legacy metadata format or not") @click.pass_context @source_multicommand(type=SourceCommandType.MULTICOMMAND_UNSUPPORTED, retrieve_click_context=True) -def sensor_metadata(ctx: SourceCommandContext, click_ctx: click.core.Context, - legacy: bool) -> None: +def sensor_metadata(ctx: SourceCommandContext, click_ctx: click.core.Context) -> None: """Display sensor metadata about the SOURCE.""" # Implements ouster-cli source metadata try: - click.echo(client.Sensor(ctx.source_uri, 7502, 7503, - _legacy_format=legacy)._fetched_meta) + click.echo(client.Sensor(ctx.source_uri, 7502, 7503)._fetched_meta) + except RuntimeError as e: + raise click.ClickException(str(e)) + + +@click.command +@click.option('-s', default=None, type=str, help='Set userdata to the provided string') +@click.pass_context +@source_multicommand(type=SourceCommandType.MULTICOMMAND_UNSUPPORTED, + retrieve_click_context=True) +def sensor_userdata(ctx: SourceCommandContext, click_ctx: click.core.Context, s: str) -> None: + """Retrieve or set userdata from the current sensor (if supported by the firmware)""" + from ouster.sdk.client._client import SensorHttp + try: + http = SensorHttp.create(ctx.source_uri) + if s is None: + click.echo(http.get_user_data()) + else: + http.set_user_data(s) except RuntimeError as e: raise click.ClickException(str(e)) diff --git a/python/src/ouster/cli/plugins/source_util.py b/python/src/ouster/cli/plugins/source_util.py index 0d1d5901..c32913dc 100644 --- a/python/src/ouster/cli/plugins/source_util.py +++ b/python/src/ouster/cli/plugins/source_util.py @@ -153,7 +153,7 @@ def _join_with_conjunction(things_to_join: List[str], separator: str = ', ', con Please verify that ROS Python modules are available. The best option is to try to install unofficial rospy packages that work -with Python 3.8 on Ubuntu 18.04/20.04 and Debian 10 without ROS: +with Python 3.8 on Ubuntu 20.04 and Debian 10 without ROS: pip install --extra-index-url https://rospypi.github.io/simple/ rospy rosbag tf2_ros diff --git a/python/src/ouster/client/__init__.py b/python/src/ouster/client/__init__.py deleted file mode 100644 index f238a589..00000000 --- a/python/src/ouster/client/__init__.py +++ /dev/null @@ -1,5 +0,0 @@ -# flake8: noqa (unused imports) - -print("warning: the ouster.client module has been moved to ouster.sdk.client, " - "please use the new path to avoid this warning.") -from ouster.sdk.client import * diff --git a/python/src/ouster/osf/__init__.py b/python/src/ouster/osf/__init__.py deleted file mode 100644 index 4032af47..00000000 --- a/python/src/ouster/osf/__init__.py +++ /dev/null @@ -1,5 +0,0 @@ -# flake8: noqa (unused imports) - -print("warning: the ouster.osf module has been moved to ouster.sdk.osf, " - "please use the new path to avoid this warning.") -from ouster.sdk.osf import * \ No newline at end of file diff --git a/python/src/ouster/pcap/__init__.py b/python/src/ouster/pcap/__init__.py deleted file mode 100644 index a53a354e..00000000 --- a/python/src/ouster/pcap/__init__.py +++ /dev/null @@ -1,5 +0,0 @@ -# flake8: noqa (unused imports) - -print("warning: the ouster.pcap module has been moved to ouster.sdk.pcap, " - "please use the new path to avoid this warning.") -from ouster.sdk.pcap import * diff --git a/python/src/ouster/sdk/__init__.py b/python/src/ouster/sdk/__init__.py index 9e64bf68..cd354ad5 100644 --- a/python/src/ouster/sdk/__init__.py +++ b/python/src/ouster/sdk/__init__.py @@ -2,8 +2,8 @@ Copyright (c) 2024, Ouster, Inc. All rights reserved. -Ouster-SDL +Ouster-SDK """ # flake8: noqa (unused imports) -from .open_source import open_source \ No newline at end of file +from .open_source import open_source diff --git a/python/src/ouster/sdk/client/__init__.py b/python/src/ouster/sdk/client/__init__.py index 138836ee..5fab5cc5 100644 --- a/python/src/ouster/sdk/client/__init__.py +++ b/python/src/ouster/sdk/client/__init__.py @@ -7,6 +7,7 @@ # flake8: noqa (unused imports) from ._client import SensorInfo +from ._client import ProductInfo from ._client import DataFormat from ._client import LidarMode from ._client import TimestampMode @@ -14,37 +15,36 @@ from ._client import MultipurposeIOMode from ._client import Polarity from ._client import NMEABaudRate -from ._client import ChanField from ._client import UDPProfileLidar from ._client import UDPProfileIMU from ._client import SensorConfig from ._client import SensorCalibration from ._client import ShotLimitingStatus from ._client import ThermalShutdownStatus +from ._client import FieldClass from ._client import FullScaleRange from ._client import ReturnOrder from ._client import init_logger -from ._client import convert_to_legacy from ._client import get_config from ._client import set_config +from ._client import FieldType from ._client import LidarScan from ._client import get_field_types -from ._client import _Packet -from ._client import _LidarPacket -from ._client import _ImuPacket +from ._client import Packet +from ._client import LidarPacket +from ._client import ImuPacket +from ._client import PacketValidationFailure +from ._client import PacketFormat +from ._client import PacketWriter from .data import BufferT from .data import FieldDType from .data import FieldTypes -from .data import Packet -from .data import ImuPacket -from .data import LidarPacket -from .data import LidarPacketValidator from .data import ColHeader from .data import XYZLut from .data import destagger -from .data import PacketValidationFailure, PacketIdError, PacketSizeError from .data import packet_ts +from .data import ChanField from .scan_source import ScanSource from .multi_scan_source import MultiScanSource @@ -70,4 +70,4 @@ from .multi import PacketMultiSource # type: ignore from .multi import PacketMultiWrapper # type: ignore -from .multi import ScansMulti # type: ignore \ No newline at end of file +from .multi import ScansMulti # type: ignore diff --git a/python/src/ouster/sdk/client/_client.pyi b/python/src/ouster/sdk/client/_client.pyi index a73309e7..12597cf8 100644 --- a/python/src/ouster/sdk/client/_client.pyi +++ b/python/src/ouster/sdk/client/_client.pyi @@ -14,30 +14,59 @@ Note: """ # flake8: noqa (linter complains about scoping, but afaict mypy doesn't care) +import numpy as np from numpy import ndarray -from typing import (ClassVar, Dict, Iterator, List, Optional, overload, Tuple) +from typing import (Any, ClassVar, Dict, Iterator, List, Optional, overload, Tuple) from .data import (BufferT, ColHeader, FieldDType, FieldTypes) -class _Packet: - _host_timestamp: int +class PacketValidationFailure: + NONE: ClassVar[PacketValidationFailure] + ID: ClassVar[PacketValidationFailure] + PACKET_SIZE: ClassVar[PacketValidationFailure] + + __members__: ClassVar[Dict[str, PacketValidationFailure]] + + def __init__(self, code: int) -> None: + ... + + def __int__(self) -> int: + ... + + @property + def name(self) -> str: + ... + + @property + def value(self) -> int: + ... + + @classmethod + def from_string(cls, s: str) -> PacketValidationFailure: + ... + + +class Packet: + host_timestamp: int capture_timestamp: Optional[float] - def __init__(self, size: int) -> None: + def __init__(self, size: int = ...) -> None: ... @property - def _data(self) -> ndarray: + def buf(self) -> ndarray: ... -class _LidarPacket(_Packet): - pass +class LidarPacket(Packet): + def validate(self, metadata: SensorInfo, packet_format: PacketFormat) -> PacketValidationFailure: + ... -class _ImuPacket(_Packet): - pass +class ImuPacket(Packet): + def validate(self, metadata: SensorInfo, packet_format: PacketFormat) -> PacketValidationFailure: + ... class Event: @@ -71,10 +100,10 @@ class SensorConnection: def poll(self, timeout_sec: int) -> ClientState: ... - def read_lidar_packet(self, packet: _LidarPacket, pf: PacketFormat) -> bool: + def read_lidar_packet(self, packet: LidarPacket, pf: PacketFormat) -> bool: ... - def read_imu_packet(self, packet: _ImuPacket, pf: PacketFormat) -> bool: + def read_imu_packet(self, packet: ImuPacket, pf: PacketFormat) -> bool: ... @property @@ -85,7 +114,7 @@ class SensorConnection: def imu_port(self) -> int: ... - def get_metadata(self, timeout_sec: int, legacy: bool) -> str: + def get_metadata(self, timeout_sec: int) -> str: ... def shutdown(self) -> None: @@ -118,7 +147,7 @@ class UDPPacketSource: def pop(self, timeout_sec: float) -> Event: ... - def packet(self, e: Event) -> _Packet: + def packet(self, e: Event) -> Packet: ... def advance(self, e: Event) -> None: @@ -162,15 +191,15 @@ class Client: def pop(self, timeout_sec: float) -> ClientState: ... - def packet(self, st: ClientState) -> _Packet: + def packet(self, st: ClientState) -> Packet: ... def advance(self, st: ClientState) -> None: ... def consume(self, - lidarp: _LidarPacket, - imup: _ImuPacket, + lidarp: LidarPacket, + imup: ImuPacket, timeout_sec: float) -> ClientState: ... @@ -218,11 +247,17 @@ class ClientState: ... +class ProductInfo: + full_product_info: str + form_factor: str + short_range: bool + beam_config: str + beam_count: int + + class SensorInfo: - hostname: str sn: str fw_rev: str - mode: LidarMode prod_line: str format: DataFormat beam_azimuth_angles: List[float] @@ -233,25 +268,28 @@ class SensorInfo: beam_to_lidar_transform: ndarray extrinsic: ndarray init_id: int - udp_port_lidar: int - udp_port_imu: int build_date: str image_rev: str prod_pn: str status: str cal: SensorCalibration config: SensorConfig + user_data: str @classmethod def from_default(cls, mode: LidarMode) -> SensorInfo: ... @classmethod - def original_string(cls) -> str: + def to_json_string(cls) -> str: ... @classmethod - def updated_metadata_string(cls) -> str: + def get_version(self) -> Version: + ... + + @classmethod + def get_product_info(self) -> ProductInfo: ... @classmethod @@ -283,6 +321,9 @@ class DataFormat: class PacketFormat: + def __init__(self, metadata: SensorInfo) -> None: + ... + @property def lidar_packet_size(self) -> int: ... @@ -352,16 +393,16 @@ class PacketFormat: ... @property - def fields(self) -> Iterator[ChanField]: + def fields(self) -> Iterator[str]: ... - def field_value_mask(self, field: ChanField) -> int: + def field_value_mask(self, field: str) -> int: ... - def field_bitness(self, field: ChanField) -> int: + def field_bitness(self, field: str) -> int: ... - def packet_field(self, field: ChanField, buf: BufferT) -> ndarray: + def packet_field(self, field: str, buf: BufferT) -> ndarray: ... def packet_header(self, header: ColHeader, buf: BufferT) -> ndarray: @@ -398,6 +439,10 @@ class PacketFormat: def from_info(info: SensorInfo) -> PacketFormat: ... + @staticmethod + def from_metadata(info: SensorInfo) -> PacketFormat: + ... + @staticmethod def from_profile(udp_profile_lidar: UDPProfileLidar, pixels_per_column: int, @@ -416,26 +461,26 @@ class PacketWriter(PacketFormat): columns_per_packet: int) -> PacketWriter: ... - def set_col_timestamp(self, packet: _LidarPacket, col_idx: int, ts: int) -> None: + def set_col_timestamp(self, packet: LidarPacket, col_idx: int, ts: int) -> None: ... def set_col_measurement_id(self, - packet: _LidarPacket, + packet: LidarPacket, col_idx: int, m_id: int) -> None: ... - def set_col_status(self, packet: _LidarPacket, col_idx: int, status: int) -> None: + def set_col_status(self, packet: LidarPacket, col_idx: int, status: int) -> None: ... - def set_frame_id(self, packet: _LidarPacket, frame_id: int) -> None: + def set_frame_id(self, packet: LidarPacket, frame_id: int) -> None: ... - def set_field(self, packet: _LidarPacket, chan: ChanField, field: ndarray) -> None: + def set_field(self, packet: LidarPacket, chan: str, field: ndarray) -> None: ... -def scan_to_packets(ls: LidarScan, pw: PacketWriter, init_id: int, prod_sn: int) -> List[_LidarPacket]: +def scan_to_packets(ls: LidarScan, pw: PacketWriter, init_id: int, prod_sn: int) -> List[LidarPacket]: ... @@ -457,14 +502,6 @@ class LidarMode: def __int__(self) -> int: ... - @property - def cols(self) -> int: - ... - - @property - def frequency(self) -> int: - ... - @property def name(self) -> str: ... @@ -667,59 +704,6 @@ class NMEABaudRate: ... -class ChanField: - RANGE: ClassVar[ChanField] - RANGE2: ClassVar[ChanField] - SIGNAL: ClassVar[ChanField] - SIGNAL2: ClassVar[ChanField] - REFLECTIVITY: ClassVar[ChanField] - REFLECTIVITY2: ClassVar[ChanField] - FLAGS: ClassVar[ChanField] - FLAGS2: ClassVar[ChanField] - NEAR_IR: ClassVar[ChanField] - RAW_HEADERS: ClassVar[ChanField] - CUSTOM0: ClassVar[ChanField] - CUSTOM1: ClassVar[ChanField] - CUSTOM2: ClassVar[ChanField] - CUSTOM3: ClassVar[ChanField] - CUSTOM4: ClassVar[ChanField] - CUSTOM5: ClassVar[ChanField] - CUSTOM6: ClassVar[ChanField] - CUSTOM7: ClassVar[ChanField] - CUSTOM8: ClassVar[ChanField] - CUSTOM9: ClassVar[ChanField] - RAW32_WORD1: ClassVar[ChanField] - RAW32_WORD2: ClassVar[ChanField] - RAW32_WORD3: ClassVar[ChanField] - RAW32_WORD4: ClassVar[ChanField] - RAW32_WORD5: ClassVar[ChanField] - RAW32_WORD6: ClassVar[ChanField] - RAW32_WORD7: ClassVar[ChanField] - RAW32_WORD8: ClassVar[ChanField] - RAW32_WORD9: ClassVar[ChanField] - - __members__: ClassVar[Dict[str, ChanField]] - values: ClassVar[Iterator[ChanField]] - - def __init__(self, code: int) -> None: - ... - - def __int__(self) -> int: - ... - - @property - def name(self) -> str: - ... - - @property - def value(self) -> int: - ... - - @classmethod - def from_string(cls, s: str) -> ChanField: - ... - - class UDPProfileLidar: PROFILE_LIDAR_LEGACY: ClassVar[UDPProfileLidar] PROFILE_LIDAR_RNG19_RFL8_SIG16_NIR16_DUAL: ClassVar[UDPProfileLidar] @@ -838,6 +822,7 @@ class SensorCalibration: reflectivity_status: Optional[bool] reflectivity_timestamp: Optional[str] + class SensorConfig: udp_dest: Optional[str] udp_port_lidar: Optional[int] @@ -875,8 +860,33 @@ class SensorConfig: def __init__(self, config_string: str) -> None: ... -def convert_to_legacy(metadata: str) -> str: - ... + +class FieldClass: + PIXEL_FIELD: ClassVar[FieldClass] + COLUMN_FIELD: ClassVar[FieldClass] + PACKET_FIELD: ClassVar[FieldClass] + + __members__: ClassVar[Dict[str, FieldClass]] + values: ClassVar[Iterator[FieldClass]] + + def __init__(self, code: int) -> None: + ... + + def __int__(self) -> int: + ... + + @property + def name(self) -> str: + ... + + @property + def value(self) -> int: + ... + + @classmethod + def from_string(cls, s: str) -> FieldClass: + ... + def init_logger(log_level: str, log_file_path: str = ..., @@ -901,6 +911,10 @@ class Version: major: int minor: int patch: int + stage: str + machine: str + prerelease: str + build: str def __init__(self) -> None: ... @@ -916,6 +930,16 @@ class Version: ... +class FieldType: + name: str + element_type: Any + extra_dims: Tuple[int, ...] + field_class: FieldClass + + def __init__(self, name: str, dtype: Any, extra_dims: Tuple[int, ...] = (), field_class: FieldClass = FieldClass.PIXEL_FIELD): + ... + + class LidarScan: frame_id: int @@ -934,12 +958,7 @@ class LidarScan: ... @overload - def __init__(self, h: int, w: int, fields: Dict[ChanField, - FieldDType]) -> None: - ... - - @overload - def __init__(self, h: int, w: int, fields: Dict[ChanField, FieldDType], columns_per_packet: int) -> None: + def __init__(self, h: int, w: int, fields: List[FieldType], columns_per_packet: int = ...) -> None: ... @overload @@ -947,7 +966,7 @@ class LidarScan: ... @overload - def __init__(self, scan: LidarScan, fields: Dict[ChanField, FieldDType]) -> None: + def __init__(self, scan: LidarScan, fields: List[FieldType]) -> None: ... @property @@ -958,7 +977,6 @@ class LidarScan: def h(self) -> int: ... - def thermal_shutdown(self) -> int: ... @@ -969,7 +987,34 @@ class LidarScan: def packet_timestamp(self) -> ndarray: ... - def field(self, field: ChanField) -> ndarray: + @overload + def field(self, field: str) -> ndarray: + ... + + @overload + def field(self, name: str) -> ndarray: + ... + + @property + def field_types(self) -> List[FieldType]: + ... + + @overload + def add_field(self, field_type: FieldType) -> ndarray: + ... + + @overload + def add_field(self, name: str, array: ndarray, field_class: FieldClass = ...) -> ndarray: + ... + + @overload + def add_field(self, name: str, dtype: FieldDType, shape: Tuple[int, ...], field_class: FieldClass = ...) -> ndarray: + ... + + def del_field(self, name: str) -> ndarray: + ... + + def field_class(self, name: str) -> FieldClass: ... @property @@ -992,7 +1037,7 @@ class LidarScan: ... @property - def fields(self) -> Iterator[ChanField]: + def fields(self) -> Iterator[str]: ... @@ -1064,7 +1109,7 @@ class ScanBatcher: ... @overload - def __call__(self, packet: _LidarPacket, ls: LidarScan) -> bool: + def __call__(self, packet: LidarPacket, ls: LidarScan) -> bool: ... @@ -1114,20 +1159,20 @@ class FieldInfo: ... offset: int - mask: int - shift: int + mask: int + shift: int + def add_custom_profile(profile_nr: int, name: str, - fields: List[Tuple[int, FieldInfo]], + fields: List[Tuple[str, FieldInfo]], chan_data_size: int) -> None: ... -@overload -def get_field_types(scan: LidarScan) -> FieldTypes: ... @overload -def get_field_types(info: SensorInfo) -> FieldTypes: ... +def get_field_types(info: SensorInfo) -> List[FieldType]: ... + @overload -def get_field_types(udp_profile_lidar: UDPProfileLidar) -> FieldTypes: ... +def get_field_types(udp_profile_lidar: UDPProfileLidar) -> List[FieldType]: ... diff --git a/python/src/ouster/sdk/client/_digest.py b/python/src/ouster/sdk/client/_digest.py index 61e37b79..babea34f 100644 --- a/python/src/ouster/sdk/client/_digest.py +++ b/python/src/ouster/sdk/client/_digest.py @@ -14,29 +14,8 @@ import numpy as np -from .data import (LidarPacket, LidarScan, ColHeader) -from .core import (Packets, PacketSource, Scans) - - -# NOTE[pb]: Extracted from LidarPacket for keeping tests based on digests working -# this method is deprecated along with ColHeader and should be cleaned -# fully eventually. -def _get_packet_header(packet: LidarPacket, header: ColHeader) -> np.ndarray: - """Create a view of the specified column header. - - This method is deprecated. Use the ``timestamp``, ``measurement_id`` or - ``status`` properties instead. - - Args: - header: The column header to parse - - Returns: - A numpy array containing a copy of the specified header values - """ - - res = packet._pf.packet_header(header, packet._data) - res.flags.writeable = False - return res +from .data import (LidarScan, ColHeader) +from .core import (Packets, LidarPacket, PacketSource, Scans, PacketFormat) def _md5(a: np.ndarray) -> str: @@ -69,20 +48,20 @@ def check(self, other: 'FieldDigest'): assert other.hashes.get(k) == v, f"Match failure key: {k}" @classmethod - def from_packet(cls, packet: LidarPacket) -> 'FieldDigest': - return cls.from_packets([packet]) + def from_packet(cls, packet: LidarPacket, pf: PacketFormat) -> 'FieldDigest': + return cls.from_packets([packet], pf) @classmethod - def from_packets(cls, packets: Iterable[LidarPacket]) -> 'FieldDigest': + def from_packets(cls, packets: Iterable[LidarPacket], pf: PacketFormat) -> 'FieldDigest': # hashlib._Hash doesn't exist at runtime hashes: Dict[str, 'hashlib._Hash'] = defaultdict(hashlib.md5) for idx, packet in enumerate(packets): # TODO: add packet headers for h in ColHeader: - hashes[h.name].update(_get_packet_header(packet, h).tobytes()) - for f in packet.fields: - hashes[f.name].update(packet.field(f).tobytes()) + hashes[h.name].update(pf.packet_header(h, packet.buf).tobytes()) + for field_name in pf.fields: + hashes[field_name].update(pf.packet_field(field_name, packet.buf).tobytes()) return cls(**{k: v.hexdigest() for k, v in hashes.items()}) @@ -97,7 +76,7 @@ def from_scan(cls, ls: LidarScan) -> 'FieldDigest': hashes['STATUS'] = _md5(ls.status.astype(np.uint64)) hashes['MEASUREMENT_ID'] = _md5(ls.measurement_id.astype(np.uint16)) - hashes.update({c.name: _md5(ls.field(c)) for c in ls.fields}) + hashes.update({field_name: _md5(ls.field(field_name)) for field_name in ls.fields}) return cls(**hashes) @@ -151,6 +130,7 @@ def from_packets(cls, source: PacketSource) -> 'StreamDigest': plist = [p for p in source1 if isinstance(p, LidarPacket)] logging.debug(f"Creating digest with {len(plist)} Lidar packets out of total {len(allpackets)}") packets = Packets(plist, source.metadata) + pf = PacketFormat(source.metadata) scans_list1, scans_list2 = tee(Scans(packets)) for scan in scans_list1: @@ -158,7 +138,7 @@ def from_packets(cls, source: PacketSource) -> 'StreamDigest': # scan_digests = list(map(FieldDigest.from_scan, Scans(packets))) scan_digests = list(map(FieldDigest.from_scan, scans_list2)) - packet_digest = FieldDigest.from_packets(plist) + packet_digest = FieldDigest.from_packets(plist, pf) return cls(packet_hash=packet_digest, scans=scan_digests) diff --git a/python/src/ouster/sdk/client/core.py b/python/src/ouster/sdk/client/core.py index 0bf1f16d..5e268cb0 100644 --- a/python/src/ouster/sdk/client/core.py +++ b/python/src/ouster/sdk/client/core.py @@ -7,25 +7,26 @@ generated using pybind11. """ from contextlib import closing -from typing import (cast, Dict, Iterable, Iterator, List, Optional, Tuple, +from typing import (cast, Iterable, Iterator, List, Optional, Tuple, Union, Callable) from threading import Thread import time +import logging from math import ceil import numpy as np - from more_itertools import take from typing_extensions import Protocol -from ._client import (SensorInfo, SensorConnection, Client, ClientState, - PacketFormat, LidarScan, ScanBatcher, get_field_types) +from ._client import (SensorInfo, SensorConnection, Client, ClientState, PacketValidationFailure, + PacketFormat, LidarScan, ScanBatcher, get_field_types, LidarPacket, ImuPacket, + Packet, FieldType) -from .data import (ChanField, FieldDType, ImuPacket, - LidarPacket, Packet, PacketIdError, - FieldTypes) +from .data import (FieldTypes) from .scan_source import ScanSource +logger = logging.getLogger("ouster.sdk.client.core") + class ClientError(Exception): """Base class for client errors.""" @@ -132,7 +133,6 @@ def __init__(self, _overflow_err: bool = False, _flush_before_read: bool = True, _flush_frames: int = 5, - _legacy_format: bool = False, soft_id_check: bool = False, _skip_metadata_beam_validation: bool = False) -> None: """ @@ -150,7 +150,6 @@ def __init__(self, _overflow_err: if True, raise ClientOverflow _flush_before_read: if True, try to clear buffers before reading _flush_frames: the number of frames to skip/flush on start of a new iter - _legacy_format: if True, use legacy metadata format soft_id_check: if True, don't skip lidar packets buffers on, id mismatch (init_id/sn pair), _skip_metadata_beam_validation: if True, skip metadata beam angle check @@ -166,7 +165,6 @@ def __init__(self, self._cache = None self._fetched_meta = "" self._flush_frames = _flush_frames - self._legacy_format = _legacy_format self._soft_id_check = soft_id_check self._id_error_count = 0 @@ -186,10 +184,8 @@ def __init__(self, self._cli = Client(self._connection, buf_size, self._pf.lidar_packet_size, buf_size, self._pf.imu_packet_size) - self._lidarbuf = LidarPacket(None, - self._metadata, - _raise_on_id_check=not self._soft_id_check) - self._imubuf = ImuPacket(packet_format=self._pf) + self._lidarbuf = LidarPacket(self._pf.lidar_packet_size) + self._imubuf = ImuPacket(self._pf.imu_packet_size) # Use args to avoid capturing self causing circular reference self._producer = Thread(target=self._cli.produce) @@ -213,7 +209,7 @@ def _fetch_metadata(self, timeout: Optional[float] = None) -> None: timeout_sec = ceil(timeout) if not self._fetched_meta: self._fetched_meta = self._connection.get_metadata( - legacy=self._legacy_format, timeout_sec=timeout_sec) + timeout_sec=timeout_sec) if not self._fetched_meta: raise ClientError("Failed to collect metadata") @@ -242,14 +238,30 @@ def _next_packet(self) -> Optional[Packet]: else: raise ValueError() if st & ClientState.LIDAR_DATA: - packet = LidarPacket(self._lidarbuf._data, self._metadata, - self._lidarbuf.capture_timestamp, - _raise_on_id_check = not self._soft_id_check) - if packet.id_error: + msg = self._lidarbuf + self._lidarbuf = LidarPacket(self._pf.lidar_packet_size) + res = msg.validate(self._metadata, self._pf) + if res == PacketValidationFailure.PACKET_SIZE: + raise ValueError(f"Packet was unexpected size {len(msg.buf)}") + if res == PacketValidationFailure.ID: self._id_error_count += 1 - return packet + init_id = self._pf.init_id(msg.buf) + prod_sn = self._pf.prod_sn(msg.buf) + error_msg = f"Metadata init_id/sn does not match: " \ + f"expected by metadata - {self._metadata.init_id}/{self._metadata.sn}, " \ + f"but got from packet buffer - {init_id}/{prod_sn}" + if not self._soft_id_check: + raise ValueError(error_msg) + else: + # Continue with warning. When init_ids/sn doesn't match + # the resulting LidarPacket has high chances to be + # incompatible with data format set in metadata json file + logger.warn(f"LidarPacket validation: {error_msg}") + return msg elif st & ClientState.IMU_DATA: - return self._imubuf + msg2 = self._imubuf + self._imubuf = ImuPacket(self._pf.imu_packet_size) + return msg2 elif st == ClientState.TIMEOUT: raise ClientTimeout(f"No packets received within {self._timeout}s from sensor " f"{self._hostname} using udp destination {self._metadata.config.udp_dest} " @@ -301,8 +313,6 @@ def __iter__(self) -> Iterator[Packet]: yield p else: break - except PacketIdError: - self._id_error_count += 1 except ValueError: # bad packet size here: this can happen when # packets are buffered by the OS, not necessarily an error @@ -337,7 +347,7 @@ def flush(self, n_frames: int = 3, *, full=False) -> int: # check next packet to see if it's the start of a new frame st = self._peek() if st & ClientState.LIDAR_DATA: - frame = self._pf.frame_id(self._lidarbuf._data) + frame = self._pf.frame_id(self._lidarbuf.buf) if frame != last_frame: last_frame = frame n_frames -= 1 @@ -404,7 +414,7 @@ def __init__(self, *, complete: bool = False, timeout: Optional[float] = 2.0, - fields: Optional[Dict[ChanField, FieldDType]] = None, + fields: Optional[List[FieldType]] = None, _max_latency: int = 0) -> None: """ Args: @@ -419,10 +429,14 @@ def __init__(self, self._timeout = timeout self._max_latency = _max_latency # used to initialize LidarScan - self._fields: FieldTypes = ( + self._field_types: FieldTypes = ( fields if fields is not None else get_field_types(self._source.metadata.format.udp_profile_lidar)) + self._fields = [] + for f in self._field_types: + self._fields.append(f.name) + def __iter__(self) -> Iterator[LidarScan]: """Get an iterator.""" @@ -458,11 +472,12 @@ def __iter__(self) -> Iterator[LidarScan]: if self._timeout is not None and (time.monotonic() >= start_ts + self._timeout): - raise ClientTimeout(f"No valid frames received within {self._timeout}s") + raise ClientTimeout( + f"No valid frames received within {self._timeout}s") if isinstance(packet, LidarPacket): ls_write = ls_write or LidarScan( - h, w, self._fields, columns_per_packet) + h, w, self._field_types, columns_per_packet) if batch(packet, ls_write): # Got a new frame, return it and start another @@ -504,7 +519,11 @@ def is_indexed(self) -> bool: return False @property - def fields(self) -> FieldTypes: + def field_types(self) -> List[FieldType]: + return self._field_types + + @property + def fields(self) -> List[str]: return self._fields @property @@ -514,18 +533,24 @@ def scans_num(self) -> Optional[int]: def __len__(self) -> int: raise TypeError("len is not supported on live or non-indexed sources") - def _seek(self, int) -> None: + def _seek(self, _) -> None: raise RuntimeError( "can not invoke __getitem__ on non-indexed source") - def __getitem__(self, key: Union[int, slice] - ) -> Union[Optional[LidarScan], List[Optional[LidarScan]]]: + def __getitem__(self, _: Union[int, slice] + ) -> Union[Optional[LidarScan], ScanSource]: raise RuntimeError( "can not invoke __getitem__ on non-indexed source") def __del__(self) -> None: pass + def _slice_iter(self, _: slice) -> Iterator[Optional[LidarScan]]: + raise NotImplementedError + + def slice(self, _: slice) -> ScanSource: + raise NotImplementedError + @classmethod def sample( cls, @@ -577,7 +602,7 @@ def stream( timeout: Optional[float] = 2.0, complete: bool = True, metadata: Optional[SensorInfo] = None, - fields: Optional[Dict[ChanField, FieldDType]] = None) -> 'Scans': + fields: Optional[List[FieldType]] = None) -> 'Scans': """Stream scans from a sensor. Will drop frames preemptively to avoid filling up internal buffers and @@ -609,19 +634,20 @@ def stream( class FrameBorder: """Create callable helper that indicates the cross frames packets.""" - def __init__(self, pred: Callable[[Packet], bool] = lambda _: True): + def __init__(self, meta: SensorInfo, pred: Callable[[Packet], bool] = lambda _: True): self._last_f_id = -1 self._last_packet_ts = None self._last_packet_res = False self._pred = pred + self._pf = PacketFormat(meta) def __call__(self, packet: Packet) -> bool: if isinstance(packet, LidarPacket): # don't examine packets again - if (self._last_packet_ts and packet.capture_timestamp and - self._last_packet_ts == packet.capture_timestamp): + if (self._last_packet_ts and (packet.host_timestamp != 0) and + self._last_packet_ts == packet.host_timestamp): return self._last_packet_res - f_id = packet.frame_id + f_id = self._pf.frame_id(packet.buf) changed = (self._last_f_id != -1 and f_id != self._last_f_id) self._last_packet_res = changed and self._pred(packet) self._last_f_id = f_id diff --git a/python/src/ouster/sdk/client/data.py b/python/src/ouster/sdk/client/data.py index 4109c7c7..8d946d25 100644 --- a/python/src/ouster/sdk/client/data.py +++ b/python/src/ouster/sdk/client/data.py @@ -4,13 +4,12 @@ """ from enum import Enum -from typing import Callable, Iterator, Type, List, Optional, Union, Dict +from typing import Callable, List, Union, Any import logging import numpy as np -from ._client import (ChanField, LidarScan, SensorInfo, PacketFormat, - _ImuPacket, _LidarPacket) +from ._client import (LidarScan, SensorInfo, Packet, FieldType) from ._client import (destagger_int8, destagger_int16, destagger_int32, destagger_int64, destagger_uint8, destagger_uint16, @@ -22,91 +21,35 @@ BufferT = Union[bytes, bytearray, memoryview, np.ndarray] """Types that support the buffer protocol.""" -FieldDType = Type[np.unsignedinteger] +FieldDType = Any """Numpy dtype of fields.""" -Packet = Union['ImuPacket', 'LidarPacket'] -"""Packets emitted by a sensor.""" - -FieldTypes = Dict[ChanField, FieldDType] +FieldTypes = List[FieldType] """LidarScan chan fields with types""" logger = logging.getLogger("ouster.sdk.client.data") -class ImuPacket(_ImuPacket): - """Read IMU Packet data from a buffer.""" - _pf: PacketFormat - - def __init__(self, - data: Optional[BufferT] = None, - info: Optional[SensorInfo] = None, - timestamp: Optional[float] = None, - *, - packet_format: Optional[PacketFormat] = None) -> None: - """ - Args: - data: Buffer containing the packet payload - info: Metadata associated with the sensor packet stream - timestamp: A capture timestamp, in seconds - - Raises: - ValueError: If the buffer is smaller than the size specified by the - packet format - """ - if packet_format: - self._pf = packet_format - elif info: - # TODO: we should deprecate this, constructing a full PacketFormat - # for every single packet seems like an antipattern -- Tim T. - self._pf = PacketFormat.from_info(info) - else: - raise ValueError("either packet_format or info should be specified") - - n = self._pf.imu_packet_size - super().__init__(size=n) - if data is not None: - self._data[:] = np.frombuffer(data, dtype=np.uint8, count=n) - self.capture_timestamp = timestamp - - def __deepcopy__(self, memo) -> 'ImuPacket': - cls = type(self) - cpy = cls(self._data, packet_format=self._pf) - cpy._host_timestamp = self._host_timestamp - return cpy - - @property - def sys_ts(self) -> int: - """System timestamp in nanoseconds.""" - return self._pf.imu_sys_ts(self._data) - - @property - def accel_ts(self) -> int: - """Accelerometer read time in nanoseconds.""" - return self._pf.imu_accel_ts(self._data) - - @property - def gyro_ts(self) -> int: - """Gyro read time in nanoseconds.""" - return self._pf.imu_gyro_ts(self._data) - - @property - def accel(self) -> np.ndarray: - """Acceleration as a 3-D vector in G.""" - return np.array([ - self._pf.imu_la_x(self._data), - self._pf.imu_la_y(self._data), - self._pf.imu_la_z(self._data) - ]) - - @property - def angular_vel(self) -> np.ndarray: - """Angular velocity as a 3-D vector in deg/second.""" - return np.array([ - self._pf.imu_av_x(self._data), - self._pf.imu_av_y(self._data), - self._pf.imu_av_z(self._data) - ]) +class ChanField: + RANGE = "RANGE" + RANGE2 = "RANGE2" + SIGNAL = "SIGNAL" + SIGNAL2 = "SIGNAL2" + REFLECTIVITY = "REFLECTIVITY" + REFLECTIVITY2 = "REFLECTIVITY2" + NEAR_IR = "NEAR_IR" + FLAGS = "FLAGS" + FLAGS2 = "FLAGS2" + RAW_HEADERS = "RAW_HEADERS" + RAW32_WORD1 = "RAW32_WORD1" + RAW32_WORD2 = "RAW32_WORD2" + RAW32_WORD3 = "RAW32_WORD3" + RAW32_WORD4 = "RAW32_WORD4" + RAW32_WORD5 = "RAW32_WORD5" + RAW32_WORD6 = "RAW32_WORD6" + RAW32_WORD7 = "RAW32_WORD7" + RAW32_WORD8 = "RAW32_WORD8" + RAW32_WORD9 = "RAW32_WORD9" class ColHeader(Enum): @@ -124,226 +67,6 @@ def __int__(self) -> int: return self.value -class PacketValidationFailure(Exception): - def __eq__(self, other): - return type(self) is type(other) and self.args == other.args - - def __hash__(self): - return hash((type(self), self.args)) - - -class PacketIdError(PacketValidationFailure): - """Exception raised when init_id/sn from metadata and packet doesn't match.""" - pass - - -class PacketSizeError(PacketValidationFailure): - """Exception raised when the packet size wrong for the given metadata.""" - pass - - -class LidarPacketValidator: - """A utility class for validating lidar packets for a given sensor info.""" - def __init__(self, metadata: SensorInfo, checks=['id_and_sn_valid', 'packet_size_valid']): - self._metadata = metadata - self._metadata_init_id = metadata.init_id - self._metadata_sn = int(metadata.sn) if metadata.sn else 0 - self._pf = PacketFormat.from_info(metadata) - self._checks = [getattr(self, check) for check in checks] - - def check_packet(self, data: BufferT, n_bytes: int) -> List[PacketValidationFailure]: - errors = [] - for check in self._checks: - error = check(data, n_bytes) - if error: - errors.append(error) - return errors - - def id_and_sn_valid(self, data: BufferT, n_bytes: int) -> Optional[PacketValidationFailure]: - """Check the metadata init_id/sn and packet init_id/sn mismatch.""" - init_id = self._pf.init_id(data) - sn = self._pf.prod_sn(data) - if bool(init_id and (init_id != self._metadata_init_id or sn != self._metadata_sn)): - error_msg = f"Metadata init_id/sn does not match: " \ - f"expected by metadata - {self._metadata_init_id}/{self._metadata_sn}, " \ - f"but got from packet buffer - {init_id}/{sn}" - return PacketIdError(error_msg) - return None - - def packet_size_valid(self, data: BufferT, n_bytes: int) -> Optional[PacketValidationFailure]: - if self._pf.lidar_packet_size != n_bytes: - return PacketSizeError( - f"Expected a packet of size {self._pf.lidar_packet_size} but got a buffer of size {n_bytes}") - return None - - -class LidarPacket(_LidarPacket): - """Read lidar packet data as numpy arrays. - - The dimensions of returned arrays depend on the sensor product line and - configuration. Measurement headers will be arrays of size matching the - configured ``columns_per_packet``, while measurement fields will be 2d - arrays of size ``pixels_per_column`` by ``columns_per_packet``. - """ - _pf: PacketFormat - _metadata_init_id: int - _metadata_sn: int - - def __init__(self, - data: Optional[BufferT] = None, - info: Optional[SensorInfo] = None, - timestamp: Optional[float] = None, - *, - packet_format: Optional[PacketFormat] = None, - _raise_on_id_check: bool = True) -> None: - """ - Args: - data: Buffer containing the packet payload - info: Metadata associated with the sensor packet stream - timestamp: A capture timestamp, in seconds - _raise_on_id_check: raise PacketIdError if metadata - init_id/sn doesn't match packet init_id/sn. - - Raises: - ValueError: If the buffer is smaller than the size specified by the - packet format, or if the init_id doesn't match the metadata - """ - if packet_format: - self._pf = packet_format - elif info: - # TODO: we should deprecate this, constructing a full PacketFormat - # for every single packet seems like an antipattern -- Tim T. - self._pf = PacketFormat.from_info(info) - else: - raise ValueError("either packet_format or info should be specified") - - n = self._pf.lidar_packet_size - super().__init__(size=n) - if data is not None: - self._data[:] = np.frombuffer(data, dtype=np.uint8, count=n) - self.capture_timestamp = timestamp - - if info: - self._metadata_init_id = info.init_id - self._metadata_sn = int(info.sn) if info.sn else 0 - - # check that metadata came from the same sensor initialization as data - if info and self.id_error: - error_msg = f"Metadata init_id/sn does not match: " \ - f"expected by metadata - {info.init_id}/{info.sn}, " \ - f"but got from packet buffer - {self.init_id}/{self.prod_sn}" - if _raise_on_id_check: - raise PacketIdError(error_msg) - else: - # Continue with warning. When init_ids/sn doesn't match - # the resulting LidarPacket has high chances to be - # incompatible with data format set in metadata json file - logger.warn(f"LidarPacket validation: {error_msg}") - - def __deepcopy__(self, memo) -> 'LidarPacket': - cls = type(self) - cpy = cls(self._data, packet_format=self._pf) - cpy._host_timestamp = self._host_timestamp - return cpy - - @property - def id_error(self) -> bool: - """Check the metadata init_id/sn and packet init_id/sn mismatch.""" - return bool(self.init_id and (self.init_id != self._metadata_init_id or - self.prod_sn != self._metadata_sn)) - - @property - def packet_type(self) -> int: - """Get the type header of the packet.""" - return self._pf.packet_type(self._data) - - @property - def frame_id(self) -> int: - """Get the frame id of the packet.""" - return self._pf.frame_id(self._data) - - @property - def init_id(self) -> int: - """Get the initialization id of the packet.""" - return self._pf.init_id(self._data) - - @property - def prod_sn(self) -> int: - """Get the serial no header of the packet.""" - return self._pf.prod_sn(self._data) - - @property - def countdown_thermal_shutdown(self) -> int: - """Get the thermal shutdown countdown of the packet.""" - return self._pf.countdown_thermal_shutdown(self._data) - - @property - def countdown_shot_limiting(self) -> int: - """Get the shot limiting countdown of the packet.""" - return self._pf.countdown_shot_limiting(self._data) - - @property - def thermal_shutdown(self) -> int: - """Get the thermal shutdown status of the packet.""" - return self._pf.thermal_shutdown(self._data) - - @property - def shot_limiting(self) -> int: - """Get the shot limiting status of the packet.""" - return self._pf.shot_limiting(self._data) - - @property - def fields(self) -> Iterator[ChanField]: - """Get available fields of LidarScan as Iterator.""" - return self._pf.fields - - def field(self, field: ChanField) -> np.ndarray: - """Create a view of the specified channel field. - - Args: - field: The channel field to view - - Returns: - A numpy array containing a copy of the specified field values - """ - res = self._pf.packet_field(field, self._data) - res.flags.writeable = False - return res - - @property - def timestamp(self) -> np.ndarray: - """Parse the measurement block timestamps out of a packet buffer. - - Returns: - An array of the timestamps of all measurement blocks in the packet. - """ - res = self._pf.packet_header(ColHeader.TIMESTAMP, self._data) - res.flags.writeable = False - return res - - @property - def measurement_id(self) -> np.ndarray: - """Parse the measurement ids out of a packet buffer. - - Returns: - An array of the ids of all measurement blocks in the packet. - """ - res = self._pf.packet_header(ColHeader.MEASUREMENT_ID, self._data) - res.flags.writeable = False - return res - - @property - def status(self) -> np.ndarray: - """Parse the measurement statuses of a packet buffer. - - Returns: - An array of the statuses of all measurement blocks in the packet. - """ - res = self._pf.packet_header(ColHeader.STATUS, self._data) - res.flags.writeable = False - return res - - def _destagger(field: np.ndarray, shifts: List[int], inverse: bool) -> np.ndarray: return { @@ -444,5 +167,4 @@ def res(ls: Union[LidarScan, np.ndarray]) -> np.ndarray: def packet_ts(packet: Packet) -> int: """Return the packet timestamp in nanoseconds""" - return int(packet.capture_timestamp * - 10**9) if packet.capture_timestamp else 0 + return packet.host_timestamp diff --git a/python/src/ouster/sdk/client/multi.py b/python/src/ouster/sdk/client/multi.py index 15325985..a855f992 100644 --- a/python/src/ouster/sdk/client/multi.py +++ b/python/src/ouster/sdk/client/multi.py @@ -5,8 +5,8 @@ import copy from ._client import (SensorInfo, LidarScan, PacketFormat, ScanBatcher, - get_field_types) -from .data import Packet, ImuPacket, LidarPacket, packet_ts, FieldTypes + get_field_types, Packet, ImuPacket, LidarPacket, FieldType) +from .data import packet_ts, FieldTypes from .core import PacketSource, first_valid_packet_ts from .scan_source import ScanSource from .multi_scan_source import MultiScanSource @@ -174,9 +174,15 @@ def __init__( if fields: if len(fields) != len(file_fields): raise ValueError("Size of Field override doens't match") - self._fields = fields + self._field_types = fields else: - self._fields = file_fields + self._field_types = file_fields + self._fields = [] + for l in self._field_types: + fl = [] + for f in l: + fl.append(f.name) + self._fields.append(fl) @property def sensors_count(self) -> int: @@ -199,9 +205,13 @@ def is_indexed(self) -> bool: return self._source.is_indexed @property - def fields(self) -> List[FieldTypes]: + def fields(self) -> List[str]: return self._fields + @property + def field_types(self) -> List[FieldType]: + return self._field_types + @property def scans_num(self) -> List[Optional[int]]: if self.is_live or not self.is_indexed: @@ -210,7 +220,8 @@ def scans_num(self) -> List[Optional[int]]: def __len__(self) -> int: if self.is_live or not self.is_indexed: - raise TypeError("len is not supported on unindexed or live sources") + raise TypeError( + "len is not supported on unindexed or live sources") raise NotImplementedError def __iter__(self) -> Iterator[List[Optional[LidarScan]]]: @@ -233,6 +244,8 @@ def _scans_iter(self, restart=True, cycle=False, deep_copy=False pf = [None] * self.sensors_count ls_write = [None] * self.sensors_count batch = [None] * self.sensors_count + ls_write = [None] * self.sensors_count + yielded = [None] * self.sensors_count for i, sinfo in enumerate(self.metadata): w[i] = sinfo.format.columns_per_frame @@ -241,6 +254,8 @@ def _scans_iter(self, restart=True, cycle=False, deep_copy=False columns_per_packet[i] = sinfo.format.columns_per_packet pf[i] = PacketFormat.from_info(sinfo) batch[i] = ScanBatcher(w[i], pf[i]) + ls_write[i] = LidarScan( + h[i], w[i], self._field_types[i], columns_per_packet[i]) # autopep8: off scan_shallow_yield = lambda x: x @@ -254,17 +269,17 @@ def _scans_iter(self, restart=True, cycle=False, deep_copy=False had_message = False for idx, packet in self._source: if isinstance(packet, LidarPacket): - ls_write[idx] = ls_write[idx] or LidarScan( - h[idx], w[idx], self._fields[idx], columns_per_packet[idx]) - if batch[idx](packet._data, packet_ts(packet), ls_write[idx]): + if batch[idx](packet.buf, packet_ts(packet), ls_write[idx]): if not self._complete or ls_write[idx].complete(col_window[idx]): had_message = True yield idx, scan_yield_op(ls_write[idx]) + yielded[idx] = ls_write[idx].frame_id # return the last not fully cut scans in the sensor timestamp order if # they satisfy the completeness criteria + skip_ls = lambda idx, ls: ls is None or ls.frame_id in [yielded[idx], -1] last_scans = sorted( - [(idx, ls) for idx, ls in enumerate(ls_write) if ls is not None], + [(idx, ls) for idx, ls in enumerate(ls_write) if not skip_ls(idx, ls)], key=lambda si: first_valid_packet_ts(si[1])) while last_scans: idx, ls = last_scans.pop(0) @@ -284,7 +299,7 @@ def _seek(self, offset: int) -> None: self._source.seek(offset) def __getitem__(self, key: Union[int, slice] - ) -> Union[List[Optional[LidarScan]], List[List[Optional[LidarScan]]]]: + ) -> Union[List[Optional[LidarScan]], MultiScanSource]: if not self.is_indexed: raise RuntimeError( diff --git a/python/src/ouster/sdk/client/multi_scan_source.py b/python/src/ouster/sdk/client/multi_scan_source.py index 76001a24..2b26efae 100644 --- a/python/src/ouster/sdk/client/multi_scan_source.py +++ b/python/src/ouster/sdk/client/multi_scan_source.py @@ -15,7 +15,7 @@ def sensors_count(self) -> int: @property def metadata(self) -> List[SensorInfo]: - """A list of Metadata objects associated with every scan streams.""" + """A list of 'SensorInfo' objects associated with every scan streams.""" ... @property @@ -42,25 +42,27 @@ def is_indexed(self) -> bool: """ ... - # Pavlo/Oct19: Optional field, that is currently available only for OSFs and IndexedPcap, and - # None everywhere else. - # UN/Nov21: I don't understand why this only available in OSF and IndexedPcap source @property - def fields(self) -> List[FieldTypes]: + def field_types(self) -> List[FieldTypes]: """Field types are present in the LidarScan objects on read from iterator""" ... + @property + def fields(self) -> List[List[str]]: + """Fields are present in the LidarScan objects on read from iterator""" + ... + @property def scans_num(self) -> List[Optional[int]]: - """Number of scans available, in case of a live sensor or non-indexable scan source this method - returns a None for that stream""" + """Number of scans available, in case of a live sensor or non-indexable scan source + this method returns a None for that stream""" ... def __len__(self) -> int: - """returns the number of scans containe with the scan_source, in case scan_source holds more than - one stream then this would measure the number of collated scans across the streams - in the case of a live sensor or non-indexable scan source this method throws a TypeError - """ + """returns the number of scans containe with the scan_source, in case scan_source has + more than one sensor then this would measure the number of collated scans across the + streams in the case of a live sensor or non-indexable scan source this method throws + a TypeError exception""" ... def __iter__(self) -> Iterator[List[Optional[LidarScan]]]: @@ -72,7 +74,7 @@ def _seek(self, key: int) -> None: ... def __getitem__(self, key: Union[int, slice] - ) -> Union[List[Optional[LidarScan]], List[List[Optional[LidarScan]]]]: + ) -> Union[List[Optional[LidarScan]], 'MultiScanSource']: """Indexed access and slices support""" ... @@ -86,3 +88,10 @@ def __del__(self) -> None: def single_source(self, stream_idx: int) -> ScanSource: ... + + def _slice_iter(self, key: slice) -> Iterator[List[Optional[LidarScan]]]: + ... + + def slice(self, key: slice) -> 'MultiScanSource': + """Constructs a MultiScanSource matching the specificed slice""" + ... diff --git a/python/src/ouster/sdk/client/multi_sliced_scan_source.py b/python/src/ouster/sdk/client/multi_sliced_scan_source.py new file mode 100644 index 00000000..63e10640 --- /dev/null +++ b/python/src/ouster/sdk/client/multi_sliced_scan_source.py @@ -0,0 +1,111 @@ +from typing import Optional, Iterator, Union, List +from itertools import islice +from ouster.sdk.client.scan_source import ScanSource +from ouster.sdk.client.multi_scan_source import MultiScanSource +from ouster.sdk.client._client import SensorInfo, LidarScan +from ouster.sdk.client.data import FieldTypes +from ouster.sdk.util.forward_slicer import ForwardSlicer + + +class MultiSlicedScanSource(MultiScanSource): + + def __init__(self, scan_source: MultiScanSource, key: slice) -> None: + + # Make sure current requirements are met + if key.start is None or key.stop is None or key.step is None: + raise TypeError("can't work with non-normalized key(slice)") + if key.start < 0 or key.stop < 0 or key.step < 0: + raise TypeError("can't work with negative key(slice)") + if key.stop > len(scan_source): + raise TypeError("range of key(slice) can't exceed scan_source") + + self._scan_source = scan_source + self._key = key + + @property + def sensors_count(self) -> int: + return self._scan_source.sensors_count + + @property + def metadata(self) -> List[SensorInfo]: + return self._scan_source.metadata + + @property + def is_live(self) -> bool: + return self._scan_source.is_live + + @property + def is_seekable(self) -> bool: + return self._scan_source.is_seekable + + @property + def is_indexed(self) -> bool: + return self._scan_source.is_indexed + + @property + def field_types(self) -> List[FieldTypes]: + return self._scan_source.field_types + + @property + def fields(self) -> List[List[str]]: + return self._scan_source.fields + + @property + def scans_num(self) -> List[Optional[int]]: + # TODO: this is not a true implementation of scans_num property in + # the sliced scan source. Without iterating over each source it + # wouldn't be completely possible to get real values. so for the time + # being we are using the main length property + return [len(self)] * self.sensors_count + + def __len__(self) -> int: + k = self._key + return (k.stop - k.start + k.step - 1) // k.step + + def __iter__(self) -> Iterator[List[Optional[LidarScan]]]: + return self._scan_source._slice_iter(self._key) + + def _seek(self, key: int) -> None: + self._scan_source._seek(key) + + def __getitem__(self, key: Union[int, slice] + ) -> Union[List[Optional[LidarScan]], MultiScanSource]: + if isinstance(key, int): + L = len(self) + if key < 0: + key += L + if key < 0 or key >= L: + raise IndexError("index is out of range") + # TODO: This is functionally correct but can be in-efficient + # when the slice range is rather large, the operator can be + # improved by relying on the original scan_source idx but that + # requires a major revision of current approach. + # https://ouster.atlassian.net/browse/FLEETSW-6251 + return next(islice(iter(self), key, key + 1, 1)) + if isinstance(key, slice): + return self.slice(key) + raise TypeError( + f"indices must be integer or slice, not {type(key).__name__}") + + def close(self) -> None: + pass + + def __del__(self) -> None: + self.close() + + def single_source(self, stream_idx: int) -> ScanSource: + from ouster.sdk.client.scan_source_adapter import ScanSourceAdapter + return ScanSourceAdapter(self, stream_idx) + + def _slice_iter(self, key: slice) -> Iterator[List[Optional[LidarScan]]]: + # here we are slicing the current slice + L = len(self) + k = ForwardSlicer.normalize(key, L) + return islice(iter(self), k.start, k.stop, k.step) + + def slice(self, key: slice) -> MultiScanSource: + L = len(self) + k = ForwardSlicer.normalize(key, L) + if k.step < 0: + raise TypeError("slice() can't work with negative step") + return MultiSlicedScanSource(self, k) diff --git a/python/src/ouster/sdk/client/scan_source.py b/python/src/ouster/sdk/client/scan_source.py index d18195bb..4a584dfe 100644 --- a/python/src/ouster/sdk/client/scan_source.py +++ b/python/src/ouster/sdk/client/scan_source.py @@ -1,7 +1,6 @@ -from typing import Iterator, List, Union, Optional +from typing import Iterator, Union, Optional, List from typing_extensions import Protocol -from ._client import SensorInfo, LidarScan -from .data import FieldTypes +from ._client import SensorInfo, LidarScan, FieldType class ScanSource(Protocol): @@ -9,7 +8,7 @@ class ScanSource(Protocol): @property def metadata(self) -> SensorInfo: - """A metadata object associated with the scan streams.""" + """A 'SensorInfo' object associated with the scan streams.""" ... @property @@ -37,14 +36,19 @@ def is_indexed(self) -> bool: ... @property - def fields(self) -> FieldTypes: + def field_types(self) -> List[FieldType]: """Field types are present in the LidarScan objects on read from iterator""" ... + @property + def fields(self) -> List[str]: + """Fields are present in the LidarScan objects on read from iterator""" + ... + @property def scans_num(self) -> Optional[int]: - """Number of scans available, in case of a live sensor or non-indexable scan source this method - returns None""" + """Number of scans available, in case of a live sensor or non-indexable scan source + this method returns None""" ... def __len__(self) -> int: @@ -67,14 +71,21 @@ def _seek(self, key: int) -> None: # Optional[LidarScan] since MultiScanSource returns collate scans by default. # This can be solved by provide a method that gives access to uncollated scans def __getitem__(self, key: Union[int, slice] - ) -> Union[Optional[LidarScan], List[Optional[LidarScan]]]: + ) -> Union[Optional[LidarScan], 'ScanSource']: """Indexed access and slices support""" ... def close(self) -> None: - """Release the underlying resource, if any.""" + """Manually release any underlying resource.""" ... def __del__(self) -> None: """Automatic release of any underlying resource.""" ... + + def _slice_iter(self, key: slice) -> Iterator[Optional[LidarScan]]: + ... + + def slice(self, key: slice) -> 'ScanSource': + """Constructs a ScanSource matching the specificed slice""" + ... diff --git a/python/src/ouster/sdk/client/scan_source_adapter.py b/python/src/ouster/sdk/client/scan_source_adapter.py index f06c5124..e210e847 100644 --- a/python/src/ouster/sdk/client/scan_source_adapter.py +++ b/python/src/ouster/sdk/client/scan_source_adapter.py @@ -1,17 +1,22 @@ -from typing import Iterator, List, Union, Optional -import typing +from typing import Iterator, List, Union, Optional, cast + +from ._client import SensorInfo, LidarScan +from ouster.sdk.util.forward_slicer import ForwardSlicer from .scan_source import ScanSource from .multi_scan_source import MultiScanSource -from ._client import SensorInfo, LidarScan from .data import FieldTypes +# TODO: since we have more than one adapter we out to rename this class +# to something specific + class ScanSourceAdapter(ScanSource): """Represents only data stream from one stream.""" def __init__(self, scan_source: MultiScanSource, stream_idx: int = 0) -> None: if stream_idx < 0 or stream_idx >= scan_source.sensors_count: - raise ValueError(f"stream_idx needs to be within the range [0, {scan_source.sensors_count})") + raise ValueError( + f"stream_idx needs to be within the range [0, {scan_source.sensors_count})") self._scan_source = scan_source self._stream_idx = stream_idx @@ -45,8 +50,12 @@ def is_indexed(self) -> bool: return self._scan_source.is_indexed @property - def fields(self) -> FieldTypes: + def field_types(self) -> FieldTypes: """Field types are present in the LidarScan objects on read from iterator""" + return self._scan_source.field_types[self._stream_idx] + + @property + def fields(self) -> List[str]: return self._scan_source.fields[self._stream_idx] @property @@ -57,7 +66,8 @@ def scans_num(self) -> Optional[int]: def __len__(self) -> int: if self.scans_num is None: - raise TypeError("len is not supported on live or non-indexed sources") + raise TypeError( + "len is not supported on live or non-indexed sources") return self.scans_num # NOTE: we need to consider a case without collation of scans @@ -75,22 +85,41 @@ def _seek(self, key: int) -> None: raise NotImplementedError def __getitem__(self, key: Union[int, slice] - ) -> Union[Optional[LidarScan], List[Optional[LidarScan]]]: + ) -> Union[Optional[LidarScan], ScanSource]: """Indexed access and slices support""" if isinstance(key, int): - return self._scan_source[key][self._stream_idx] - elif isinstance(key, slice): scans_list = self._scan_source[key] - scans_list = typing.cast(List[List[Optional[LidarScan]]], scans_list) - return [ls[self._stream_idx] for ls in scans_list] if scans_list else None + scans_list = cast(List[Optional[LidarScan]], scans_list) + return scans_list[self._stream_idx] + elif isinstance(key, slice): + return self.slice(key) raise TypeError( f"indices must be integer or slice, not {type(key).__name__}") - # TODO: should this actually the parent scan source? any object why not def close(self) -> None: """Release the underlying resource, if any.""" - self._scan_source.close() + pass def __del__(self) -> None: """Automatic release of any underlying resource.""" self.close() + + def _slice_iter(self, key: slice) -> Iterator[Optional[LidarScan]]: + L = len(self) + k = ForwardSlicer.normalize(key, L) + count = k.stop - k.start + if count <= 0: + return iter(()) + return ForwardSlicer.slice_iter(iter(self), k) + + def slice(self, key: slice) -> ScanSource: + # NOTE: rather than creating a SlicedScanSource use the combination + # of two decorators to achieve the same functionality + from ouster.sdk.client.multi_sliced_scan_source import MultiSlicedScanSource + L = len(self) + k = ForwardSlicer.normalize(key, L) + if k.step < 0: + raise TypeError("slice() can't work with negative step") + sliced = MultiSlicedScanSource(self._scan_source, k) # type: ignore + scan_source = ScanSourceAdapter(sliced, self._stream_idx) # type: ignore + return cast(ScanSource, scan_source) diff --git a/python/src/ouster/sdk/convert_to_legacy.py b/python/src/ouster/sdk/convert_to_legacy.py deleted file mode 100644 index b9b6e79a..00000000 --- a/python/src/ouster/sdk/convert_to_legacy.py +++ /dev/null @@ -1,31 +0,0 @@ -""" -Copyright (c) 2023, Ouster, Inc. -All rights reserved. -""" - -# TODO[UN]: move to ouster/cli/util - -import argparse - -from ouster.sdk import client - - -def main() -> None: - descr = """Convert non-legacy metadata to legacy format""" - - parser = argparse.ArgumentParser(description=descr) - parser.add_argument('metadata_path') - parser.add_argument('-o', '--output-path') - - args = parser.parse_args() - - with open(args.metadata_path) as json: - print(f"Reading metadata from: {args.metadata_path}") - legacy = client.convert_to_legacy(json.read()) - - if args.output_path: - with open(args.output_path, "w") as output: - print(f"Writing converted legacy metadata to: {args.output_path}") - output.write(legacy) - else: - print(legacy) diff --git a/python/src/ouster/sdk/examples/client.py b/python/src/ouster/sdk/examples/client.py index 2f3ee912..7b88399f 100644 --- a/python/src/ouster/sdk/examples/client.py +++ b/python/src/ouster/sdk/examples/client.py @@ -97,7 +97,7 @@ def fetch_metadata(hostname: str) -> None: print(f" serial no: {source.metadata.sn}") print(f" firmware version: {source.metadata.fw_rev}") print(f" product line: {source.metadata.prod_line}") - print(f" lidar mode: {source.metadata.mode}") + print(f" lidar mode: {source.metadata.config.lidar_mode}") print(f"Writing to: {hostname}.json") # write metadata to disk @@ -150,7 +150,7 @@ def filter_3d_by_range_and_azimuth(hostname: str, (range_min * 1000)) # get first 3/4 of scan - to_col = math.floor(metadata.mode.cols * 3 / 4) + to_col = math.floor(metadata.format.columns_per_frame * 3 / 4) xyz_filtered = xyz_filtered[:, 0:to_col, :] # [doc-etag-filter-3d] @@ -260,7 +260,7 @@ def record_pcap(hostname: str, # make a descriptive filename for metadata/pcap files time_part = datetime.now().strftime("%Y%m%d_%H%M%S") meta = source.metadata - fname_base = f"{meta.prod_line}_{meta.sn}_{meta.mode}_{time_part}" + fname_base = f"{meta.prod_line}_{meta.sn}_{meta.config.lidar_mode}_{time_part}" print(f"Saving sensor metadata to: {fname_base}.json") source.write_metadata(f"{fname_base}.json") diff --git a/python/src/ouster/sdk/examples/open3d.py b/python/src/ouster/sdk/examples/open3d.py index 809bc1f5..99d74a79 100644 --- a/python/src/ouster/sdk/examples/open3d.py +++ b/python/src/ouster/sdk/examples/open3d.py @@ -136,7 +136,7 @@ def canvas_set_image_data(pic: o3d.geometry.TriangleMesh, np.copyto(pic_img_data, img_data) -def range_for_field(f: client.ChanField) -> client.ChanField: +def range_for_field(f: str) -> str: if f in (client.ChanField.RANGE2, client.ChanField.SIGNAL2, client.ChanField.REFLECTIVITY2): return client.ChanField.RANGE2 @@ -248,7 +248,7 @@ def update_data(vis: o3d.visualization.Visualizer): ts = time.monotonic() # update data at scan frequency to avoid blocking the rendering thread - if not paused and ts - last_ts >= 1 / metadata.mode.frequency: + if not paused and ts - last_ts >= 1 / metadata.format.fps: scan = next(scans_iter) update_data(vis) last_ts = ts diff --git a/python/src/ouster/sdk/examples/osf.py b/python/src/ouster/sdk/examples/osf.py index 9e70a354..49b3432b 100644 --- a/python/src/ouster/sdk/examples/osf.py +++ b/python/src/ouster/sdk/examples/osf.py @@ -1,6 +1,5 @@ import argparse import os -import numpy as np def osf_read_scans(osf_file: str) -> None: @@ -109,18 +108,14 @@ def osf_slice_scans(osf_file: str) -> None: # New field types should be a subset of fields in encoded LidarScan so we just assume that # RANGE, SIGNAL and REFLECTIVITY fields will be present in the input OSF file. - new_field_types = dict({ - client.ChanField.RANGE: np.dtype('uint32'), - client.ChanField.SIGNAL: np.dtype('uint16'), - client.ChanField.REFLECTIVITY: np.dtype('uint16') - }) + fields_to_write = [client.ChanField.RANGE, client.ChanField.SIGNAL, client.ChanField.REFLECTIVITY] output_file_base = os.path.splitext(os.path.basename(osf_file))[0] output_file = output_file_base + '_sliced.osf' # Create Writer with a subset of fields to save (i.e. slicing will happen # automatically on write) - writer = osf.Writer(output_file, scans.metadata, new_field_types) + writer = osf.Writer(output_file, scans.metadata, fields_to_write) # Read scans and write back for ts, scan in scans.withTs(): diff --git a/python/src/ouster/sdk/examples/pcap.py b/python/src/ouster/sdk/examples/pcap.py index 67f00b35..bb36265a 100644 --- a/python/src/ouster/sdk/examples/pcap.py +++ b/python/src/ouster/sdk/examples/pcap.py @@ -250,21 +250,30 @@ def pcap_read_packets( ) -> None: """Basic read packets example from pcap file. """ # [doc-stag-pcap-read-packets] + packet_format = client.PacketFormat(metadata) for packet in source: if isinstance(packet, client.LidarPacket): # Now we can process the LidarPacket. In this case, we access # the measurement ids, timestamps, and ranges - measurement_ids = packet.measurement_id - timestamps = packet.timestamp - ranges = packet.field(client.ChanField.RANGE) + measurement_ids = packet_format.packet_header(client.ColHeader.MEASUREMENT_ID, packet.buf) + timestamps = packet_format.packet_header(client.ColHeader.TIMESTAMP, packet.buf) + ranges = packet_format.packet_field(client.ChanField.RANGE, packet.buf) print(f' encoder counts = {measurement_ids.shape}') print(f' timestamps = {timestamps.shape}') print(f' ranges = {ranges.shape}') elif isinstance(packet, client.ImuPacket): # and access ImuPacket content - print(f' acceleration = {packet.accel}') - print(f' angular_velocity = {packet.angular_vel}') + ax = packet_format.imu_la_x(packet.buf) + ay = packet_format.imu_la_y(packet.buf) + az = packet_format.imu_la_z(packet.buf) + + wx = packet_format.imu_av_x(packet.buf) + wy = packet_format.imu_av_y(packet.buf) + wz = packet_format.imu_av_z(packet.buf) + + print(f' acceleration = {ax}, {ay}, {az}') + print(f' angular_velocity = {wx}, {wy}, {wz}') # [doc-etag-pcap-read-packets] diff --git a/python/src/ouster/sdk/mapping/__init__.py b/python/src/ouster/sdk/mapping/__init__.py new file mode 100644 index 00000000..6acff2ee --- /dev/null +++ b/python/src/ouster/sdk/mapping/__init__.py @@ -0,0 +1,45 @@ +""" +Copyright (c) 2024, Ouster, Inc. +All rights reserved. + +Slam Module +""" +import json +import importlib.resources +from packaging.markers import Marker + +mapping_has_platform_support = True + + +def get_unsupported(): + result = [] + with importlib.resources.open_binary("ouster.sdk.mapping", 'requirements.json') as f: + requirements = json.loads(f.read()) + + for item in requirements: + marker = Marker(requirements[item]['marker']) + if not marker.evaluate(): + result.append(item) + return result + + +mapping_exception = None +try: + from .slam_backend import SLAMBackend # noqa: F401 + from .kiss_backend import KissBackend # noqa: F401 +except Exception as e: # noqa: E722 + mapping_exception = e + +if mapping_exception is not None: + mapping_error = "Mapping not supported on this platform." + unsupported = get_unsupported() + if len(unsupported) > 0: + mapping_error += "The following dependencies are not supported: " + mapping_error += ", ".join(unsupported) + mapping_error += f" Additional Info: {mapping_exception}" + class SLAMBackend: # type: ignore # noqa + def __init__(*kwargs): # noqa + raise Exception(mapping_error) # noqa + class KissBackend: # type: ignore # noqa + def __init__(*kwargs): # noqa + raise Exception(mapping_error) # noqa diff --git a/python/src/ouster/sdk/mapping/internal/calc_loop_drift.py b/python/src/ouster/sdk/mapping/internal/calc_loop_drift.py new file mode 100644 index 00000000..d4ed45eb --- /dev/null +++ b/python/src/ouster/sdk/mapping/internal/calc_loop_drift.py @@ -0,0 +1,122 @@ +import os +import argparse +import logging +import open3d +import math +import numpy as np +from pathlib import Path +from datetime import datetime +from collections import deque +from ouster.sdk import open_source +import ouster.sdk.client as client +import ouster.sdk.util.pose_util as pu +from ouster.sdk.mapping.internal.stitch import scans_to_cloud + +logger = logging.getLogger() +logging.basicConfig(level=logging.INFO) + +# slam pose and icp pose difference threadhold +# if greater than the threadhold, the icp mostly likely has bad alignment +slam_icp_diff_thread = 3 + + +def off_score(icp_diff_log): + rot_weight = 0.5 + trans_weight = 0.5 + + rot_off = math.sqrt((icp_diff_log[0])**2 + + (icp_diff_log[1])**2 + + (icp_diff_log[2])**2) + + trans_off = math.sqrt((icp_diff_log[3])**2 + + (icp_diff_log[4])**2 + + (icp_diff_log[5])**2) + + return rot_weight * rot_off + trans_weight * trans_off + + +def run(osf_file, debug): + source = open_source(osf_file) + info = source.metadata + window = info.format.column_window + xyzlut = client.XYZLut(info, use_extrinsics=True) + + first_scan = None + last_scan = None + for scan in source: + if scan.complete(window): + if not first_scan: + first_scan = scan + else: + last_scan = scan + + if not first_scan or not last_scan: + logger.error("Valid scans are less than 2, Exits") + return + + logger.info(f"first scan ts = {first_scan.timestamp[client.first_valid_column(first_scan)]}") + logger.info(f"last scan ts = {last_scan.timestamp[client.first_valid_column(last_scan)]}") + + first_scan_pose = first_scan.pose[client.first_valid_column(first_scan)] + last_scan_pose = last_scan.pose[client.first_valid_column(last_scan)] + first_cloud = scans_to_cloud(deque([first_scan]), info, xyzlut) + last_cloud = scans_to_cloud(deque([last_scan]), info, xyzlut) + + max_correspondence_distance = 5 + criteria = open3d.pipelines.registration.ICPConvergenceCriteria(relative_fitness=1e-12, + relative_rmse=1e-12, + max_iteration=2000) + icp_result = open3d.pipelines.registration.registration_icp( + first_cloud, last_cloud, max_correspondence_distance, + estimation_method=open3d.pipelines.registration.TransformationEstimationPointToPlane(), + criteria=criteria) + + slam_diff_h = np.matmul(np.linalg.inv(first_scan_pose), last_scan_pose) + slam_diff_log = pu.log_pose(slam_diff_h) + + icp_diff_h = icp_result.transformation.copy() + icp_diff_log = pu.log_pose(icp_diff_h) + + slam_icp_diff = slam_diff_log - icp_diff_log + + logger.info("ICP Pose Diff = %s", icp_diff_log) + + if debug: + logger.info("SLAM Pose Diff = %s", slam_diff_log) + logger.info("SLAM ICP Diff = %s", slam_icp_diff) + + slam_icp_off_score = off_score(slam_icp_diff) + logger.info(f"ICP align mismatch: {slam_icp_off_score}") + + if slam_icp_off_score > slam_icp_diff_thread: + logger.info("SLAM pose is far off than ICP alignment. ICP may mis-matched." + "Use --debug to view the point clouds. Or SLAM has huge drift.") + + # the smaller off the better slam result + + if debug: + folder_name = "slam_eval_" + datetime.now().strftime("%Y%m%d_%H%M%S") + os.makedirs(Path(folder_name)) + open3d.io.write_point_cloud(folder_name + "/first_cloud.ply", first_cloud) + open3d.io.write_point_cloud(folder_name + "/last_cloud.ply", last_cloud) + aligned_first_cloud = first_cloud.transform(icp_diff_h) + open3d.io.write_point_cloud(folder_name + "/aligned_first_cloud.ply", aligned_first_cloud) + logger.info(f"Point Cloud Saved at {folder_name}. Expect the aligned_first_cloud " + f"perfectly matched with the last_cloud. Use cloudcompare to view") + + logger.info(" !! SLAM Start/End Poses Drift = %s", off_score(icp_diff_log)) + + +def main(): + parser = argparse.ArgumentParser(description="Calculate loop type map drift.") + parser.add_argument("slam_osf", help="slam output osf file") + parser.add_argument('--debug', action='store_true', default=False, + help='debug point cloud saving and pose printout') + + args = parser.parse_args() + + run(args.slam_osf, args.debug) + + +if __name__ == "__main__": + main() diff --git a/python/src/ouster/sdk/mapping/internal/pcaps_to_osf.py b/python/src/ouster/sdk/mapping/internal/pcaps_to_osf.py new file mode 100644 index 00000000..97ff875d --- /dev/null +++ b/python/src/ouster/sdk/mapping/internal/pcaps_to_osf.py @@ -0,0 +1,125 @@ +import logging +import sys +import numpy as np +from pathlib import Path + +import ouster.sdk.osf as osf +from ouster.sdk import client +from ouster.sdk.open_source import open_source + +logging.basicConfig(level=logging.INFO) +logger = logging.getLogger('mapping_tool') + + +def find_pcap_files(directory): + directory_path = Path(directory) + return [str(file_path) for file_path in directory_path.glob('*.pcap')] + + +def update_scan_ts(scan, ts_diff): + for i, timestamp in enumerate(scan.timestamp): + if timestamp != 0: + scan.timestamp[i] -= ts_diff + + +def write_osf_from_source(data_source, osf_writer, ts_diff, last_ts): + cal_diff = True + + for scan in data_source: + if last_ts != 0 and cal_diff: + cur_ts = client.first_valid_column_ts(scan) + ts_diff = cur_ts - last_ts + cal_diff = False + continue + + update_scan_ts(scan, ts_diff) + ts = client.first_valid_column_ts(scan) + osf_writer.save(0, scan, ts) + + return ts + + +def pcaps_to_osfs(files_path, osf_out_name): + """ + One pcap to One osf conversion. Use it in stitch.py only. + Because stitch.py output one OSF file and OSF has to have increasing ts. So + we have to manipulate timestamp in scan here too + + Require the discreted pcap files are recorded while sensor is always on, that + ensure the timestamps are in the similiar ranges and ascendant. + + Unknow behavior if sensor is restarted. Timestampes in very different ranges + may cause int overflow issue or other unknow issues. + """ + osf_out_name = Path(osf_out_name) + pcap_files = find_pcap_files(files_path) + sorted_files = sorted(pcap_files) + ts_diff = 0 + last_ts = 0 + out_dir = osf_out_name.parent + file_wo_ext = osf_out_name.stem + outfile_ext = osf_out_name.suffix + + new_field_types = dict({ + client.ChanField.RANGE: np.dtype('uint32'), + client.ChanField.SIGNAL: np.dtype('uint16'), + client.ChanField.REFLECTIVITY: np.dtype('uint16')}) + + for idx, file in enumerate(sorted_files): + data_source = open_source(file, sensor_idx=0) + osf_filename = str( + out_dir / Path(file_wo_ext + str(idx + 1) + outfile_ext)) + logger.info(f"Convert {file} to {osf_filename} ") + osf_writer = osf.Writer(osf_filename, data_source.metadata, new_field_types) + + last_ts = write_osf_from_source( + data_source, osf_writer, ts_diff, last_ts) + + osf_writer.close() + + +def pcaps_to_osf(files_path, osf_out_name): + """ + Combined pcap files into one osf file. Manipulate ts and make discrete pcap + files into a continous osf file. + + Require the discreted pcap files are recorded while sensor is always on, that + ensure the timestamps are in the similiar ranges and ascendant. + + Unknow behavior if sensor is restarted. Timestampes in very different ranges + may cause int overflow issue or other unknow issues. + """ + pcap_files = find_pcap_files(files_path) + sorted_files = sorted(pcap_files) + ts_diff = 0 + last_ts = 0 + new_field_types = dict({ + client.ChanField.RANGE: np.dtype('uint32'), + client.ChanField.SIGNAL: np.dtype('uint16'), + client.ChanField.REFLECTIVITY: np.dtype('uint16')}) + + peak_reader = open_source(str(pcap_files[0])) + info = peak_reader.metadata + del peak_reader + osf_writer = osf.Writer(osf_out_name, info, new_field_types) + + for file in sorted_files: + logger.info(f"Convert {file} to {osf_out_name} ") + data_source = open_source(file, sensor_idx=0) + last_ts = write_osf_from_source( + data_source, osf_writer, ts_diff, last_ts) + + osf_writer.close() + + +if __name__ == "__main__": + if len(sys.argv) != 3: + logger.error("Usage: python3 pcaps_to_osf.py PCAP_Files_PATH/ PAHT/out_filename\nExit") + exit(1) + + files_path = Path(sys.argv[1]) + osf_out_name = sys.argv[2] + logger.info(f"pcap files path {sys.argv[1]}") + logger.info(f"output osf filename {sys.argv[2]}") + # pcaps_to_osf(files_path, osf_out_name) + pcaps_to_osfs(files_path, osf_out_name) diff --git a/python/src/ouster/sdk/mapping/internal/stitch.py b/python/src/ouster/sdk/mapping/internal/stitch.py new file mode 100644 index 00000000..801b045a --- /dev/null +++ b/python/src/ouster/sdk/mapping/internal/stitch.py @@ -0,0 +1,341 @@ +import logging +import os +import shutil +import sys +import subprocess +import shlex +from collections import deque +from pathlib import Path +from typing import Deque, Union, cast + +from pcaps_to_osf import pcaps_to_osfs # type: ignore + +import ouster.sdk.util.pose_util as pu +from ouster.sdk.open_source import open_source +from ouster.sdk import client, osf + +try: + import numpy as np + import open3d +except ImportError: + raise + +logger = logging.getLogger() +logging.basicConfig(level=logging.DEBUG) +logger = logging.getLogger('mapping_tool') + +MIN_DIST = 3 # meter +MIN_Z = 2 # meter +MAX_Z = 12 # meter +PC_MATCH_CHECK = True +VOXEL_SIZE = 1.8 + +PathSource = Union[str, Path] +Output = Union[str, Path] +Meta = Union[str, Path] + + +def compute_unit_normals(dewarped_points, scan, info): + """ + Use 2D depth image to calculate unit normal value for each valid pixel + Ignore range less than mis_dist + + return a stagger normals value image + """ + normals = np.zeros_like(dewarped_points) + ranges = client.destagger(info, scan.field(client.ChanField.RANGE)) + destagger_points = client.destagger(info, dewarped_points) + + for u in range(scan.h): + for v in range(scan.w): + if ranges[u][v] < MIN_DIST: + continue + above = (max(0, u - 1), v) + below = (min(scan.h - 1, u + 1), v) + left = (u, (v - 1 + scan.w) % scan.w) + right = (u, (v + 1) % scan.w) + + if (ranges[above[0]][above[1]] < MIN_DIST or + ranges[below[0]][below[1]] < MIN_DIST or + ranges[left[0]][left[1]] < MIN_DIST or + ranges[right[0]][right[1]] < MIN_DIST): + continue + diff_u = destagger_points[above[0]][above[1] + ] - destagger_points[below[0]][below[1]] + diff_v = destagger_points[left[0]][left[1] + ] - destagger_points[right[0]][right[1]] + normal = np.cross(diff_u, diff_v) + n = np.sqrt(np.sum(normal ** 2)) + if n == 0: + continue + unit_normal = normal / n + normals[u][v] = unit_normal + + stagger_normals = client.destagger(info, normals, True) + return stagger_normals + + +def scans_to_cloud(scans: Deque[client.LidarScan], + info, xyzlut, target_pose=np.eye(4)): + """ + Convert LidarScan to open3D PointCloud + Filter out point which range is less than min_range and add normal value for valid points + + """ + cloud = open3d.geometry.PointCloud() + comb_points = np.empty((0, 3)) + comb_normals = np.empty((0, 3)) + + for scan in scans: + local_points = xyzlut(scan) + + # filter out ground and ceiling for better alignment + flat_local_points = local_points.reshape( + local_points.shape[0] * local_points.shape[1], 3) + filter_idx = (flat_local_points[:, 2] >= MIN_Z) & ( + flat_local_points[:, 2] <= MAX_Z) + global_points = pu.dewarp( + local_points, + scan_pose=target_pose, + column_poses=scan.pose) + flat_global_points = global_points.reshape( + global_points.shape[0] * global_points.shape[1], 3) + filtered_points = flat_global_points[filter_idx] + + # normal calculation + normals = compute_unit_normals(global_points, scan, info) + flat_normals = normals.reshape(normals.shape[0] * normals.shape[1], 3) + filtered_normals = flat_normals[filter_idx] + + comb_points = np.vstack((comb_points, filtered_points)) + comb_normals = np.vstack((comb_normals, filtered_normals)) + + cloud.points = open3d.utility.Vector3dVector(comb_points) + cloud.normals = open3d.utility.Vector3dVector(comb_normals) + return cloud + + +def apply_icp(scan, icp_diff, last_pose): + """ + Update scan per column pose using icp results + + """ + pose_corrected = np.matmul(icp_diff, np.matmul(last_pose, scan.pose)) + scan.pose[:] = pose_corrected + + +def register_two_scans(path, target_scan, source_scan, xyzlut, + target_pose, info, idx, debug=True) -> np.ndarray: + """ + Update scan per column pose using icp results + + """ + target_cloud = scans_to_cloud(target_scan, info, xyzlut) + source_cloud = scans_to_cloud(source_scan, info, xyzlut, target_pose) + + max_correspondence_distance = 4 + criteria = open3d.pipelines.registration.ICPConvergenceCriteria(relative_fitness=1e-12, + relative_rmse=1e-12, + max_iteration=900) + icp_result = open3d.pipelines.registration.registration_icp( + source_cloud, target_cloud, max_correspondence_distance, + estimation_method=open3d.pipelines.registration.TransformationEstimationPointToPlane(), + criteria=criteria) + + result_cloud = source_cloud.transform(icp_result.transformation) + + """ + TODO remove the printout after development + """ + if debug: + logger.debug(f"transformation = {icp_result.transformation}") + open3d.io.write_point_cloud(str(path) + + "/target_cloud_normal" + + str(idx) + + ".ply", + target_cloud) + open3d.io.write_point_cloud(str(path) + + "/source_cloud_normal" + + str(idx) + + ".ply", + source_cloud) + open3d.io.write_point_cloud(str(path) + + "/result_cloud_normal" + + str(idx) + + ".ply", + result_cloud) + + return icp_result.transformation + +# Have this function to avoid A010.ply < A02.ply case + + +def sort_key(file_name): + name, ext = os.path.splitext(file_name) + numeric_part = ''.join(filter(str.isdigit, name)) + return (name.replace(numeric_part, ''), int( + numeric_part)) if numeric_part else (file_name, 0) + + +def stitch_osfs(osf_path: PathSource, output: Output): + """ + Stitch OSF files into one. + + Use the last N scans and the first N scans to stitch clouds. + This function works only for single lidar setups. + + Args: + osf_path (Path, str): Path to the directory containing OSF files. + output (Path, str): Path to the output file. + """ + osf_path = Path(osf_path) + output = Path(output) + if osf_path.is_dir(): + osf_files = list(osf_path.glob("*.osf")) + else: + logger.error("Expect a directory which contains OSF files") + return + + logger.info(f"Combining {len(osf_files)} OSF files") + + # Sort the list of OSF files + sorted_osf_files = sorted(osf_files, key=sort_key) + + fields_to_write = [client.ChanField.RANGE, client.ChanField.SIGNAL, client.ChanField.REFLECTIVITY] + + # peak the first OSF file + peak_reader = open_source(str(sorted_osf_files[0])) + info = peak_reader.metadata + del peak_reader + xyzlut = client.XYZLut(info, use_extrinsics=True) # type: ignore + osf_writer = osf.Writer(str(output), info, fields_to_write) + + # large max_scans could improve the stitch accuracy but increase the + # processing time + max_scans = 5 + last_pose = np.eye(4) + icp_diff = np.eye(4) + first_scan = True + last_scans_deque: Deque[client.LidarScan] = deque(maxlen=max_scans) + curr_scans_deque: Deque[client.LidarScan] = deque(maxlen=max_scans) + + for file_id, file in enumerate(sorted_osf_files): + logger.info(f"Processing #{file_id} OSF file {file}") + reader = osf.Reader(str(file)) + for msg in reader.messages(): + if msg.of(osf.LidarScanStream): + # Skip the first scan from second OSF file as the first scan per + # column poses are all eye(4) when SLAM restart. It will affect + # ICP matches + if file_id != 0 and first_scan: + first_scan = False + continue + + obj = msg.decode() + scan = cast(client.LidarScan, obj) + if file_id != 0: + if len(curr_scans_deque) < max_scans: + curr_scans_deque.append(scan) + continue + + if np.all(icp_diff == np.eye(4)): + last_scan = last_scans_deque[-1] + last_pose = last_scan.pose[client.first_valid_column( + last_scan)] + icp_diff = register_two_scans(osf_path, last_scans_deque, curr_scans_deque, + xyzlut, last_pose, info, file_id, PC_MATCH_CHECK) + for stored_scan in curr_scans_deque: + apply_icp(stored_scan, icp_diff, last_pose) + osf_writer.save(0, stored_scan) + last_scans_deque.append(stored_scan) + apply_icp(scan, icp_diff, last_pose) + + osf_writer.save(0, scan) + last_scans_deque.append(scan) + + icp_diff = np.eye(4) + curr_scans_deque.clear() + first_scan = True + + osf_writer.close() + + +def process_osf_files(osf_path: PathSource, output: Output): + """ + Run slam on given OSF files. + This function works only for single lidar setups. + + Args: + osf_path (Path, str): Path of the directory containing osf files. + output (Path, str): Path to the output file. + """ + osf_path = Path(osf_path) + output = Path(output) + if osf_path.is_dir(): + osf_files = list(osf_path.glob("*.osf")) + elif osf_path.is_file(): + osf_files = [osf_path] + else: + logger.error( + f"Input {osf_path} is neither a directory nor a file") + return + + sorted_osf_files = sorted(osf_files, key=sort_key) + logger.info(f"Start to process {len(osf_files)} osf files") + + file_wo_ext = str(output.stem) + for idx, _osf in enumerate(sorted_osf_files): + osf_output_file = os.path.join( + output.parent, file_wo_ext + str(idx) + '.osf') + execution_command = f"ouster-cli source {_osf} slam -v {VOXEL_SIZE} save --ts lidar {osf_output_file}" + process_slam = subprocess.run( + shlex.split(execution_command), + text=True, + capture_output=True) + logger.info(process_slam.stdout) + if process_slam.returncode != 0: + logger.error(f"Error executing slam command: {process_slam.stderr}") + sys.exit(1) + else: + logger.info("Save SLAM output {osf_output_file}") + + +def delete_and_create_folder(folder_path): + path_obj = Path(folder_path) + + if path_obj.exists(): + shutil.rmtree(folder_path) + logger.info(f"Old result '{folder_path}' deleted") + + path_obj.mkdir(parents=True, exist_ok=True) + logger.info(f"New folder {folder_path} created for result saving") + + +if __name__ == "__main__": + if len(sys.argv) != 3: + logger.error( + "Usage: python3 stitich.py PATH/data.pcap PATH/output.osf \nExit") + exit(1) + + logger.info(f"pcap file {sys.argv[1]}") + logger.info(f"output file {sys.argv[2]}") + + input_path = Path(sys.argv[1]) + output_osf_file = Path(sys.argv[2]) + + # create folder to store the cache results + curr_dir = Path.cwd() + osf_dir = curr_dir / 'stitch_osf_cache' + converted_osf_dir = osf_dir / 'osf' + slammed_osf_dir = osf_dir / 'slam_processed_osf' + converted_osf_results = converted_osf_dir / 'converted.osf' + slamed_osf = slammed_osf_dir / 'processed.osf' + + # clean cache and avoid unexpected cache files merging + delete_and_create_folder(osf_dir) + delete_and_create_folder(converted_osf_dir) + delete_and_create_folder(slammed_osf_dir) + + pcaps_to_osfs(input_path, osf_out_name=converted_osf_results) + process_osf_files(converted_osf_dir, output=slamed_osf) + stitch_osfs(slammed_osf_dir, output=output_osf_file) diff --git a/python/src/ouster/sdk/mapping/internal/traj_eval.py b/python/src/ouster/sdk/mapping/internal/traj_eval.py new file mode 100644 index 00000000..0b5e7117 --- /dev/null +++ b/python/src/ouster/sdk/mapping/internal/traj_eval.py @@ -0,0 +1,368 @@ +import argparse +import datetime +import logging +import math +import os + +import numpy as np + +import ouster.sdk.client as client +import ouster.sdk.osf as osf +import ouster.sdk.util.pose_util as pu + +try: + import yaml # type: ignore + _no_yaml = False +except ImportError: + # PyYAML is needed for this internal tool. It rely on internal users + # manually install + _no_yaml = True + +try: + import matplotlib.pyplot as plt # type: ignore + _no_plotlib = False +except ImportError: + # matplotlib is needed for this internal tool. It rely on internal users + # manually install + _no_plotlib = True + +logger = logging.getLogger() +logging.basicConfig(level=logging.INFO) + +truth_TrajEval = None +cmpar_TrajEval = None +start_ts = None +end_ts = None +s_to_ns = 1e9 +# valid if ls cnt bewteen two osf is less than 5% +valid_ls_cnt_diff = 0.05 + + +def cal_dist(point): + return math.sqrt(point[3]**2 + point[4]**2 + point[5]**2) + + +def cal_diff_percnt(diff_part, dist_part): + return (diff_part / dist_part) * 100 + + +def loadOSFPoses(ground_truth, comparison, out_yaml): + """ + Load trajectory from OSF files and get overal drift info + + """ + def process_osf_messages(osf_scans, trajectory_poses): + osf_start_ts = None + osf_end_ts = None + scan_cnt = 0 + for scan in osf_scans: + start_col = client.core.first_valid_column(scan) + end_col = client.core.last_valid_column(scan) + start_ts = scan.timestamp[start_col] + end_ts = scan.timestamp[end_col] + if not osf_start_ts: + osf_start_ts = start_ts + osf_end_ts = end_ts + trajectory_poses.append((start_ts, scan.pose[start_col].copy())) + trajectory_poses.append((end_ts, scan.pose[end_col].copy())) + scan_cnt += 1 + + return { + 'ls_count': scan_cnt, + 'osf_start_ts': osf_start_ts, + 'osf_end_ts': osf_end_ts} + + truth_TrajPoses = [] + cmpar_TrajPoses = [] + + global truth_TrajEval, cmpar_TrajEval, start_ts, end_ts + + truth_osf_scans = osf.Scans(ground_truth) + + truth_results = process_osf_messages(truth_osf_scans, truth_TrajPoses) + + # exit if too short + if truth_results['ls_count'] < 15: + logger.error("Too small. No need to compare. Nothing run and Exit") + exit(1) + truth_TrajEval = pu.TrajectoryEvaluator(truth_TrajPoses) + + cmpar_osf_scans = osf.Scans(comparison) + cmpar_results = process_osf_messages(cmpar_osf_scans, cmpar_TrajPoses) + cmpar_TrajEval = pu.TrajectoryEvaluator(cmpar_TrajPoses) + + start_ts = max(truth_results['osf_start_ts'], cmpar_results['osf_start_ts']) + end_ts = min(truth_results['osf_end_ts'], cmpar_results['osf_end_ts']) + + # grouth truth / to compare + result_numb_ratio = truth_results['ls_count'] / cmpar_results['ls_count'] + if result_numb_ratio < (1 - valid_ls_cnt_diff) or result_numb_ratio > (1 + valid_ls_cnt_diff): + logger.error(f"Two OSF files' lidarscan obj are more than {valid_ls_cnt_diff*100} %") + exit(1) + + pose_diff = pu.log_pose( + np.matmul( + np.linalg.inv( + truth_TrajEval.pose_at( + end_ts - 1)), + cmpar_TrajEval.pose_at( + end_ts - 1))) + + # Save 3 decimal. Pose [0:2] is angular and [3-5] is translation + duration = round((end_ts - start_ts) / s_to_ns, 3) + pose_diff_wx = round(pose_diff[0], 3) + pose_diff_wy = round(pose_diff[1], 3) + pose_diff_wz = round(pose_diff[2], 3) + pose_diff_x = round(pose_diff[3], 3) + pose_diff_y = round(pose_diff[4], 3) + pose_diff_z = round(pose_diff[5], 3) + pose_diff_dist = round(cal_dist(pose_diff), 3) + logger.info(f"Duration {duration} second") + logger.info( + "Final pose angular diff (rad): \n" + f"wx({pose_diff_wx}), wy({pose_diff_wy}), wz({pose_diff_wz})") + logger.info( + "Final pose translation diff (m): \n" + f"x({pose_diff_x}), y({pose_diff_y}), z({pose_diff_z})") + logger.info(f"Pose diff dist : {pose_diff_dist} meter") + + if out_yaml: + data = { + "Global Comparison": { + "Duration (s)": f"{duration}", + "Final pose angular diff (rad)": { + "wx": f"{pose_diff_wx}", + "wy": f"{pose_diff_wy}", + "wz": f"{pose_diff_wz}" + }, + "Final pose translation diff (m)": { + "x": f"{pose_diff_x}", + "y": f"{pose_diff_y}", + "z": f"{pose_diff_z}" + }, + "Final Pose Diff Dist (m)": f"{pose_diff_dist}" + } + } + with open(out_yaml, 'a') as outfile: + yaml.dump(data, outfile) + + +def compare(out_yaml, ts_incrmt, print_incrmt): + """ + Compare the final pose between two input in orientation and translation + Compare two trajectories by small steps (ts_incrmt) and printout results in each + small period in translation only + See terminal printout and saved yaml file for more details + + """ + ns_ts_incrmt = int(ts_incrmt * s_to_ns) + array_shape = (6,) + zero_array = np.zeros(array_shape) + sum_diff_pose = zero_array + sum_pose = zero_array + sum_diff_pose_part = zero_array + sum_pose_part = zero_array + last_truth = truth_TrajEval.pose_at(start_ts + 1) + part_start_percnt = 0 + part_end_percnt = print_incrmt + dur_ts = end_ts - start_ts + total_count = 0 + partial_count = 0 + + if out_yaml: + data = {"Partially Comparison": {}} + + for ts in range(start_ts, end_ts, ns_ts_incrmt): + total_count += 1 + partial_count += 1 + truth_pose = truth_TrajEval.pose_at(ts) + cmpar_pose = cmpar_TrajEval.pose_at(ts) + + pose_diff = pu.log_pose(np.matmul(np.linalg.inv(truth_pose), cmpar_pose)) + sum_diff_pose = np.add(sum_diff_pose, np.abs(pose_diff)) + sum_diff_pose_part = np.add(sum_diff_pose_part, np.abs(pose_diff)) + + pose_travelled = pu.log_pose(np.matmul(np.linalg.inv(last_truth), truth_pose)) + sum_pose = np.add(sum_pose, np.abs(pose_travelled)) + sum_pose_part = np.add(sum_pose_part, np.abs(pose_travelled)) + + last_truth = truth_pose + if ts >= part_end_percnt * dur_ts + start_ts: + dist_part = cal_dist(sum_pose_part) + diff_part = cal_dist(sum_diff_pose_part) / partial_count + diff_part_percnt = cal_diff_percnt(diff_part, dist_part) + + key = f"{part_start_percnt * 100:.1f}% - {part_end_percnt * 100:.1f}%" + value = ( + f"travelled {dist_part:.3f} m and average diff" + f" {diff_part:.3f} m. Around {diff_part_percnt:.3f}% off" + ) + logger.info(f"{key}: {value}") + if out_yaml: + data["Partially Comparison"][key] = value + + part_start_percnt += print_incrmt + part_end_percnt += print_incrmt + sum_diff_pose_part = zero_array + sum_pose_part = zero_array + partial_count = 0 + + # Handle the last part. In default param, it handles 80% - 100% part + dist_part = cal_dist(sum_pose_part) + diff_part = cal_dist(sum_diff_pose_part) / partial_count + diff_part_percnt = cal_diff_percnt(diff_part, dist_part) + + key = f"{part_start_percnt * 100:.1f}% - {part_end_percnt * 100:.1f}%" + value = ( + f"travelled {dist_part:.3f} m and average diff" + f" {diff_part:.3f} m. Around {diff_part_percnt:.3f}% off" + ) + logger.info(f"{key}: {value}") + if out_yaml: + data["Partially Comparison"][key] = value + + # Total travelled and diff + dist_total = cal_dist(sum_pose) + diff_total = cal_dist(sum_diff_pose) / total_count + diff_total_percnt = cal_diff_percnt(diff_total, dist_total) + + logger.info(f"Total travelled {dist_total:.3f} m and average diff {diff_total:.3f} m. " + f"Off around {diff_total_percnt:.3f}%") + + if out_yaml: + key = "Overall Stat" + sub_key1 = "Travelled (m)" + sub_key2 = "Avg_diff (m)" + sub_key3 = "Off_percnt (%)" + value1 = f"{dist_total:.3f}" + value2 = f"{diff_total:.3f}" + value3 = f"{diff_part_percnt:.3f}" + with open(out_yaml) as infile: + old_data = yaml.safe_load(infile) + old_data["Global Comparison"][key] = {} + old_data["Global Comparison"][key][sub_key1] = value1 + old_data["Global Comparison"][key][sub_key2] = value2 + old_data["Global Comparison"][key][sub_key3] = value3 + with open(out_yaml, 'w') as outfile: + yaml.dump(old_data, outfile) + yaml.dump(data, outfile) + + +def plotTraj(out_dir, plot, print_incrmt): + """ + Plot and save the two trajectories + + """ + ns_ts_incrmt = int(print_incrmt * s_to_ns) + + ts_values = [] + for ts in range(start_ts, end_ts, ns_ts_incrmt): + ts_values.append(ts) + + truth_poses = truth_TrajEval.poses_at(ts_values) + cmpar_poses = cmpar_TrajEval.poses_at(ts_values) + + truth_x_values = truth_poses[:, 0, 3] + truth_y_values = truth_poses[:, 1, 3] + truth_z_values = truth_poses[:, 2, 3] + + cmpar_x_values = cmpar_poses[:, 0, 3] + cmpar_y_values = cmpar_poses[:, 1, 3] + cmpar_z_values = cmpar_poses[:, 2, 3] + + # Create a 3D scatter plot + fig = plt.figure() + ax = fig.add_subplot(projection='3d') + + ax.plot(truth_x_values, truth_y_values, truth_z_values, color='red', linestyle='-', label='Tray_Truth') + ax.plot(cmpar_x_values, cmpar_y_values, cmpar_z_values, color='blue', linestyle='-', label='Traj_Cmpar') + + ax.set_xlabel('X-axis') + ax.set_ylabel('Y-axis') + ax.set_zlabel('Z-axis') + ax.set_title('3D Plot with Top-Down View') + + # Set the view angle for top-down view + ax.view_init(elev=90, azim=0) # 90 degrees elevation (top-down view) + ax.legend() + + # Save the plot as a PNG image + if out_dir: + out_file = out_dir + '/Eval_Traj.png' + logger.info(f"out png saved at {out_file} ") + plt.savefig(out_dir + '/Eval_Traj_topdown.png', bbox_inches='tight') + + # Display the plot + if plot: + plt.show() + + +def main(): + parser = argparse.ArgumentParser(description="Evaluate SLAM results.") + parser.add_argument("truth_file", help="Path to grouth_truth SLAM output file A.osf") + parser.add_argument("cmpar_file", help="Path to to_compare SLAM output file B.osf") + parser.add_argument("--plot", action="store_true", help="Enable plotting") + parser.add_argument("--save", action="store_true", help="Enable saving evaluation results with auto naming") + parser.add_argument("--out_dir", type=str, help="Enable saving evaluation results and given the output dir") + parser.add_argument("--print_incrmt", type=float, default=0.2, + help="Printout percnt increment value default: 0.2, printout every 20 percnt. " + "Suggest choose between 0.05 - 0.2") + parser.add_argument("--cmpar_incrmt", type=float, default=0.1, + help="Compare ts increment value default: 0.1, unit is second. " + "Smaller value gives more precise results but takes a bit longer") + + args = parser.parse_args() + + if not args.save and not args.plot and not args.out_dir: + logger.warning("Warning: Plotting and saving results are disabled. Try --plot and --save options") + + if args.print_incrmt <= 0 or args.print_incrmt > 0.5: + logger.error("Error: --print_incrmt range is (0, 0.5]") + exit(1) + + if args.cmpar_incrmt <= 0: + logger.error("Error: --cmpar_incrmt needs to be greater than 0") + exit(1) + + out_dir = None + out_yaml = None + + if args.save or args.out_dir: + if args.out_dir: + out_dir = args.out_dir + args.save = True + else: + current_time = datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + out_dir = f"TrajEval_{current_time}" + + if not os.path.exists(out_dir): + os.mkdir(out_dir) + out_yaml = out_dir + '/results.yaml' + + logger.info(f"Evaluation results saved at '{out_dir}'.") + evaluation_info = { + "Evaluation SLAM results": { + "Ground Truth file": f"{args.truth_file}", + "To Compare file": f"{args.cmpar_file}" + } + } + with open(out_yaml, 'w') as outfile: + yaml.dump(evaluation_info, outfile) + + loadOSFPoses(args.truth_file, args.cmpar_file, out_yaml) + + compare(out_yaml, args.cmpar_incrmt, args.print_incrmt) + + if args.plot and _no_plotlib: + logger.error("Error: Missing matplotlib. Run 'pip install matplotlib' to install") + exit(1) + + if args.save and _no_yaml: + logger.error("Error: Missing PyYAML. Run 'pip install PyYAML' to install") + exit(1) + + plotTraj(out_dir, args.plot, args.print_incrmt) + + +if __name__ == "__main__": + main() diff --git a/python/src/ouster/sdk/mapping/kiss_backend.py b/python/src/ouster/sdk/mapping/kiss_backend.py new file mode 100644 index 00000000..d94ed20a --- /dev/null +++ b/python/src/ouster/sdk/mapping/kiss_backend.py @@ -0,0 +1,135 @@ +import math +import logging +import numpy as np +from typing import List, Tuple # noqa: F401 + +import ouster.sdk.mapping.ouster_kiss_icp as ouster_kiss_icp +import ouster.sdk.mapping.util as util +import ouster.sdk.client as client + +from .slam_backend import SLAMBackend + +logging.basicConfig(level=logging.INFO) +logger = logging.getLogger() + + +class KissBackend(SLAMBackend): + """Wraps kiss-icp odometry to use with Ouster pipelines.""" + + def __init__( + self, + info: client.SensorInfo, + use_extrinsics: bool = True, + max_range: float = 150.0, + min_range: float = 1.0, + voxel_size: float = -1.0, + live_stream: bool = False + ): + try: + from kiss_icp.kiss_icp import KissICP # type: ignore # noqa + except ImportError: + logger.error("kiss-icp, a package required for slam, is unsupported on " + "your platform. ") + raise + import kiss_icp.config # type: ignore # noqa + super().__init__(info, use_extrinsics) + # to store the middle valid timestamp of a scan and middle col pose + self.ts_pose = list(tuple()) # type: List[Tuple[int, np.ndarray]] + self.last_id = -1 + self.config = kiss_icp.config.KISSConfig(None) + self.config.data.deskew = True + self.config.data.max_range = max_range + self.config.data.min_range = min_range + self.voxel_size = voxel_size + self.live_stream = live_stream + self.scan_pixels = info.format.pixels_per_column * info.format.columns_per_frame + if voxel_size and voxel_size > 0: + self.config.mapping.voxel_size = voxel_size + logger.info(f"Kiss-ICP voxel map size is {voxel_size:.4g} m") + self._config_kiss_icp(self.config) + + def _config_kiss_icp(self, config): + self.kiss_icp = ouster_kiss_icp.KissICP(config) + + """Update the pose (per_column_global_pose) variable in scan and return""" + + def update(self, scan: client.LidarScan) -> client.LidarScan: + if not self.voxel_size or self.voxel_size <= 0: + self.voxel_size = self.get_voxel_size(scan) + self.config.mapping.voxel_size = self.voxel_size + self._config_kiss_icp(self.config) + logger.info(f"Auto voxel size calculated based on the first scan " + f"which is {self.voxel_size:.4g} m.") + + # Create normalized timetamps with shape (h, w) based on actual + # per-column timestamps + ts_norm = np.tile(util.getNormalizedTimestamps(scan), (self.h, 1)) + + # accumulate scan timestamps in parallel list to poses + scan_start_ts = client.first_valid_column_ts(scan) + scan_end_ts = client.core.last_valid_column_ts(scan) + scan_mid_ts = int((scan_start_ts + scan_end_ts) / 2) + + # filtering our zero returns makes it substantially faster for kiss-icp + sel_flag = scan.field(client.ChanField.RANGE) != 0 + xyz = self.xyz_lut(scan.field(client.ChanField.RANGE))[sel_flag] + + try: + self.kiss_icp.register_frame(xyz, ts_norm[sel_flag], scan_start_ts) + except Exception as e: + from kiss_icp import __version__ as kiss_icp_version + + logger.error(f"KISS-ICP {kiss_icp_version} is incompatible with " + f"ouster-mapping\n" + f"Error message: {e}") + raise + + if self.last_id != -1 and scan_mid_ts <= self.ts_pose[-1][0]: + logger.warning("Set the out of order scan invalid") + scan.field(client.ChanField.RANGE)[:] = 0 + return scan + + self.ts_pose.append((scan_mid_ts, self.kiss_icp.poses[-1])) + + if len(self.ts_pose) >= 2: + col_global_poses = util.getScanColPoseWithTs( + self.ts_pose[-2][1], self.ts_pose[-1][1], scan) + scan.pose[:] = col_global_poses + elif len(self.ts_pose) < 2: + # First scan pose in KISS in identity matrix. not enough poses to do + # perturbation in every column. Juse identity matrix for col poses + scan.pose[:] = np.tile(np.identity(4), (scan.w, 1, 1)) + + self.last_id = scan.frame_id + + return scan + + # average highest 92% to 96% range readings and use this averaged range value + # to calculate the voxel map size + def get_voxel_size(self, scan: client.LidarScan, start_pct: float = 0.92, + end_pct: float = 0.96) -> float: + sel_flag = scan.field(client.ChanField.RANGE) != 0 + scan_range = scan.field(client.ChanField.RANGE)[sel_flag] + start_value = np.percentile(scan_range, start_pct * 100) + end_value = np.percentile(scan_range, end_pct * 100) + selected_ranges = scan_range[(scan_range >= start_value) & (scan_range <= end_value)] + + # lidar range is in mm. change the unit to meter + average = np.mean(selected_ranges) / 1000 + # use the lidar range readings and a number to land voxel size in a + # proper range + voxel_size = average / 46.0 + + # the above calculation is based on 1024 * 128. Smaller pixels per scan has + # a smaller voxel size + pixels_ratio = math.sqrt(math.sqrt(self.scan_pixels / (1024 * 128))) + voxel_size *= pixels_ratio + + # For live sensor, we use a larger voxel size for better results + live_voxel_ratio = 2.2 + if self.live_stream: + voxel_size *= live_voxel_ratio + logger.warning("Auto voxel sizer uses a larger size voxel for the live " + "sensor real-time processing.") + + return voxel_size diff --git a/python/src/ouster/sdk/mapping/ouster_kiss_icp.py b/python/src/ouster/sdk/mapping/ouster_kiss_icp.py new file mode 100644 index 00000000..c49cf50b --- /dev/null +++ b/python/src/ouster/sdk/mapping/ouster_kiss_icp.py @@ -0,0 +1,172 @@ +# type: ignore +""" +# MIT License +# +# Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill +# Stachniss. +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in all +# copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +# SOFTWARE. + +Module ouster_kiss_icp + +Description: + This module was a copy from the KissICP repository https://github.com/PRBonn/kiss-icp + We edit it to fit our use cases better + +Author: + Hao Yuan + +Last modified date: Jan 29th 2024 + +Modifcation History: + 1/29/24 Copy KissICP module and handle the frame drop issue +""" + + +from collections import deque +import numpy as np +import logging + +from kiss_icp.config import KISSConfig +from kiss_icp.deskew import get_motion_compensator +from kiss_icp.mapping import get_voxel_hash_map +from kiss_icp.preprocess import get_preprocessor +from kiss_icp.registration import register_frame +from kiss_icp.threshold import get_threshold_estimator +from kiss_icp.voxelization import voxel_down_sample + +import ouster.sdk.util.pose_util as pu + +logging.basicConfig(level=logging.INFO) +logger = logging.getLogger() + + +class KissICP: + + def __init__(self, config: KISSConfig): + self.poses = [] + self.config = config + self.compensator = get_motion_compensator(config) + self.adaptive_threshold = get_threshold_estimator(self.config) + self.local_map = get_voxel_hash_map(self.config) + self.preprocess = get_preprocessor(self.config) + self.last_two_ts = deque([], maxlen=2) + self.frame_drop_ratio_prev = 1 + + def _cal_ts_diff_ratio(self, ts) -> float: + if len(self.last_two_ts) < 2: + self.last_two_ts.append(ts) + return 1.0 + + ts_diff_prev = self.last_two_ts[-1] - self.last_two_ts[-2] + if ts_diff_prev == 0 or ts < self.last_two_ts[-1]: + return 1.0 + + ts_diff_cur = ts - self.last_two_ts[-1] + # calculate frame drop ratio using real timestamps + frame_drop_ratio = ts_diff_cur / ts_diff_prev + + self.last_two_ts.append(ts) + return frame_drop_ratio + + def register_frame(self, frame, timestamps, scan_ts): + # drop out of ordered frame + if len(self.poses) >= 1 and scan_ts <= self.last_two_ts[-1]: + logger.info(f"Kiss ICP drops the out of order frame at ts {scan_ts}") + return + + frame_drop_ratio_curr = self._cal_ts_diff_ratio(scan_ts) + + if len(self.poses) >= 2: + corrected_poses = [pu.pose_interp(self.poses[-2], self.poses[-1], + t=1 - (1 / self.frame_drop_ratio_prev)), self.poses[-1]] + else: + corrected_poses = [] + + # Apply motion compensation + frame = self.compensator.deskew_scan( + frame, corrected_poses, timestamps) + + # Preprocess the input cloud + frame = self.preprocess(frame) + + # Voxelize + source, frame_downsample = self.voxelize(frame) + + # Get motion prediction and adaptive_threshold + sigma = self.get_adaptive_threshold() + + # Compute initial_guess for ICP + prediction = self.get_prediction_model(frame_drop_ratio_curr) + last_pose = self.poses[-1] if self.poses else np.eye(4) + initial_guess = last_pose @ prediction + self.frame_drop_ratio_prev = frame_drop_ratio_curr + + # Run ICP + new_pose = register_frame( + points=source, + voxel_map=self.local_map, + initial_guess=initial_guess, + max_correspondance_distance=3 * sigma, + kernel=sigma / 3, + ) + + self.adaptive_threshold.update_model_deviation( + np.linalg.inv(initial_guess) @ new_pose) + self.local_map.update(frame_downsample, new_pose) + self.poses.append(new_pose) + + def voxelize(self, iframe): + frame_downsample = voxel_down_sample( + iframe, self.config.mapping.voxel_size * 0.5) + source = voxel_down_sample( + frame_downsample, + self.config.mapping.voxel_size * 1.5) + return source, frame_downsample + + def get_adaptive_threshold(self): + return ( + self.config.adaptive_threshold.initial_threshold + if not self.has_moved() + else self.adaptive_threshold.get_threshold() + ) + + def get_prediction_model(self, frame_drop_ratio_curr): + if len(self.poses) < 2: + return np.eye(4) + pose_diff_exp = np.linalg.inv(self.poses[-2]) @ self.poses[-1] + + curr_prev_drop_ratio = frame_drop_ratio_curr / self.frame_drop_ratio_prev + # Skip matrix manipulation if frames' drop ratio is less than 5% + if abs(curr_prev_drop_ratio - 1) < 0.05: + return pose_diff_exp + + pose_diff_log = pu.log_pose(pose_diff_exp) * curr_prev_drop_ratio + + return pu.exp_pose6(pose_diff_log) + + def has_moved(self): + if len(self.poses) < 1: + return False + + def compute_motion(T1, T2): + return np.linalg.norm( + (np.linalg.inv(T1) @ T2)[:3, -1]) + motion = compute_motion(self.poses[0], self.poses[-1]) + return motion > 5 * self.config.adaptive_threshold.min_motion_th diff --git a/python/src/ouster/sdk/mapping/ply_to_png.py b/python/src/ouster/sdk/mapping/ply_to_png.py new file mode 100644 index 00000000..f03e13fd --- /dev/null +++ b/python/src/ouster/sdk/mapping/ply_to_png.py @@ -0,0 +1,208 @@ +import argparse +import logging +import os +from typing import List + +import numpy as np +from PIL import Image as PILImage +from plyfile import PlyData # type: ignore # noqa + +import ouster.sdk.viz as viz +from ouster.sdk.viz.util import push_point_viz_fb_handler + +logging.basicConfig(level=logging.INFO) +logger = logging.getLogger('PNG_Maker') + + +# if beyond max points, random delete points to prevent memory overflow +# 50000000 points is around 30 mins recording with the default PLY maker voxel +# map size. You can increase the voxel map size in OSF save command for +# large maps +max_plot_points = 50000000 + + +def get_doll_dist(points): + # to linearize the plot camera zoon in distance + lidar_default_range = 150 + min_camera_dist = 2 + # bigger number make camera more zoom in + camera_magic_n = 20 + # because ouster.viz view_distance() uses std::exp(log_distance_ * 0.01) + # to linearize the view_distance(). I time 100 for input + linearize_n = 100 + + max_values = np.max(points, axis=0) + min_values = np.min(points, axis=0) + + max_dist = max(((max(abs(max_values - min_values)) - + lidar_default_range) / camera_magic_n), min_camera_dist) + + doll_dist = np.log(max_dist) * linearize_n + + return doll_dist + + +def peak_start_point(ply_files): + if not ply_files: + logger.error("PLY files list is empty\nExit!") + return None + + # Read the first PLY file + file = ply_files[0] + try: + with open(file, 'rb') as f: + ply_data = PlyData.read(f) + logger.info("PLY file successfully read.") + # Now you can access the data in ply_data + except IOError as e: + logger.error("Error reading PLY file:", e) + + # Check if the PLY data is not empty + if len(ply_data['vertex']) == 0: + logger.error("PLY file is empty\nExit!") + return None + + # Extract the first point as start point + x = ply_data['vertex']['x'][0] + y = ply_data['vertex']['y'][0] + z = ply_data['vertex']['z'][0] + + logger.info(f"start origin point ({x}, {y}, {z})") + return x, y, z + + +def output_ext_verify(filename): + if not filename.endswith(".png"): + raise argparse.ArgumentTypeError("Expect output file end with .png") + return filename + + +def mapping_save_fb_to_png(fb_data: List, fb_width: int, fb_height: int, + screenshot_path: str): + img_arr = np.array(fb_data, + dtype=np.uint8).reshape([fb_height, fb_width, 3]) + if screenshot_path: + PILImage.fromarray(np.flip(img_arr, axis=0)).convert( + "RGB").save(screenshot_path) + return screenshot_path + + +def mapping_screenshot(viz, screenshot_path: str) -> None: + def handle_fb_once(viz, fb_data: List, fb_width: int, + fb_height: int): + saved_img_path = mapping_save_fb_to_png(fb_data, + fb_width, + fb_height, + screenshot_path) + if saved_img_path: + logger.info(f"Screenshot is saved at {saved_img_path}") + viz.pop_frame_buffer_handler() + push_point_viz_fb_handler(viz, viz, handle_fb_once) + + +def convert_ply_to_png(ply_files, screenshot_path, is_plot): + + start_point = peak_start_point(ply_files) + + if not start_point: + return + + points = np.empty((0, 3)) + keys = np.empty(0) + + for file in ply_files: + logger.info(f"processing file : {file}") + # Read the PLY file + ply_data = PlyData.read(file) + x = ply_data['vertex']['x'] - start_point[0] + y = ply_data['vertex']['y'] - start_point[1] + z = ply_data['vertex']['z'] - start_point[2] + + possible_keys = ['REFLECTIVITY', 'SIGNAL', 'NEAR_IR'] + key_field = None + + for key in possible_keys: + if key in ply_data['vertex'].data.dtype.names: + key_field = key + break + + if key_field is None: + logger.error("No valid key field found in PLY file.") + return + + k = ply_data['vertex'][key_field] * 2 + + points = np.concatenate((points, np.stack((x, y, z), axis=-1)), axis=0) + keys = np.concatenate((keys, k)) + + current_size = points.shape[0] + if current_size > max_plot_points: + logger.info( + f"current points {current_size} is greater than" + f"maximum {max_plot_points}") + points_to_delete = current_size - max_plot_points + indices_to_delete = np.random.choice( + current_size, points_to_delete, replace=False) + points = np.delete(points, indices_to_delete, axis=0) + keys = np.delete(keys, indices_to_delete) + logger.info(f"random delete {points_to_delete} points") + + doll_dist = get_doll_dist(points) + + point_viz = viz.PointViz("Viz", True, 3840, 2160) + viz.add_default_controls(point_viz) + cloud = viz.Cloud(points.shape[0]) + + cloud.set_xyz(np.asfortranarray(points)) + cloud.set_key(np.asfortranarray(keys)) + cloud.set_palette(viz.magma_palette) + point_viz.add(cloud) + mapping_screenshot(point_viz, screenshot_path) + point_viz.camera.set_dolly(doll_dist) + point_viz.update() + + if is_plot: + point_viz.run() + else: + point_viz.run_once() + + +if __name__ == "__main__": + parser = argparse.ArgumentParser(description="Plot Point Cloud") + parser.add_argument( + "PLY_files", + metavar='file', + nargs='+', + help="PLY file as input") + parser.add_argument( + "--dir", + help="PNG output dir") + parser.add_argument( + "--out", + type=output_ext_verify, + help="PNG output name") + parser.add_argument( + '--plot', + action='store_true', + default=False, + help='Enable plotting') + + args = parser.parse_args() + file_path = '' + + if args.dir: + if not os.path.exists(args.dir): + logger.error(f"The given output dir {args.dir} not exist") + exit(1) + + file_path = args.dir + if not file_path.endswith(os.path.sep): + file_path += os.path.sep + if file_path and not args.out: + logger.error(f"Given the output dir {file_path} without the output " + f"filename {args.out}. Use --out to specify the out filename") + exit(1) + if args.out: + file_path += args.out + + convert_ply_to_png(args.PLY_files, file_path, args.plot) diff --git a/python/src/ouster/sdk/mapping/py.typed b/python/src/ouster/sdk/mapping/py.typed new file mode 100644 index 00000000..e69de29b diff --git a/python/src/ouster/sdk/mapping/requirements.json b/python/src/ouster/sdk/mapping/requirements.json new file mode 100644 index 00000000..4dd46c3d --- /dev/null +++ b/python/src/ouster/sdk/mapping/requirements.json @@ -0,0 +1,17 @@ +{ + "scipy": + { + "version": ">=1.7, <2", + "marker": "platform_system != \"Darwin\" or platform_machine != \"arm64\"" + }, + "point-cloud-utils": + { + "version": ">=0.30.4, <1.0.0", + "marker": "python_version <= \"3.11\" and platform_machine != \"aarch64\" and (platform_system != \"Darwin\" or platform_machine != \"arm64\")" + }, + "kiss-icp": + { + "version": "==0.4.0", + "marker": "platform_machine != \"aarch64\"" + } +} diff --git a/python/src/ouster/sdk/mapping/slam.py b/python/src/ouster/sdk/mapping/slam.py new file mode 100644 index 00000000..e9b890f0 --- /dev/null +++ b/python/src/ouster/sdk/mapping/slam.py @@ -0,0 +1,2 @@ +from .kiss_backend import KissBackend # noqa: F401 +from .slam_backend import SLAMBackend # noqa: F401 diff --git a/python/src/ouster/sdk/mapping/slam_backend.py b/python/src/ouster/sdk/mapping/slam_backend.py new file mode 100644 index 00000000..5fe54a8b --- /dev/null +++ b/python/src/ouster/sdk/mapping/slam_backend.py @@ -0,0 +1,21 @@ +from abc import ABC, abstractmethod + +import ouster.sdk.client as client + + +class SLAMBackend(ABC): + """base class of general slam solutions for SDK cli usage""" + + def __init__(self, info: client.SensorInfo, use_extrinsics: bool = True): + self.info = info + self.xyz_lut = client.XYZLut(info, use_extrinsics=use_extrinsics) + self.w = info.format.columns_per_frame + self.h = info.format.pixels_per_column + + """ + Update the pose (per_column_global_pose) variable in scan and return + """ + + @abstractmethod + def update(self, scan: client.LidarScan) -> client.LidarScan: + pass diff --git a/python/src/ouster/sdk/mapping/util.py b/python/src/ouster/sdk/mapping/util.py new file mode 100644 index 00000000..7257c494 --- /dev/null +++ b/python/src/ouster/sdk/mapping/util.py @@ -0,0 +1,87 @@ +from typing import Tuple + +import numpy as np + +import ouster.sdk.util.pose_util as pu +from ouster.sdk import client + + +def getNormalizedTimestamps(scan: client.LidarScan): + ts = np.linspace(0, 1, scan.w, endpoint=True) + # If there are jumps in the timestamps due to poor sensor time-syncing, fall back to defaults + if np.any(np.diff(scan.timestamp[scan.status == 1]) <= 0): + return ts + else: + start_col = client.first_valid_column(scan) + stop_col = client.last_valid_column(scan) + ts_actual = scan.timestamp + start_ts = scan.timestamp[start_col] + stop_ts = scan.timestamp[stop_col] + # get 0 col to w col total ts duration + ts_dur = (stop_ts - start_ts) / (stop_col - start_col) * (scan.w - 1) + # calculate actual diff normalized ts using ts gap / total ts duration + ts_actl_diff_norm = np.divide(np.subtract(ts_actual, start_ts), ts_dur) + # update start_col to stop_col using ts_actl_diff_norm + ts[start_col] + ts[start_col:stop_col] = ts_actl_diff_norm[start_col:stop_col] + ts[start_col] + return ts + + +def getScanColPoseWithTs(start_pose_h, end_pose_h, scan: client.LidarScan) -> np.ndarray: + ts = getNormalizedTimestamps(scan) + diff_log = pu.log_pose(np.dot(end_pose_h, np.linalg.inv(start_pose_h))) + # As end_ts_pose is the kiss-icp results of the scan. end_ts will be the + # middle ts of the scan. Use end_ts as mid_scan_ts to avoid calculation + per_col_diff_v = ts[:, np.newaxis] * diff_log[np.newaxis, :] + + per_col_diff_h = pu.exp_pose6(np.array(per_col_diff_v)) + end_pose_h = end_pose_h[np.newaxis, :, :] + global_col_poses = np.einsum("ijk,ikl->ijl", per_col_diff_h, end_pose_h) + + return global_col_poses + + +def getScanColPose(start_ts_pose: Tuple[int, np.ndarray], + end_ts_pose: Tuple[int, np.ndarray], + scan: client.LidarScan) -> np.ndarray: + """ + This function return the global_per_col_pose (4x4) of the input LidarScan by using + the constant pose change rate assumption. This function currently ONLY for KISS-ICP + Pose interpolaton. + KISS-ICP registers point cloud based on the middle ts, so thepose timestamp is + also based on middle ts of a scan. + + scan: | start_pose | end_pose | + ts (beginning): | | + start_ts end_ts + ts (middle): | output_scan + funct return-> global_col_poses + start_ts/end_ts are the middle vaild ts of the start_scan/end_scan. + * end_pose MUST BE the KISS-ICP output result of the output_scan + * for example: end_pose = kiss_icp.update(output_scan) + """ + start_pose_h = start_ts_pose[1] + end_pose_h = end_ts_pose[1] + diff_log = pu.log_pose(np.dot(end_pose_h, np.linalg.inv(start_pose_h))) + + start_ts = start_ts_pose[0] + end_ts = end_ts_pose[0] + delta_ts = end_ts - start_ts + + if delta_ts < 0: + raise RuntimeError( + "end pose ts smaller then start pose ts.\nexit" + ) + + # As end_ts_pose is the kiss-icp results of the scan. end_ts will be the + # middle ts of the scan. Use end_ts as mid_scan_ts to avoid calculation + per_col_diff_v = [ + ((ts - end_ts) / delta_ts) * diff_log for ts in scan.timestamp + ] + + per_col_diff_h = pu.exp_pose6(np.array(per_col_diff_v)) + + start_pose_h = start_pose_h[np.newaxis, :, :] + end_pose_h = end_pose_h[np.newaxis, :, :] + global_col_poses = np.einsum("ijk,ikl->ijl", per_col_diff_h, end_pose_h) + + return global_col_poses diff --git a/python/src/ouster/sdk/open_source.py b/python/src/ouster/sdk/open_source.py index 98d0d08f..48bc4359 100644 --- a/python/src/ouster/sdk/open_source.py +++ b/python/src/ouster/sdk/open_source.py @@ -1,4 +1,5 @@ from typing import List, Optional, Union +import os import numpy as np from pathlib import Path from ouster.sdk.client import ScanSource, MultiScanSource @@ -44,7 +45,7 @@ def open_source(source_url: str, sensor_idx: int = 0, *args, of fields of every scan. in case of dual returns profile FLAGS2 will also be appended (default is True). """ - source_urls = [url.strip() for url in source_url.split(',') if url.strip()] + source_urls = [os.path.expanduser(url.strip()) for url in source_url.split(',') if url.strip()] if len(source_urls) == 0: raise ValueError("No valid source specified") diff --git a/python/src/ouster/sdk/osf/_osf.pyi b/python/src/ouster/sdk/osf/_osf.pyi index 4929c235..ceb4ce2d 100644 --- a/python/src/ouster/sdk/osf/_osf.pyi +++ b/python/src/ouster/sdk/osf/_osf.pyi @@ -5,7 +5,7 @@ from typing import Any, ClassVar, List from typing import (overload, Iterator) import numpy -from ouster.sdk.client import BufferT, LidarScan, SensorInfo +from ouster.sdk.client import BufferT, LidarScan, SensorInfo, FieldType class ChunkRef: @@ -166,17 +166,17 @@ class Writer: def __init__(self, file_name: str, chunk_size: int = ...) -> None: ... @overload def __init__(self, filename: str, info: SensorInfo, - field_types = ..., chunk_size: int = ...) -> None: ... + fields_to_write: List[str] = ..., chunk_size: int = ...) -> None: ... @overload def __init__(self, filename: str, info: List[SensorInfo], - field_types = ..., chunk_size: int = ...) -> None: ... + fields_to_write: List[str] = ..., chunk_size: int = ...) -> None: ... @overload def save(self, stream_id: int, scan: LidarScan) -> None: ... @overload def save(self, stream_id: int, scan: LidarScan, ts: int) -> None: ... @overload def save(self, stream_id: int, scan: List[LidarScan]) -> None: ... - def add_sensor(self, info: SensorInfo, field_types = ...) -> int: ... + def add_sensor(self, info: SensorInfo, fields_to_write: List[str] = ...) -> int: ... def add_metadata(self, arg0: object) -> int: ... def save_message(self, stream_id: int, ts: int, buffer: BufferT) -> int: ... @overload diff --git a/python/src/ouster/sdk/osf/data.py b/python/src/ouster/sdk/osf/data.py index 0008e9e2..85d31aa3 100644 --- a/python/src/ouster/sdk/osf/data.py +++ b/python/src/ouster/sdk/osf/data.py @@ -1,4 +1,4 @@ -from typing import List, Optional, Union, cast, Iterator, Tuple +from typing import Optional, Union, cast, Iterator, Tuple, List from ouster.sdk import client from ouster.sdk.client.data import FieldTypes @@ -106,9 +106,17 @@ def is_indexed(self) -> bool: return False # TODO: for now we just use False no matter what @property - def fields(self) -> FieldTypes: + def field_types(self) -> FieldTypes: return client.get_field_types(self.metadata) + @property + def fields(self) -> List[str]: + l = [] + for ft in client.get_field_types(self.metadata): + l.append(ft.name) + l.sort() + return l + @property def scans_num(self) -> Optional[int]: raise NotImplementedError # TODO: implement @@ -116,12 +124,18 @@ def scans_num(self) -> Optional[int]: def __len__(self) -> int: raise NotImplementedError # TODO: implement - def _seek(self, key: int) -> None: + def _seek(self, _: int) -> None: pass - def __getitem__(self, key: Union[int, slice] - ) -> Union[Optional[LidarScan], List[Optional[LidarScan]]]: + def __getitem__(self, _: Union[int, slice] + ) -> Union[Optional[LidarScan], ScanSource]: raise NotImplementedError def __del__(self) -> None: pass + + def _slice_iter(self, _: slice) -> Iterator[Optional[LidarScan]]: + raise NotImplementedError + + def slice(self, _: slice) -> 'ScanSource': + raise NotImplementedError diff --git a/python/src/ouster/sdk/osf/osf_scan_source.py b/python/src/ouster/sdk/osf/osf_scan_source.py index dbb03022..2f8896d1 100644 --- a/python/src/ouster/sdk/osf/osf_scan_source.py +++ b/python/src/ouster/sdk/osf/osf_scan_source.py @@ -23,7 +23,6 @@ def __init__( complete: bool = False, index: bool = False, cycle: bool = False, - flags: bool = True, **kwargs ) -> None: """ @@ -37,9 +36,6 @@ def __init__( file enabling len, index and slice operations on the scan source, if the flag is set to False indexing is skipped (default is False). cycle: repeat infinitely after iteration is finished (default is False) - flags: when this option is set, the FLAGS field will be added to the list - of fields of every scan, in case of dual returns FLAGS2 will also be - appended (default is True). Remarks: In case the OSF file didn't have builtin-index and the index flag was @@ -100,18 +96,30 @@ def __init__( self._stream_sensor_idx[stream_id] = self._sensor_idx[ stream_meta.sensor_meta_id] - def append_flags(ftypes: Dict, flags: bool) -> Dict: - import numpy as np - if flags: - ftypes.update({client.ChanField.FLAGS: np.uint8}) - if client.ChanField.RANGE2 in ftypes: - ftypes.update({client.ChanField.FLAGS2: np.uint8}) - return ftypes - + # get the first scan scan_streams = self._reader.meta_store.find(LidarScanStream) self._stream_ids = [mid for mid, _ in scan_streams.items()] - self._fields = [append_flags(lss.field_types, flags) - for _, lss in scan_streams.items()] + + self._field_types = [] + self._fields = [] + for sid, mid in enumerate(self._stream_ids): + ts_start = self._reader.ts_by_message_idx(mid, 0) + if ts_start is None: + # There are no messages in this stream, + # so there are no fields for this stream. + self._field_types.append([]) + self._fields.append([]) + continue + for idx, scan in self._scans_iter(ts_start, ts_start, False): + if idx == sid: + fts = scan.field_types + self._field_types.append(fts) + l = [] + for ft in fts: + l.append(ft.name) + self._fields.append(l) + break + # TODO: the following two properties (_scans_num, _len) are computed on # load but should rather be provided directly through OSF API. Obtain # these values directly from OSF API once implemented. @@ -185,8 +193,6 @@ def _scans_iter(self, start_ts: int, stop_ts: int, cycle: bool window = self.metadata[idx].format.column_window scan = cast(LidarScan, ls) if not self._complete or scan.complete(window): - if set(scan.fields) != set(self._fields[idx].keys()): - scan = client.LidarScan(scan, self._fields[idx]) yield idx, scan @property @@ -210,10 +216,13 @@ def is_indexed(self) -> bool: return self._indexed @property - def fields(self) -> List[client.FieldTypes]: - """Field types are present in the LidarScan objects on read from iterator""" + def fields(self) -> List[List[str]]: return self._fields + @property + def field_types(self) -> List[client.FieldTypes]: + return self._field_types + @property def scans_num(self) -> List[Optional[int]]: return self._scans_num # type: ignore @@ -234,7 +243,7 @@ def _seek(self, key: int) -> None: ... def __getitem__(self, key: Union[int, slice] - ) -> Union[List[Optional[LidarScan]], List[List[Optional[LidarScan]]]]: + ) -> Union[List[Optional[LidarScan]], MultiScanSource]: if not self.is_indexed: raise TypeError( @@ -257,21 +266,7 @@ def __getitem__(self, key: Union[int, slice] first_valid_packet_ts, dt=self._dt)) if isinstance(key, slice): - L = len(self) - k = ForwardSlicer.normalize(key, L) - count = k.stop - k.start - if count <= 0: - return [] - ts_start = min([self._reader.ts_by_message_idx(mid, k.start) - for mid in self._stream_ids]) - ts_stop = max([self._reader.ts_by_message_idx(mid, k.stop - 1) - for mid in self._stream_ids]) - scans_itr = collate_scans(self._scans_iter(ts_start, ts_stop, False), - self.sensors_count, first_valid_packet_ts, - dt=self._dt) - result = [scan for idx, scan in ForwardSlicer.slice( - enumerate(scans_itr), k) if idx < count] - return result if k.step > 0 else list(reversed(result)) + return self.slice(key) raise TypeError( f"indices must be integer or slice, not {type(key).__name__}") @@ -292,3 +287,30 @@ def __del__(self) -> None: def single_source(self, stream_idx: int) -> ScanSource: from ouster.sdk.client.scan_source_adapter import ScanSourceAdapter return ScanSourceAdapter(self, stream_idx) + + def _slice_iter(self, key: slice) -> Iterator[List[Optional[LidarScan]]]: + # NOTE: In this method if key.step was negative, this won't be + # result in the output being reversed, it is the responisbility of + # the caller to accumelate the results into a vector then return them. + L = len(self) + k = ForwardSlicer.normalize(key, L) + count = k.stop - k.start + if count <= 0: + return iter(()) + ts_start = min([self._reader.ts_by_message_idx(mid, k.start) + for mid in self._stream_ids]) + ts_stop = max([self._reader.ts_by_message_idx(mid, k.stop - 1) + for mid in self._stream_ids]) + scans_itr = collate_scans(self._scans_iter(ts_start, ts_stop, False), + self.sensors_count, first_valid_packet_ts, + dt=self._dt) + return ForwardSlicer.slice_iter(scans_itr, k) + + def slice(self, key: slice) -> 'MultiScanSource': + """Constructs a MultiScanSource matching the specificed slice""" + from ouster.sdk.client.multi_sliced_scan_source import MultiSlicedScanSource + L = len(self) + k = ForwardSlicer.normalize(key, L) + if k.step < 0: + raise TypeError("slice() can't work with negative step") + return MultiSlicedScanSource(self, k) diff --git a/python/src/ouster/sdk/pcap/packet_iter.py b/python/src/ouster/sdk/pcap/packet_iter.py index 8f40b8e7..9a82421b 100644 --- a/python/src/ouster/sdk/pcap/packet_iter.py +++ b/python/src/ouster/sdk/pcap/packet_iter.py @@ -5,7 +5,7 @@ from more_itertools import consume -from ouster.sdk.client import (Packet, PacketMultiSource, LidarPacket, ImuPacket, FrameBorder) +from ouster.sdk.client import (PacketMultiSource, LidarPacket, ImuPacket, FrameBorder, PacketFormat) from ouster.sdk.pcap.pcap import MTU_SIZE import ouster.sdk.pcap._pcap as _pcap @@ -55,23 +55,6 @@ def chunk() -> Iterator[T]: consume(c) -def ichunked_framed( - packets: Iterable[Packet], - pred: Callable[[Packet], - bool] = lambda _: True) -> Iterator[Iterator[Packet]]: - """Delimit a packets when the frame id changes and pred is true.""" - - return ichunked_before(packets, FrameBorder(pred)) - - -def n_frames(packets: Iterable[Packet], n: int) -> Iterator[Packet]: - for i, frame in enumerate(ichunked_framed(packets)): - if i < n: - yield from frame - else: - break - - # TODO: these currently account for SensorScanSource being based on Scans internally and will # require rework once that has a proper ScansMulti implementation -- Tim T. class RecordingPacketSource: @@ -102,6 +85,11 @@ def __init__(self, self.imu_port = imu_port self.use_sll_encapsulation = use_sll_encapsulation self.overwrite = overwrite + metadata = self.source.metadata + if type(metadata) is list: + metadata = metadata[self.sensor_idx] + self._metadata = metadata + self.pf = PacketFormat(metadata) @property # type: ignore def __class__(self): @@ -113,11 +101,7 @@ def __iter__(self): error = False n = 0 - metadata = self.source.metadata - if type(metadata) is list: - metadata = metadata[self.sensor_idx] - - frame_bound = FrameBorder() + frame_bound = FrameBorder(self._metadata) chunk = 0 pcap_path = self.prefix_path + f"-{chunk:03}.pcap" @@ -143,12 +127,12 @@ def __iter__(self): raise ValueError("Unexpected packet type") if has_timestamp is None: - has_timestamp = (packet.capture_timestamp is not None) - elif has_timestamp != (packet.capture_timestamp is not None): + has_timestamp = (packet.host_timestamp != 0) + elif has_timestamp != (packet.host_timestamp != 0): raise ValueError("Mixing timestamped/untimestamped packets") - ts = packet.capture_timestamp or time.time() - _pcap.record_packet(handle, self.src_ip, self.dst_ip, src_port, dst_port, packet._data, ts) + ts = (packet.host_timestamp / 1e9) or time.time() + _pcap.record_packet(handle, self.src_ip, self.dst_ip, src_port, dst_port, packet.buf, ts) if frame_bound(packet): num_frames += 1 @@ -210,8 +194,8 @@ def __iter__(self): for next_packet in self.packet_source: idx, packet = next_packet if (type(next_packet) is tuple) else (None, next_packet) if (idx is None) or (idx == self.sensor_idx): - ts = rospy.Time.from_sec(packet.capture_timestamp) - msg = PacketMsg(buf=packet._data.tobytes()) + ts = rospy.Time.from_sec(packet.host_timestamp / 1e9) + msg = PacketMsg(buf=packet.buf.tobytes()) if isinstance(packet, LidarPacket): outbag.write(self.lidar_topic, msg, ts) elif isinstance(packet, ImuPacket): diff --git a/python/src/ouster/sdk/pcap/pcap.py b/python/src/ouster/sdk/pcap/pcap.py index 0c1851af..86961b52 100644 --- a/python/src/ouster/sdk/pcap/pcap.py +++ b/python/src/ouster/sdk/pcap/pcap.py @@ -2,16 +2,13 @@ Copyright (c) 2021, Ouster, Inc. All rights reserved. """ -from copy import copy import os import socket import time -from threading import Lock -from collections import defaultdict from typing import (Iterable, Iterator, Optional, Tuple, Dict) # noqa: F401 -from ouster.sdk.client import (LidarPacketValidator, LidarPacket, ImuPacket, Packet, PacketSource, # noqa: F401 - SensorInfo, _client, PacketValidationFailure, PacketIdError) # noqa: F401 +from ouster.sdk.client import (LidarPacket, ImuPacket, Packet, PacketSource, # noqa: F401 + SensorInfo, _client, PacketValidationFailure) # noqa: F401 from . import _pcap MTU_SIZE = 1500 @@ -19,10 +16,10 @@ def _guess_ports(stream_info, sensor_info): pf = _client.PacketFormat.from_info(sensor_info) - lidar_spec, imu_spec = sensor_info.udp_port_lidar, sensor_info.udp_port_imu + lidar_spec, imu_spec = sensor_info.config.udp_port_lidar, sensor_info.config.udp_port_imu guesses = [(i.lidar, i.imu) for i in _pcap.guess_ports( stream_info, pf.lidar_packet_size, pf.imu_packet_size, - lidar_spec, imu_spec)] + lidar_spec or 0, imu_spec or 0)] guesses.sort(reverse=True, key=lambda p: (p[0] != 0, p[1] != 0, p)) return guesses @@ -38,18 +35,11 @@ def _packet_info_stream(path: str, n_packets, progress_callback=None, callback_f class Pcap(PacketSource): """Read a sensor packet stream out of a pcap file as an iterator.""" - _metadata: SensorInfo - _rate: float - _handle: Optional[_pcap.playback_handle] - _lock: Lock - def __init__(self, pcap_path: str, info: SensorInfo, *, rate: float = 0.0, - lidar_port: Optional[int] = None, - imu_port: Optional[int] = None, loop: bool = False, soft_id_check: bool = False): """Read a single sensor data stream from a packet capture. @@ -80,94 +70,14 @@ def __init__(self, loop: Specify whether to reload the PCAP file when the end is reached soft_id_check: if True, don't skip lidar packets buffers on init_id/sn mismatch """ - - # prefer explicitly specified ports (can probably remove the args?) - lidar_port = info.udp_port_lidar if lidar_port is None else lidar_port - imu_port = info.udp_port_imu if imu_port is None else imu_port - - # override ports in metadata, if explicitly specified - self._metadata = copy(info) - self._metadata.udp_port_lidar = lidar_port - self._metadata.udp_port_imu = imu_port - - self.loop = loop - self._soft_id_check = soft_id_check - self._id_error_count = 0 # TWS 20230615 TODO generialize error counting and reporting - self._errors = defaultdict(int) # type: Dict[PacketValidationFailure,int] - - # sample pcap and attempt to find UDP ports consistent with metadata - n_packets = 1000 - stats = _packet_info_stream(pcap_path, n_packets) - self._guesses = _guess_ports(stats, self._metadata) - - # fill in unspecified (0) ports with inferred values - if len(self._guesses) > 0: - lidar_guess, imu_guess = self._guesses[0] - # guess != port only if port == 0 or guess == 0 - self._metadata.udp_port_lidar = lidar_guess or lidar_port - self._metadata.udp_port_imu = imu_guess or imu_port - - self._rate = rate - self._handle = _pcap.replay_initialize(pcap_path) - self._lock = Lock() + from ouster.sdk.pcap import PcapMultiPacketReader + self._source = PcapMultiPacketReader(pcap_path, [], rate=rate, metadatas=[info], + soft_id_check=soft_id_check) def __iter__(self) -> Iterator[Packet]: - with self._lock: - if self._handle is None: - raise ValueError("I/O operation on closed packet source") - - buf = bytearray(2**16) - packet_info = _pcap.packet_info() - - real_start_ts = time.monotonic() - pcap_start_ts = None - validator = LidarPacketValidator(self.metadata) - while True: - with self._lock: - if not self._handle: - break - if not _pcap.next_packet_info(self._handle, packet_info): - if self.loop: - _pcap.replay_reset(self._handle) - _pcap.next_packet_info(self._handle, packet_info) - else: - break - n = _pcap.read_packet(self._handle, buf) - - # if rate is set, read in 'real time' simulating UDP stream - # TODO: factor out into separate packet iterator utility - timestamp = packet_info.timestamp - if self._rate: - if not pcap_start_ts: - pcap_start_ts = timestamp - real_delta = time.monotonic() - real_start_ts - pcap_delta = (timestamp - pcap_start_ts) / self._rate - delta = max(0, pcap_delta - real_delta) - time.sleep(delta) - - try: - if (packet_info.dst_port == self._metadata.udp_port_lidar): - for error in validator.check_packet(buf, n): - self._errors[error] += 1 # accumulate counts of errors - lp = LidarPacket( - buf[0:n], - self._metadata, - timestamp, - _raise_on_id_check=not self._soft_id_check) - if lp.id_error: - self._id_error_count += 1 - yield lp - elif (packet_info.dst_port == self._metadata.udp_port_imu): - yield ImuPacket(buf[0:n], self._metadata, timestamp) - except PacketIdError: - self._id_error_count += 1 - except ValueError: - # bad packet size: this can happen when - # packets are buffered by the OS, not necessarily an error - # same pass as in core.py - # TODO: introduce status for PacketSource to indicate frequency - # of bad packet size or init_id/sn errors - pass + for idx, packet in self._source: + if idx == 0: + yield packet @property def is_live(self) -> bool: @@ -175,38 +85,37 @@ def is_live(self) -> bool: @property def metadata(self) -> SensorInfo: - return self._metadata + return self._source._metadata[0] @property def ports(self) -> Tuple[int, int]: """Specified or inferred ports associated with lidar and imu data. - Values <= 0 indicate that no lidar or imu data will be read. + Values of 0 indicate that no data of that type will be read. """ - return (self._metadata.udp_port_lidar, self._metadata.udp_port_imu) + return (self._source._metadata[0].config.udp_port_lidar or 0, + self._source._metadata[0].config.udp_port_imu or 0) def reset(self) -> None: """Restart playback from beginning. Thread-safe.""" - with self._lock: - if self._handle is not None: - _pcap.replay_reset(self._handle) + self._source.restart() @property def closed(self) -> bool: """Check if source is closed. Thread-safe.""" - with self._lock: - return self._handle is None + return self._source.closed @property def id_error_count(self) -> int: - return self._id_error_count + return self._source.id_error_count + + @property + def size_error_count(self) -> int: + return self._source.size_error_count def close(self) -> None: """Release resources. Thread-safe.""" - with self._lock: - if self._handle is not None: - _pcap.replay_uninitialize(self._handle) - self._handle = None + self._source.close() def _replay(pcap_path: str, info: SensorInfo, dst_ip: str, dst_lidar_port: int, @@ -239,7 +148,7 @@ def _replay(pcap_path: str, info: SensorInfo, dst_ip: str, dst_lidar_port: int, if isinstance(item, ImuPacket): port = dst_imu_port - socket_out.sendto(item._data.tobytes(), (dst_ip, port)) + socket_out.sendto(item.buf.tobytes(), (dst_ip, port)) yield True yield False finally: @@ -286,12 +195,14 @@ def record(packets: Iterable[Packet], raise ValueError("Unexpected packet type") if has_timestamp is None: - has_timestamp = (packet.capture_timestamp is not None) - elif has_timestamp != (packet.capture_timestamp is not None): + has_timestamp = (packet.host_timestamp != 0) + elif has_timestamp != (packet.host_timestamp != 0): raise ValueError("Mixing timestamped/untimestamped packets") - ts = packet.capture_timestamp or time.time() - _pcap.record_packet(handle, src_ip, dst_ip, src_port, dst_port, packet._data, ts) + ts = packet.host_timestamp / 1e9 + if ts == 0.0: + ts = time.time() + _pcap.record_packet(handle, src_ip, dst_ip, src_port, dst_port, packet.buf, ts) n += 1 except Exception: error = True diff --git a/python/src/ouster/sdk/pcap/pcap_multi_packet_reader.py b/python/src/ouster/sdk/pcap/pcap_multi_packet_reader.py index d109ae70..3e0a8a51 100644 --- a/python/src/ouster/sdk/pcap/pcap_multi_packet_reader.py +++ b/python/src/ouster/sdk/pcap/pcap_multi_packet_reader.py @@ -3,14 +3,13 @@ import ouster.sdk.pcap._pcap as _pcap from ouster.sdk.pcap._pcap import PcapIndex # type: ignore from ouster.sdk.pcap.pcap import _guess_ports, _packet_info_stream -from functools import partial import time +import numpy as np from threading import Lock -from ouster.sdk.client import SensorInfo, PacketIdError -from ouster.sdk.client.data import Packet, LidarPacket, ImuPacket +from ouster.sdk.client import SensorInfo, PacketFormat, PacketValidationFailure, LidarPacket, ImuPacket, Packet class PcapMultiPacketReader(PacketMultiSource): @@ -26,6 +25,7 @@ def __init__(self, pcap_path: str, metadata_paths: List[str], *, + metadatas: Optional[List[SensorInfo]] = None, rate: float = 0.0, index: bool = False, soft_id_check: bool = False): @@ -44,6 +44,7 @@ def __init__(self, self._indexed = index self._soft_id_check = soft_id_check self._id_error_count = 0 + self._size_error_count = 0 self._port_info: Dict[int, object] = dict() @@ -52,37 +53,48 @@ def __init__(self, n_packets = 1000 stats = _packet_info_stream(pcap_path, n_packets) - for meta_path in metadata_paths: - with open(meta_path) as meta_file: - meta_json = meta_file.read() - meta_info = SensorInfo(meta_json) - self._metadata_json.append(meta_json) - self._metadata.append(meta_info) - idx = len(self._metadata) - 1 - - # NOTE: Rudimentary logic of port guessing that is still needed - # for old single sensor data when `udp_port_lidar` and - # `udp_port_imu` fields are not set in sensor metadata. - # In some distant future we may need to remove it. - guesses = _guess_ports(stats, self._metadata[idx]) - if len(guesses) > 0: - lidar_guess, imu_guess = guesses[0] - meta_info.udp_port_lidar = meta_info.udp_port_lidar or lidar_guess - meta_info.udp_port_imu = meta_info.udp_port_imu or imu_guess - - port_to_packet = [ - (meta_info.udp_port_lidar, - partial(LidarPacket, - _raise_on_id_check=not soft_id_check)), - (meta_info.udp_port_imu, ImuPacket) - ] - for packet_port, packet_ctor in port_to_packet: - if packet_port in self._port_info: - raise RuntimeError( - f"Port collision: {packet_port}" - f" was already used for another stream") - self._port_info[packet_port] = dict(ctor=packet_ctor, - idx=idx) + if len(metadata_paths) > 0 and metadatas is not None: + raise RuntimeError("Cannot provide both metadata and metadata paths") + + # load all metadatas + if metadatas is None: + metadatas = [] + for idx, meta_path in enumerate(metadata_paths): + with open(meta_path) as meta_file: + meta_json = meta_file.read() + meta_info = SensorInfo(meta_json) + self._metadata_json.append(meta_json) + self._metadata.append(meta_info) + else: + for m in metadatas: + self._metadata_json.append(m.to_json_string()) + self._metadata.append(m) + + for idx in range(0, len(self._metadata)): + meta_info = self._metadata[idx] + # NOTE: Rudimentary logic of port guessing that is still needed + # for old single sensor data when `udp_port_lidar` and + # `udp_port_imu` fields are not set in sensor metadata. + # In some distant future we may need to remove it. + guesses = _guess_ports(stats, meta_info) + if len(guesses) > 0: + lidar_guess, imu_guess = guesses[0] + meta_info.config.udp_port_lidar = meta_info.config.udp_port_lidar or lidar_guess + meta_info.config.udp_port_imu = meta_info.config.udp_port_imu or imu_guess + + port_to_packet = [ + (meta_info.config.udp_port_lidar, LidarPacket), + (meta_info.config.udp_port_imu, ImuPacket) + ] + for packet_port, packet_ctor in port_to_packet: + if packet_port in self._port_info: + raise RuntimeError( + f"Port collision: {packet_port}" + f" was already used for another stream") + if packet_port is None: + raise RuntimeError(f"Metadata was for {meta_info.sn} was missing port numbers.") + self._port_info[packet_port] = dict(ctor=packet_ctor, + idx=idx) self._rate = rate self._reader: Optional[_pcap.IndexedPcapReader] = \ @@ -90,6 +102,9 @@ def __init__(self, if self._indexed: self._reader.build_index() self._lock = Lock() + self._pf = [] + for m in self._metadata: + self._pf.append(PacketFormat(m)) def __iter__(self) -> Iterator[Tuple[int, Packet]]: with self._lock: @@ -125,22 +140,25 @@ def __iter__(self) -> Iterator[Tuple[int, Packet]]: delta = max(0, pcap_delta - real_delta) time.sleep(delta) - try: - port_info = self._port_info[packet_info.dst_port] - idx = port_info["idx"] # type: ignore - packet = port_info["ctor"](buf[0:n], self._metadata[idx], timestamp) # type: ignore - if isinstance(packet, LidarPacket) and packet.id_error: - self._id_error_count += 1 - yield (idx, packet) - except PacketIdError: - self._id_error_count += 1 - except ValueError: + port_info = self._port_info[packet_info.dst_port] + idx = port_info["idx"] # type: ignore + packet = port_info["ctor"](n) # type: ignore + data = buf[0:n] + packet.buf[:] = np.frombuffer(data, dtype=np.uint8, count=n) + packet.host_timestamp = int(timestamp * 1e9) + res = packet.validate(self._metadata[idx], self._pf[idx]) + if res == PacketValidationFailure.PACKET_SIZE: # bad packet size here: this can happen when # packets are buffered by the OS, not necessarily an error # same pass as in core.py - # TODO: introduce status for PacketSource to indicate frequency - # of bad packet size or init_id/sn errors - pass + self._size_error_count += 1 + continue + if res == PacketValidationFailure.ID: + self._id_error_count += 1 + if not self._soft_id_check: + continue + + yield (idx, packet) @property def metadata(self) -> List[SensorInfo]: @@ -173,6 +191,10 @@ def seek(self, offset: int) -> None: def id_error_count(self) -> int: return self._id_error_count + @property + def size_error_count(self) -> int: + return self._size_error_count + def restart(self) -> None: """Restart playback, only relevant to non-live sources""" with self._lock: @@ -183,3 +205,9 @@ def close(self) -> None: """Release Pcap resources. Thread-safe.""" with self._lock: self._reader = None + + @property + def closed(self) -> bool: + """Check if source is closed. Thread-safe.""" + with self._lock: + return self._reader is None diff --git a/python/src/ouster/sdk/pcap/pcap_scan_source.py b/python/src/ouster/sdk/pcap/pcap_scan_source.py index f25fba54..88f4227f 100644 --- a/python/src/ouster/sdk/pcap/pcap_scan_source.py +++ b/python/src/ouster/sdk/pcap/pcap_scan_source.py @@ -1,7 +1,8 @@ -from typing import List, Optional, Tuple, Union +from typing import Iterator, List, Optional, Tuple, Union from ouster.sdk.client import LidarScan, first_valid_packet_ts from ouster.sdk.client import ScansMulti # type: ignore +from ouster.sdk.client import MultiScanSource from ouster.sdk.client.multi import collate_scans # type: ignore from ouster.sdk.util import (resolve_field_types, resolve_metadata_multi, ForwardSlicer, progressbar) # type: ignore @@ -76,16 +77,11 @@ def __init__( super().__init__(self._source, dt=dt, complete=complete, cycle=cycle, fields=field_types) - # TODO[IMPORTANT]: there is a bug with collate scans in which it always - # skips the first frame - def collate_scans_itr(scans_itr): - return collate_scans(scans_itr, self.sensors_count, - first_valid_packet_ts, dt=self._dt) - if index: self._frame_offset = [] pi = self._source._index # type: ignore - scans_itr = collate_scans_itr(self._scans_iter(True, False, False)) + scans_itr = self._collated_scans_itr( + self._scans_iter(True, False, True)) # scans count in first source scans_count = len(pi.frame_id_indices[0]) # type: ignore for scan_idx, scans in enumerate(scans_itr): @@ -95,6 +91,10 @@ def collate_scans_itr(scans_itr): progressbar(scan_idx, scans_count, "", "indexed") print("\nfinished building index") + def _collated_scans_itr(self, scans_itr): + return collate_scans(scans_itr, self.sensors_count, + first_valid_packet_ts, dt=self._dt) + @property def scans_num(self) -> List[Optional[int]]: if not self.is_indexed: @@ -108,7 +108,7 @@ def __len__(self) -> int: return len(self._frame_offset) def __getitem__(self, key: Union[int, slice] - ) -> Union[List[Optional[LidarScan]], List[List[Optional[LidarScan]]]]: + ) -> Union[List[Optional[LidarScan]], MultiScanSource]: if not self.is_indexed: raise TypeError( @@ -123,24 +123,34 @@ def __getitem__(self, key: Union[int, slice] offset = self._frame_offset[key] self._source.seek(offset) # type: ignore scans_itr = self._scans_iter(False, False, True) - return next(collate_scans(scans_itr, self.sensors_count, - first_valid_packet_ts, dt=self._dt)) + return next(self._collated_scans_itr(scans_itr)) if isinstance(key, slice): - L = len(self) - k = ForwardSlicer.normalize(key, L) - count = k.stop - k.start - if count <= 0: - return [] - offset = self._frame_offset[k.start] - self._source.seek(offset) # type: ignore - scans_itr = collate_scans(self._scans_iter(False, False, True), - self.sensors_count, - first_valid_packet_ts, - dt=self._dt) - result = [scan for idx, scan in ForwardSlicer.slice( - enumerate(scans_itr), k) if idx < count] - return result if k.step > 0 else list(reversed(result)) + return self.slice(key) raise TypeError( f"indices must be integer or slice, not {type(key).__name__}") + + def _slice_iter(self, key: slice) -> Iterator[List[Optional[LidarScan]]]: + # NOTE: In this method if key.step was negative, this won't be + # result in the output being reversed, it is the responsibility of + # the caller to accumulate the results into a vector then return them. + L = len(self) + k = ForwardSlicer.normalize(key, L) + count = k.stop - k.start + if count <= 0: + return iter(()) + offset = self._frame_offset[k.start] + self._source.seek(offset) # type: ignore + scans_itr = self._collated_scans_itr( + self._scans_iter(False, False, True)) + return ForwardSlicer.slice_iter(scans_itr, k) + + def slice(self, key: slice) -> MultiScanSource: + """Constructs a MultiScanSource matching the specificed slice""" + from ouster.sdk.client.multi_sliced_scan_source import MultiSlicedScanSource + L = len(self) + k = ForwardSlicer.normalize(key, L) + if k.step < 0: + raise TypeError("slice() can't work with negative step") + return MultiSlicedScanSource(self, k) diff --git a/python/src/ouster/sdk/sensor/sensor_multi_packet_reader.py b/python/src/ouster/sdk/sensor/sensor_multi_packet_reader.py index 18053fa3..01f4e100 100644 --- a/python/src/ouster/sdk/sensor/sensor_multi_packet_reader.py +++ b/python/src/ouster/sdk/sensor/sensor_multi_packet_reader.py @@ -8,10 +8,8 @@ from threading import Thread import ouster.sdk.client as client -from ouster.sdk.client import PacketMultiSource +from ouster.sdk.client import PacketMultiSource, SensorInfo, Packet, PacketValidationFailure import ouster.sdk.client._client as _client -from ouster.sdk.client import SensorInfo, PacketIdError -from ouster.sdk.client.data import Packet, LidarPacket, ImuPacket logger = logging.getLogger("multi-logger") @@ -95,7 +93,7 @@ def _fetch_metadata(self, timeout: Optional[float] = None) -> None: timeout_sec = ceil(timeout) if not self._fetched_meta: self._fetched_meta = [c.get_metadata( - legacy=False, timeout_sec=timeout_sec) for c in self._connections] + timeout_sec=timeout_sec) for c in self._connections] if not all(self._fetched_meta): raise client.ClientError("Failed to collect metadata. UPS :(") @@ -112,14 +110,17 @@ def _next_packet(self) -> Optional[Tuple[int, Packet]]: f"but ClientOverflow can't be raised so we are " f"raising ValueError, hmmm ...") if e.state & _client.ClientState.LIDAR_DATA: - p = self._cli.packet(e) - packet = LidarPacket( - p._data, self._metadata[e.source], p.capture_timestamp) + packet = self._cli.packet(e) + res = packet.validate(self._metadata[e.source], self._pf[e.source]) + if res == PacketValidationFailure.ID: + self._id_error_count[e.source] += 1 + # todo add option for soft check here + raise ValueError("Metadata init_id/sn does not match") + if res == PacketValidationFailure.PACKET_SIZE: + raise ValueError("Unexpected packet size") return (e.source, packet) elif e.state & _client.ClientState.IMU_DATA: - p = self._cli.packet(e) - packet = ImuPacket( - p._data, self._metadata[e.source], p.capture_timestamp) + packet = self._cli.packet(e) return (e.source, packet) elif e.state == _client.ClientState.TIMEOUT: raise client.ClientTimeout( @@ -128,9 +129,6 @@ def _next_packet(self) -> Optional[Tuple[int, Packet]]: raise client.ClientError("Client returned ERROR state") elif e.state & _client.ClientState.EXIT: return None - except PacketIdError as err: - self._id_error_count[e.source] += 1 - raise err finally: # LidarPacket/ImuPacket ctors may raise but we always want to # advance the subscriber so to not overflow @@ -159,7 +157,7 @@ def __iter__(self) -> Iterator[Tuple[int, Packet]]: yield p else: break - except (ValueError, PacketIdError): + except (ValueError): # bad packet size here: this can happen when # packets are buffered by the OS, not necessarily an error # same pass as in data.py @@ -180,7 +178,7 @@ def _flush_impl( frames_cnt = [n_frames] * len(self.metadata) sensor_flushed = [False] * len(self.metadata) - frame_bound = [client.FrameBorder() for _ in self.metadata] + frame_bound = [client.FrameBorder(m) for m in self.metadata] def flush_impl(p: Tuple[int, client.Packet]) -> bool: nonlocal frame_bound @@ -243,5 +241,5 @@ def buf_use(self) -> int: @property def id_error_count(self) -> List[int]: - """Number of PacketIdError accumulated per connection/sensor""" + """Number of packet id check failures accumulated per connection/sensor""" return self._id_error_count diff --git a/python/src/ouster/sdk/util/__init__.py b/python/src/ouster/sdk/util/__init__.py index 6d24ce25..26a59f56 100644 --- a/python/src/ouster/sdk/util/__init__.py +++ b/python/src/ouster/sdk/util/__init__.py @@ -15,9 +15,6 @@ from .parsing import default_scan_fields # type: ignore from .parsing import scan_to_packets # type: ignore from .parsing import resolve_field_types # type: ignore -from .parsing import PacketFormat # type: ignore -from .parsing import ColHeader # type: ignore -from .parsing import FusaDualFormat # type: ignore from .extrinsics import resolve_extrinsics # type: ignore from .extrinsics import _parse_extrinsics_file # type: ignore diff --git a/python/src/ouster/sdk/util/forward_slicer.py b/python/src/ouster/sdk/util/forward_slicer.py index 7f2f4bfe..803ec2bc 100644 --- a/python/src/ouster/sdk/util/forward_slicer.py +++ b/python/src/ouster/sdk/util/forward_slicer.py @@ -33,32 +33,50 @@ def _slice_clamp(value, length, default): return slice(start, stop, step) @staticmethod - def slice(data_iter: Iterator, key: slice): + def _stepper(data_iter, start, stop, step) -> Iterator: + if step < 0: + # align with the end + step = -step + aligned_start = (stop - 1) - (stop - start) // step * step + if aligned_start < start: + aligned_start += step + for _ in range(aligned_start - start): + next(data_iter) + count = 0 + while count < stop - start: + try: + count += 1 + yield next(data_iter) + for _ in range(step - 1): + count += 1 + next(data_iter) + except StopIteration: + break + + @staticmethod + def slice_iter(data_iter: Iterator, key: slice) -> Iterator: """ Performs forward slicing on a dataset with step Parameters: - key: must be a normalized slice key with relation to the used data_iter. a normalized slice key is one where key.start < key.stop and no non-values + + Returns: + Iterator: an iterator scoped to the input key """ + return ForwardSlicer._stepper(data_iter, key.start, key.stop, key.step) - def _stepper(data_iter, start, stop, step): - out = [] - if step < 0: - # align with the end - step = -step - aligned_start = (stop - 1) - (stop - start) // step * step - if aligned_start < start: - aligned_start += step - for _ in range(aligned_start - start): - next(data_iter) - while True: - try: - out.append(next(data_iter)) - for _ in range(step - 1): - next(data_iter) - except StopIteration: - break - return out + @staticmethod + def slice(data_iter: Iterator, key: slice) -> list: + """ + Performs forward slicing on a dataset with step + + Parameters: + - key: must be a normalized slice key with relation to the used data_iter. + a normalized slice key is one where key.start < key.stop and no non-values - return _stepper(data_iter, key.start, key.stop, key.step) + Returns: + List: a list of items scoped to the input key + """ + return [*ForwardSlicer._stepper(data_iter, key.start, key.stop, key.step)] diff --git a/python/src/ouster/sdk/util/metadata.py b/python/src/ouster/sdk/util/metadata.py index 5fa6f587..789a38e7 100644 --- a/python/src/ouster/sdk/util/metadata.py +++ b/python/src/ouster/sdk/util/metadata.py @@ -6,12 +6,39 @@ from packaging import version import requests import re +from ouster.sdk.client import SensorInfo data_must_be_a_file_err = "The source parameter must be a path to a file." meta_must_be_a_file_err = "The metadata parameter must be a path to a file." +def _check_sensor_metadata_for_duplicates(data_path, metas): + """Check sensor metadata if multiple metadata files are found for the same data path in dir + + Args: + data_path: data filepath found in the directory + metas: metadata filepaths found in the directory + + Returns: + serial number if the metadata files are duplicates else None + """ + metas_set = set() + for meta in metas: + # Open each file, read its content, and create a SensorInfo object + with open(meta) as file: + meta_content = file.read() + si = SensorInfo(meta_content) + if si.sn in metas_set: + s = ["The following metadata files identified for " + f"{data_path} contain configuration for the same sensor {si.sn}. Files: " + f"{set(metas)}", + "To resolve this, remove the extra metadata file(s) or specify the metadata " + "files manually using the --meta option."] + raise RuntimeError("\n".join(s)) + metas_set.add(si.sn) + + def _resolve_metadata_multi_with_prefix_guess(data_path: str) -> List[str]: """Look for best-matching metadata files from all json files in the same dir @@ -49,10 +76,13 @@ def _resolve_metadata_multi_with_prefix_guess(data_path: str) -> List[str]: # Now it requires at least a single character to be common. return [] else: - return [ + metas = [ os.path.join(dirname, b_path) for b_path, _ in filter( lambda i: i[1] == best_score, sorted_options) ] + if len(metas) > 1: + _check_sensor_metadata_for_duplicates(data_path, metas) + return metas def resolve_metadata(data_path: str, diff --git a/python/src/ouster/sdk/util/parsing.py b/python/src/ouster/sdk/util/parsing.py index 7d365404..7a93a878 100644 --- a/python/src/ouster/sdk/util/parsing.py +++ b/python/src/ouster/sdk/util/parsing.py @@ -4,16 +4,13 @@ Doesn't rely on custom C++ extensions (just numpy). Provides writable view of packet data for testing and development. """ -from abc import ABC, abstractmethod -from dataclasses import dataclass -from typing import (Callable, ClassVar, Dict, Type, Union, List, Optional) +from typing import (Dict, Union, List, Optional) import numpy as np import ouster.sdk.client as client -from ouster.sdk.client import (ChanField, ColHeader, FieldDType, SensorInfo, - UDPProfileLidar, LidarPacket) -from ouster.sdk.client._client import PacketWriter, get_field_types +from ouster.sdk.client import (ChanField, FieldDType, UDPProfileLidar, LidarPacket) +from ouster.sdk.client._client import PacketWriter, get_field_types, FieldType def default_scan_fields( @@ -37,13 +34,14 @@ def default_scan_fields( fields = get_field_types(profile) + # todo remove me when we default fields on in C++ if flags: - fields.update({ChanField.FLAGS: np.uint8}) + fields.append(FieldType(client.ChanField.FLAGS, np.uint8)) if profile in [ UDPProfileLidar.PROFILE_LIDAR_RNG19_RFL8_SIG16_NIR16_DUAL, UDPProfileLidar.PROFILE_LIDAR_FUSA_RNG15_RFL8_NIR8_DUAL ]: - fields.update({ChanField.FLAGS2: np.uint8}) + fields.append(FieldType(client.ChanField.FLAGS2, np.uint8)) if raw_headers: # Getting the optimal field type for RAW_HEADERS is not possible with @@ -51,8 +49,8 @@ def default_scan_fields( # Alternatively you can use `osf.resolve_field_types()` that chooses # the more optimal dtype for RAW_HEADERS field - fields.update({ChanField.RAW_HEADERS: np.uint32}) - + fields.append(FieldType(ChanField.RAW_HEADERS, np.uint32)) + fields.sort() return fields.copy() @@ -91,39 +89,30 @@ def resolve_field_types( ftypes = client.get_field_types(m) profile = m.format.udp_profile_lidar - # HACK: Overwrite fields to reduced datatypes for LEGACY (saves ~15% of - # space in a file) - if profile == client.UDPProfileLidar.PROFILE_LIDAR_LEGACY: - ftypes.update( - dict({ - client.ChanField.RANGE: np.uint32, - client.ChanField.SIGNAL: np.uint16, - client.ChanField.REFLECTIVITY: np.uint16, - client.ChanField.NEAR_IR: np.uint16 - })) + dual = False + if profile in [ + UDPProfileLidar.PROFILE_LIDAR_RNG19_RFL8_SIG16_NIR16_DUAL, + UDPProfileLidar.PROFILE_LIDAR_FUSA_RNG15_RFL8_NIR8_DUAL + ]: + dual = True + # todo remove me when we default fields on in C++ if flags: - ftypes.update({client.ChanField.FLAGS: np.uint8}) - if client.ChanField.RANGE2 in ftypes: - ftypes.update({client.ChanField.FLAGS2: np.uint8}) + ftypes.append(FieldType(client.ChanField.FLAGS, np.uint8)) + if dual: + ftypes.append(FieldType(client.ChanField.FLAGS2, np.uint8)) if raw_fields: - ftypes.update({client.ChanField.RAW32_WORD1: np.uint32}) + ftypes.append(FieldType(client.ChanField.RAW32_WORD1, np.uint32)) if profile != client.UDPProfileLidar.PROFILE_LIDAR_RNG15_RFL8_NIR8: # not Low Bandwidth - ftypes.update( - {client.ChanField.RAW32_WORD2: np.uint32}) - ftypes.update( - {client.ChanField.RAW32_WORD3: np.uint32}) - if client.ChanField.RANGE2 in ftypes: - ftypes.update( - {client.ChanField.RAW32_WORD4: np.uint32}) - if profile == client.UDPProfileLidar.PROFILE_LIDAR_FIVE_WORD_PIXEL: - ftypes.update( - dict({ - client.ChanField.RAW32_WORD4: np.uint32, - client.ChanField.RAW32_WORD5: np.uint32 - })) + ftypes.append( + FieldType(client.ChanField.RAW32_WORD2, np.uint32)) + ftypes.append( + FieldType(client.ChanField.RAW32_WORD3, np.uint32)) + if dual: + ftypes.append( + FieldType(client.ChanField.RAW32_WORD4, np.uint32)) if raw_headers: # getting the optimal field type for RAW_HEADERS @@ -137,375 +126,13 @@ def resolve_field_types( np.uint16, np.uint32 ][int(raw_headers_space / h)] - ftypes.update({client.ChanField.RAW_HEADERS: dtype}) # type: ignore - + ftypes.append(FieldType(client.ChanField.RAW_HEADERS, dtype)) # type: ignore + ftypes.sort() field_types.append(ftypes) return field_types[0] if single_result else field_types -@dataclass -class FieldDescr: - offset: int - dtype: type - mask: int = 0 - shift: int = 0 - - -class MaskedView: - - def __init__(self, data: np.ndarray, pf: 'PacketFormat', - field: Union[ColHeader, ChanField]) -> None: - if len(data) < pf.lidar_packet_size: - raise ValueError("Packet buffer smaller than expected size") - if isinstance(field, ChanField): - f = pf._FIELDS[field] - self._data = np.lib.stride_tricks.as_strided( - data[pf._packet_header_size + pf._col_header_size + - f.offset:].view(dtype=f.dtype), - shape=(pf._pixels_per_column, pf._columns_per_packet), - strides=(pf._channel_data_size, pf.column_size)) - elif isinstance(field, ColHeader): - f = pf._HEADERS[field] - start = 0 if f.offset >= 0 else pf.column_size - self._data = np.lib.stride_tricks.as_strided( - data[pf._packet_header_size + f.offset + - start:].view(dtype=f.dtype), - shape=(pf._columns_per_packet, ), - strides=(pf.column_size, )) - - self._mask = f.mask - self._shift = f.shift - - def __getitem__(self, key) -> np.ndarray: - value = self._data.__getitem__(key) - if self._mask: - value = value & self._mask - if self._shift > 0: - value = value >> self._shift - elif self._shift < 0: - value = value << abs(self._shift) - return value - - def __setitem__(self, key, value) -> None: - # TODO: how to support operators for MaskedView - if self._shift > 0: - value = np.left_shift(value, self._shift) - elif self._shift < 0: - value = np.right_shift(value, abs(self._shift)) - if self._mask: - old = self._data.__getitem__(key) - value = np.bitwise_and(value, self._mask) | np.bitwise_and( - old, ~self._mask) - self._data.__setitem__(key, value) - - def __len__(self) -> int: - return self._data.__len__() - - def __repr__(self) -> str: - return self._data.__repr__() - - def __getattribute__(self, att): - if att in ['_data', '_mask', '_shift']: - return object.__getattribute__(self, att) - return getattr(self._data, att) - - -class PacketFormat(ABC): - """Read lidar packet data using numpy views.""" - - _pixels_per_column: int - _columns_per_packet: int - - _packet_header_size: int - _packet_footer_size: int - _col_header_size: int - _col_footer_size: int - _channel_data_size: int - - column_size: int - lidar_packet_size: int - - _HEADERS: ClassVar[Dict[ColHeader, FieldDescr]] = {} - _FIELDS: ClassVar[Dict[ChanField, FieldDescr]] = {} - - def __init__(self, *, packet_header_size: int, columns_per_packet: int, - col_header_size: int, pixels_per_column: int, - channel_data_size: int, col_footer_size: int, - packet_footer_size: int) -> None: - - self._packet_header_size = packet_header_size - self._columns_per_packet = columns_per_packet - self._col_header_size = col_header_size - self._pixels_per_column = pixels_per_column - self._channel_data_size = channel_data_size - self._col_footer_size = col_footer_size - self._packet_footer_size = packet_footer_size - - self.column_size = (self._col_header_size + - self._pixels_per_column * self._channel_data_size + - self._col_footer_size) - self.lidar_packet_size = (self._packet_header_size + - self._columns_per_packet * self.column_size + - self._packet_footer_size) - - # blah ... could make formats a view and these properties - @abstractmethod - def packet_type(self, data: np.ndarray) -> int: - ... - - @abstractmethod - def set_packet_type(self, data: np.ndarray, val: int) -> None: - ... - - @abstractmethod - def frame_id(self, data: np.ndarray) -> int: - ... - - @abstractmethod - def set_frame_id(self, data: np.ndarray, val: int) -> None: - ... - - @abstractmethod - def init_id(self, data: np.ndarray) -> int: - ... - - @abstractmethod - def set_init_id(self, data: np.ndarray, val: int) -> None: - ... - - @abstractmethod - def prod_sn(self, data: np.ndarray) -> int: - ... - - @abstractmethod - def set_prod_sn(self, data: np.ndarray, val: int) -> None: - ... - - def field(self, data: np.ndarray, field: ChanField) -> MaskedView: - return MaskedView(data, self, field) - - def header(self, data: np.ndarray, header: ColHeader) -> MaskedView: - return MaskedView(data, self, header) - - @staticmethod - def from_profile(profile: UDPProfileLidar, pixels_per_column: int, - columns_per_packet: int) -> 'PacketFormat': - - formats: Dict[UDPProfileLidar, Callable[[int, int], PacketFormat]] = { - UDPProfileLidar.PROFILE_LIDAR_LEGACY: LegacyFormat, - UDPProfileLidar.PROFILE_LIDAR_RNG19_RFL8_SIG16_NIR16_DUAL: - DualFormat, - UDPProfileLidar.PROFILE_LIDAR_RNG19_RFL8_SIG16_NIR16: SingleFormat, - UDPProfileLidar.PROFILE_LIDAR_RNG15_RFL8_NIR8: LBFormat, - UDPProfileLidar.PROFILE_LIDAR_FUSA_RNG15_RFL8_NIR8_DUAL: FusaDualFormat, - } - return formats[profile](pixels_per_column, columns_per_packet) - - @staticmethod - def from_metadata(meta: SensorInfo) -> 'PacketFormat': - return PacketFormat.from_profile(meta.format.udp_profile_lidar, - meta.format.pixels_per_column, - meta.format.columns_per_packet) - - @staticmethod - def convertible(a: Type['PacketFormat'], b: Type['PacketFormat']) -> bool: - """Check if it's possible to convert packets of ``a`` to ``b``.""" - return (all(f in a._FIELDS.keys() for f in b._FIELDS.keys()) - and all(h in a._HEADERS.keys() for h in b._HEADERS.keys())) - - -class LegacyFormat(PacketFormat): - - _HEADERS: ClassVar[Dict[ColHeader, FieldDescr]] = { - ColHeader.TIMESTAMP: FieldDescr(offset=0, dtype=np.uint64), - ColHeader.MEASUREMENT_ID: FieldDescr(8, np.uint16), - ColHeader.FRAME_ID: FieldDescr(offset=10, dtype=np.uint16), - ColHeader.ENCODER_COUNT: FieldDescr(12, np.uint32), - ColHeader.STATUS: FieldDescr(-4, np.uint32), - } - - _FIELDS: ClassVar[Dict[ChanField, FieldDescr]] = { - ChanField.RANGE: FieldDescr(0, np.uint32, mask=0x000fffff), - ChanField.REFLECTIVITY: FieldDescr(4, np.uint16), - ChanField.SIGNAL: FieldDescr(6, np.uint16), - ChanField.FLAGS: FieldDescr(3, np.uint8, shift=4), - ChanField.NEAR_IR: FieldDescr(8, np.uint16), - } - - def __init__(self, pixels_per_column: int, columns_per_packet: int): - super().__init__(packet_header_size=0, - columns_per_packet=columns_per_packet, - col_header_size=16, - pixels_per_column=pixels_per_column, - channel_data_size=12, - col_footer_size=4, - packet_footer_size=0) - - def packet_type(self, data: np.ndarray) -> int: - return 0 - - def set_packet_type(self, data: np.ndarray, val: int) -> None: - pass - - def frame_id(self, data: np.ndarray) -> int: - return self.header(data, ColHeader.FRAME_ID)[0].item() - - def set_frame_id(self, data: np.ndarray, val: int) -> None: - self.header(data, ColHeader.FRAME_ID)[:] = val - - def init_id(self, data: np.ndarray) -> int: - return 0 - - def set_init_id(self, data: np.ndarray, val: int) -> None: - pass - - def prod_sn(self, data: np.ndarray) -> int: - return 0 - - def set_prod_sn(self, data: np.ndarray, val: int) -> None: - pass - - -class EUDPFormat(PacketFormat): - - _HEADERS: ClassVar[Dict[ColHeader, FieldDescr]] = { - ColHeader.TIMESTAMP: FieldDescr(0, np.uint64), - ColHeader.MEASUREMENT_ID: FieldDescr(8, np.uint16), - ColHeader.STATUS: FieldDescr(10, np.uint16), - } - - def __init__(self, pixels_per_column: int, columns_per_packet: int, - channel_data_size: int) -> None: - super().__init__(packet_header_size=32, - columns_per_packet=columns_per_packet, - col_header_size=12, - pixels_per_column=pixels_per_column, - channel_data_size=channel_data_size, - col_footer_size=0, - packet_footer_size=32) - - def packet_type(self, data: np.ndarray) -> int: - return int.from_bytes(data[0:2].tobytes(), byteorder='little') - - def set_packet_type(self, data: np.ndarray, val: int) -> None: - data[0:2] = memoryview(val.to_bytes(2, byteorder='little')) - - def frame_id(self, data: np.ndarray) -> int: - return int.from_bytes(data[2:4].tobytes(), byteorder='little') - - def set_frame_id(self, data: np.ndarray, val: int) -> None: - data[2:4] = memoryview(val.to_bytes(2, byteorder='little')) - - def init_id(self, data: np.ndarray) -> int: - return int.from_bytes(data[4:7].tobytes(), byteorder='little') - - def set_init_id(self, data: np.ndarray, val: int) -> None: - data[4:7] = memoryview(val.to_bytes(3, byteorder='little')) - - def prod_sn(self, data: np.ndarray) -> int: - return int.from_bytes(data[7:12].tobytes(), byteorder='little') - - def set_prod_sn(self, data: np.ndarray, val: int) -> None: - data[7:12] = memoryview(val.to_bytes(5, byteorder='little')) - - -class LBFormat(EUDPFormat): - """PROFILE_RNG15_RFL8_NIR8""" - - _FIELDS: ClassVar[Dict[ChanField, FieldDescr]] = { - ChanField.RANGE: FieldDescr(0, np.uint16, mask=0x7fff, shift=-3), - ChanField.REFLECTIVITY: FieldDescr(2, np.uint8), - ChanField.NEAR_IR: FieldDescr(3, np.uint16, mask=0xff00, shift=4), - } - - def __init__(self, pixels_per_column: int, - columns_per_packet: int) -> None: - super().__init__(pixels_per_column, - columns_per_packet, - channel_data_size=4) - - -class SingleFormat(EUDPFormat): - """PROFILE_RNG19_RFL8_SIG16_NIR16""" - - _FIELDS: ClassVar[Dict[ChanField, FieldDescr]] = { - ChanField.RANGE: FieldDescr(0, np.uint32, mask=0x000fffff), - ChanField.REFLECTIVITY: FieldDescr(4, np.uint16), - ChanField.SIGNAL: FieldDescr(6, np.uint16), - ChanField.FLAGS: FieldDescr(3, np.uint8, shift=4), - ChanField.NEAR_IR: FieldDescr(8, np.uint16), - } - - def __init__(self, pixels_per_column: int, - columns_per_packet: int) -> None: - super().__init__(pixels_per_column, - columns_per_packet, - channel_data_size=12) - - -class DualFormat(EUDPFormat): - """PROFILE_RNG19_RFL8_SIG16_NIR16_DUAL""" - - _FIELDS: ClassVar[Dict[ChanField, FieldDescr]] = { - ChanField.RANGE: FieldDescr(0, np.uint32, mask=0x0007ffff), - ChanField.REFLECTIVITY: FieldDescr(3, np.uint8), - ChanField.RANGE2: FieldDescr(4, np.uint32, mask=0x0007ffff), - ChanField.REFLECTIVITY2: FieldDescr(7, np.uint8), - ChanField.SIGNAL: FieldDescr(8, np.uint16), - ChanField.SIGNAL2: FieldDescr(10, np.uint16), - ChanField.FLAGS: FieldDescr(2, np.uint8, mask=0b11111000, shift=3), - ChanField.FLAGS2: FieldDescr(6, np.uint8, mask=0b11111000, shift=3), - ChanField.NEAR_IR: FieldDescr(12, np.uint16), - } - - def __init__(self, pixels_per_column: int, columns_per_packet) -> None: - super().__init__(pixels_per_column=pixels_per_column, - columns_per_packet=columns_per_packet, - channel_data_size=16) - - -class FusaDualFormat(EUDPFormat): - """PROFILE_FUSA_RNG15_RFL8_NIR8_DUAL""" - - _FIELDS: ClassVar[Dict[ChanField, FieldDescr]] = { - ChanField.RANGE: FieldDescr(0, np.uint16, mask=0x0007fff, shift=-3), - ChanField.REFLECTIVITY: FieldDescr(2, np.uint8, mask=0xff), - ChanField.NEAR_IR: FieldDescr(3, np.uint16, mask=0xff, shift=-4), - ChanField.RANGE2: FieldDescr(4, np.uint16, mask=0x0007fff, shift=-3), - ChanField.REFLECTIVITY2: FieldDescr(6, np.uint8, mask=0xff), - } - - def __init__(self, pixels_per_column: int, columns_per_packet) -> None: - super().__init__(pixels_per_column=pixels_per_column, - columns_per_packet=columns_per_packet, - channel_data_size=8) - - def packet_type(self, data: np.ndarray) -> int: - return int.from_bytes(data[0:1].tobytes(), byteorder='little') - - def set_packet_type(self, data: np.ndarray, val: int) -> None: - data[0:1] = memoryview(val.to_bytes(2, byteorder='little')) - - def frame_id(self, data: np.ndarray) -> int: - return int.from_bytes(data[4:8].tobytes(), byteorder='little') - - def set_frame_id(self, data: np.ndarray, val: int) -> None: - data[4:8] = memoryview(val.to_bytes(4, byteorder='little')) - - def init_id(self, data: np.ndarray) -> int: - return int.from_bytes(data[1:4].tobytes(), byteorder='little') - - def set_init_id(self, data: np.ndarray, val: int) -> None: - data[1:4] = memoryview(val.to_bytes(3, byteorder='little')) - - def prod_sn(self, data: np.ndarray) -> int: - return int.from_bytes(data[11:16].tobytes(), byteorder='little') - - def set_prod_sn(self, data: np.ndarray, val: int) -> None: - data[11:16] = memoryview(val.to_bytes(5, byteorder='little')) - - def tohex(data: client.BufferT) -> str: """Makes a hex string for debug print outs of buffers. @@ -533,120 +160,6 @@ def tohex(data: client.BufferT) -> str: return "[]" -class LidarPacketHeaders: - """Gets access to the headers and footers of the lidar packet buffer.""" - - def __init__(self, pf: client._client.PacketFormat) -> None: - self._pf = pf - - def packet_header(self, packet_buf: client.BufferT) -> np.ndarray: - if self._pf.packet_header_size: - return self._uint8_view(packet_buf)[:self._pf.packet_header_size] - else: - return np.empty(0, dtype=np.uint8) - - def packet_footer(self, packet_buf: client.BufferT) -> np.ndarray: - if self._pf.packet_footer_size: - return self._uint8_view(packet_buf)[-self._pf.packet_footer_size:] - else: - return np.empty(0, dtype=np.uint8) - - def col_header(self, packet_buf: client.BufferT, - col_idx: int) -> np.ndarray: - if self._pf.col_header_size: - col_offset = (self._pf.packet_header_size + - self._pf.col_size * col_idx) - return self._uint8_view(packet_buf)[col_offset:col_offset + - self._pf.col_header_size] - else: - return np.empty(0, dtype=np.uint8) - - def col_footer(self, packet_buf: client.BufferT, - col_idx: int) -> np.ndarray: - if self._pf.col_footer_size: - col_end_offset = (self._pf.packet_header_size + self._pf.col_size * - (col_idx + 1)) - return self._uint8_view( - packet_buf)[col_end_offset - - self._pf.col_footer_size:col_end_offset] - else: - return np.empty(0, dtype=np.uint8) - - def _uint8_view(self, packet_buf: client.BufferT) -> np.ndarray: - return np.frombuffer(packet_buf, - dtype=np.uint8, - count=self._pf.lidar_packet_size) - - -class RawHeadersFormat: - """Accessor to the single column headers layout. - - ``col_view`` param in every accessor function is a packer column - from the RAW_HEADERS field, the layout used to pack the packet - headers and footers is the following: - - -- [ col_header ] [col_footer ] [ packet_header ] [ packet_footer ] -- - - ``col_view`` element data type is uint32, thus the conversion is required - to a uint8 view. - - Also ``col_view`` by default is not a CONTIGUOUS byte buffer because of - internal LidarScan fields representation as a C (i.e. row major) order, - thus there is additional handler to convert ``col_view`` buffer into a - `C_CONTIGUOUS` byte order for a direct buffer accessing operations. - - NOTE[pb]: There definitely other options to make it more efficient with - numpy ops and Python, but I wasn't able to figure out the - faster way and there is a know speed issues with such accessors. - - Better alternative would be to implement accessors and - ``scan_to_packets`` operations in C++ with a corresponding - bindings, maybe sometime later when there will be asks ... - """ - - def __init__(self, pf: client._client.PacketFormat) -> None: - self._pf = pf - - def packet_header(self, col_view: np.ndarray) -> np.ndarray: - if self._pf.packet_header_size: - header_offset = self._pf.col_header_size + self._pf.col_footer_size - return self._as_uint8(col_view)[header_offset:header_offset + - self._pf.packet_header_size] - else: - return np.empty(0, dtype=np.uint8) - - def packet_footer(self, col_view: np.ndarray) -> np.ndarray: - if self._pf.packet_footer_size: - footer_offset = (self._pf.col_header_size + - self._pf.col_footer_size + - self._pf.packet_header_size) - return self._as_uint8(col_view)[footer_offset:footer_offset + - self._pf.packet_footer_size] - else: - return np.empty(0, dtype=np.uint8) - - def col_footer(self, col_view: np.ndarray) -> np.ndarray: - if self._pf.col_footer_size: - return self._as_uint8( - col_view)[self._pf.col_header_size:self._pf.col_header_size + - self._pf.col_footer_size] - else: - return np.empty(0, dtype=np.uint8) - - def col_header(self, col_view: np.ndarray) -> np.ndarray: - if self._pf.col_header_size: - return self._as_uint8(col_view)[:self._pf.col_header_size] - else: - return np.empty(0, dtype=np.uint8) - - def _as_uint8(self, col_buf: np.ndarray) -> np.ndarray: - if not col_buf.flags['C_CONTIGUOUS']: - col_buf_cont = np.ascontiguousarray(col_buf) - return np.frombuffer(col_buf_cont, dtype=np.uint8) - else: - return np.frombuffer(col_buf, dtype=np.uint8) - - def scan_to_packets(ls: client.LidarScan, info: client.SensorInfo) -> List[LidarPacket]: """Converts LidarScar to a lidar_packet buffers @@ -689,10 +202,10 @@ def terminator_packet(info: client.SensorInfo, # get frame_id using client.PacketFormat pf = client._client.PacketFormat.from_info(info) - curr_fid = pf.frame_id(last_packet._data) + curr_fid = pf.frame_id(last_packet.buf) pw = PacketWriter.from_info(info) - last_buf_view = np.frombuffer(last_packet._data, + last_buf_view = np.frombuffer(last_packet.buf, dtype=np.uint8, count=pf.lidar_packet_size) diff --git a/python/src/ouster/sdk/viz/__init__.py b/python/src/ouster/sdk/viz/__init__.py index 2d2d5d7a..e7435dad 100644 --- a/python/src/ouster/sdk/viz/__init__.py +++ b/python/src/ouster/sdk/viz/__init__.py @@ -24,7 +24,6 @@ from .core import push_point_viz_handler from .core import LidarScanViz from .core import SimpleViz -from .core import scans_accum_for_cli from .view_mode import ImageMode from .view_mode import CloudMode from .view_mode import ImageCloudMode @@ -33,4 +32,4 @@ from .util import AxisWithLabel -from .scans_accum import ScansAccumulator \ No newline at end of file +from .scans_accum import ScansAccumulator diff --git a/python/src/ouster/sdk/viz/core.py b/python/src/ouster/sdk/viz/core.py index 85432b30..e2163030 100644 --- a/python/src/ouster/sdk/viz/core.py +++ b/python/src/ouster/sdk/viz/core.py @@ -35,7 +35,7 @@ from .util import push_point_viz_handler, push_point_viz_fb_handler from . import util as vizu -from .view_mode import (ImageMode, CloudMode, LidarScanVizMode, ReflMode, +from .view_mode import (ImageMode, CloudMode, LidarScanVizMode, ReflMode, RGBMode, SimpleMode, is_norm_reflectivity_mode, CloudPaletteItem) from .scans_accum import ScansAccumulator @@ -167,7 +167,8 @@ def __init__( self._refl_cloud_palettes: List[CloudPaletteItem] self._refl_cloud_palettes = [ - CloudPaletteItem("Cal. Ref. Ouster Colors", spezia_cal_ref_palette), + CloudPaletteItem("Cal. Ref. Ouster Colors", + spezia_cal_ref_palette), CloudPaletteItem("Cal. Ref. Greyscale", grey_cal_ref_palette), CloudPaletteItem("Cal. Ref. Viridis", viridis_cal_ref_palette), CloudPaletteItem("Cal. Ref. Magma", magma_cal_ref_palette), @@ -201,7 +202,8 @@ def __init__( self._modes: List[LidarScanVizMode] self._modes = [ ReflMode(info=meta), - SimpleMode(ChanField.NEAR_IR, info=meta, use_ae=True, use_buc=True), + SimpleMode(ChanField.NEAR_IR, info=meta, + use_ae=True, use_buc=True), SimpleMode(ChanField.SIGNAL, info=meta), SimpleMode(ChanField.RANGE, info=meta), ] @@ -211,15 +213,13 @@ def __init__( self._modes.extend(_ext_modes or []) - self._image_modes: List[ImgModeItem] - self._image_modes = [ - ImgModeItem(mode, name, num) for mode in self._modes - if isinstance(mode, ImageMode) - for num, name in enumerate(mode.names) - ] + self._populate_image_cloud_modes() - self._cloud_modes: List[CloudMode] - self._cloud_modes = [m for m in self._modes if isinstance(m, CloudMode)] + self._amended_fields: List[str] = [ChanField.RANGE, ChanField.RANGE2, + ChanField.SIGNAL, ChanField.SIGNAL2, + ChanField.REFLECTIVITY, ChanField.REFLECTIVITY2, + ChanField.NEAR_IR, + ChanField.FLAGS, ChanField.FLAGS2] # TODO[pb]: Extract FlagsMode to custom processor (TBD the whole thing) # initialize masks to just green with zero opacity @@ -263,7 +263,8 @@ def __init__( # scan identity poses to re-use self._scan_column_poses_identity = np.array( - [np.eye(4) for _ in range(self._metadata.format.columns_per_frame)], + [np.eye(4) + for _ in range(self._metadata.format.columns_per_frame)], dtype=np.float32) # extension point for the OSD text, inserts before the "axes" line @@ -321,6 +322,7 @@ def handle_keys(self: LidarScanViz, ctx: WindowCtx, key: int, "n": "Cycle bottom 2D image", 'm': "Cycle through point cloud coloring mode", 'f': "Cycle through point cloud color palette", + 'c': "Cycle current highlight mode", # 't': "Toggle scan poses", 'u': "Toggle camera mode FOLLOW/FIXED", "9": "Toggle axis helpers at scan origin", @@ -343,6 +345,19 @@ def handle_keys(self: LidarScanViz, ctx: WindowCtx, key: int, push_point_viz_handler(self._viz, self, handle_keys) add_default_controls(self._viz) + def _populate_image_cloud_modes(self) -> None: + + self._image_modes: List[ImgModeItem] + self._image_modes = [ + ImgModeItem(mode, name, num) for mode in self._modes + if isinstance(mode, ImageMode) + for num, name in enumerate(mode.names) + ] + + self._cloud_modes: List[CloudMode] + self._cloud_modes = [ + m for m in self._modes if isinstance(m, CloudMode)] + def cycle_img_mode(self, i: int, *, direction: int = 1) -> None: """Change the displayed field of the i'th image.""" with self._lock: @@ -469,7 +484,8 @@ def print_keys(self) -> None: with self._lock: print(">---------------- Key Bindings --------------<") for key_binding in self._key_definitions: - print(f"{key_binding:^7}: {self._key_definitions[key_binding]}") + print( + f"{key_binding:^7}: {self._key_definitions[key_binding]}") print(">--------------------------------------------<") @property @@ -487,6 +503,33 @@ def scan_num(self) -> int: """The currently displayed scan number""" return self._scan_num + def _amend_view_modes(self, scan: client.LidarScan) -> None: + # Look for any new field that doesn't have a view mode associated with yet + # NOTE: we don't currently handle edge cases like removing a view mode if + # the field associated with it was dropped or deleted. + # Another situation that is not currently handled if there are more than one + # field that share the same name but could have different dimensions. In the + # current code the first field to appear will acquire the view mode. + # Will handle all these edge cases in "LidarScanViz Reforms" work. + amended_fields_length = len(self._amended_fields) # save the value + for field_name in scan.fields: + if field_name not in self._amended_fields: + # we encountered new field + field = scan.field(field_name) + # for now only consider 2D + 3D + if np.ndim(field) == 2: + self._modes.extend([ + SimpleMode(field_name, info=self.metadata)]) + self._amended_fields.append(field_name) + elif np.ndim(field) == 3 and field.shape[2] == 3: + self._modes.extend([ + RGBMode(field_name, info=self.metadata)]) + self._amended_fields.append(field_name) + # else any other shape we simply skip over + + if amended_fields_length != len(self._amended_fields): + self._populate_image_cloud_modes() + def update(self, scan: client.LidarScan, scan_num: Optional[int] = None) -> None: @@ -496,6 +539,8 @@ def update(self, else: self._scan_num += 1 + self._amend_view_modes(scan) + def draw(self, update: bool = True) -> bool: """Process and draw the latest state to the screen.""" with self._lock: @@ -589,7 +634,8 @@ def _draw(self) -> None: first_ts_s = client.first_valid_column_ts(scan) / 1e9 cloud_idxs_str = str([i + 1 for i in range(len(self._cloud_enabled))]) - cloud_states_str = ", ".join(["ON" if e else "OFF" for e in self._cloud_enabled]) + cloud_states_str = ", ".join( + ["ON" if e else "OFF" for e in self._cloud_enabled]) osd_str_extra = self._osd_text_extra() if osd_str_extra: @@ -599,6 +645,7 @@ def _draw(self) -> None: osd_str = f"image [B, N]: {img_modes[self._img_ind[0]].name}, {img_modes[self._img_ind[1]].name}\n" \ f"cloud {cloud_idxs_str}: {cloud_states_str}\n" osd_str += f" mode [M]: {cloud_modes[self._cloud_mode_ind].name}\n" \ + f" flags highlight mode[c]: {self._flags_mode.name}\n" \ f" palette [F]: {self._cloud_palette_name}\n" \ f" point size [P]: {int(self._cloud_pt_size)}\n" osd_str += f"{osd_str_extra}" \ @@ -606,7 +653,7 @@ def _draw(self) -> None: f"camera mode [U]: {'FOLLOW' if self._camera_follow_enabled else 'FIXED'}\n" \ f"frame: {scan.frame_id}, sensor ts: {first_ts_s:.3f}s\n" \ f"profile: {str(meta.format.udp_profile_lidar).replace('_', '..')}\n" \ - f"{meta.prod_line} {meta.fw_rev} {meta.mode}" + f"{meta.prod_line} {meta.fw_rev} {meta.config.lidar_mode}" else: osd_str = "" @@ -749,10 +796,10 @@ def close(self) -> None: def _save_fb_to_png(fb_data: List, - fb_width: int, - fb_height: int, - action_name: Optional[str] = "screenshot", - file_path: Optional[str] = None): + fb_width: int, + fb_height: int, + action_name: Optional[str] = "screenshot", + file_path: Optional[str] = None): img_arr = np.array(fb_data, dtype=np.uint8).reshape([fb_height, fb_width, 3]) img_fname = datetime.now().strftime( @@ -838,7 +885,7 @@ def __init__(self, metas = ([self._scan_viz.metadata] if isinstance( self._scan_viz.metadata, client.SensorInfo) else - self._scan_viz.metadata) + self._scan_viz.metadata) self._scans_accum = None # TODO[pb]: Here we consider ScansAccumulator separately for @@ -853,7 +900,8 @@ def __init__(self, if self._scans_accum.viz is None: self._scans_accum.set_point_viz(self._viz) else: - self._scans_accum = ScansAccumulator(metas, point_viz=self._viz) + self._scans_accum = ScansAccumulator( + metas, point_viz=self._viz) self._scans_accum.toggle_osd(False) if hasattr(self._scan_viz, "_osd_text_extra"): @@ -1044,9 +1092,9 @@ def _process(self, seekable: _Seekable[client.LidarScan]) -> None: self._update_playback_osd() - # sleep for remainder of scan period + # sleep for remainder of scan period if not live to_sleep = max(0.0, period - (time.monotonic() - last_ts)) - if scan_idx > 0: + if scan_idx > 0 and not self._live: time.sleep(to_sleep) now_t = time.monotonic() @@ -1127,35 +1175,6 @@ def run(self, scans: Iterable[client.LidarScan]) -> None: proc_thread.join() -def scans_accum_for_cli(metas: Union[client.SensorInfo, - List[client.SensorInfo]], - *, - accum_num: int = 0, - accum_every: Optional[int] = None, - accum_every_m: Optional[float] = None, - accum_map: bool = False, - accum_map_ratio: float = 0.001) -> ScansAccumulator: - """Create ScansAccumulator using its CLI params.""" - a_every = 1 - a_every_m = 0.0 - if accum_every is None and accum_every_m is not None: - a_every = 0 - a_every_m = accum_every_m - elif accum_every is not None and accum_every_m is None: - a_every = accum_every - a_every_m = 0.0 - elif accum_every is not None and accum_every_m is not None: - a_every = accum_every - a_every_m = accum_every_m - - return ScansAccumulator(metas, - accum_max_num=accum_num, - accum_min_dist_num=a_every, - accum_min_dist_meters=a_every_m, - map_enabled=accum_map, - map_select_ratio=accum_map_ratio) - - __all__ = [ 'PointViz', 'Cloud', 'Image', 'Cuboid', 'Label', 'WindowCtx', 'Camera', 'TargetDisplay', 'add_default_controls', 'calref_palette', 'spezia_palette', diff --git a/python/src/ouster/sdk/viz/multi_viz.py b/python/src/ouster/sdk/viz/multi_viz.py index 7d0c8ca7..645f674c 100644 --- a/python/src/ouster/sdk/viz/multi_viz.py +++ b/python/src/ouster/sdk/viz/multi_viz.py @@ -24,15 +24,12 @@ class CloudMode(Enum): RANGE = 1 SIGNAL = 2 NEAR_IR = 3 - RGB = 4 _mode_to_channels: Dict[CloudMode, List[ChanField]] = { CloudMode.RANGE: [ChanField.RANGE], CloudMode.REFLECTIVITY: [ChanField.REFLECTIVITY], CloudMode.SIGNAL: [ChanField.SIGNAL], - CloudMode.NEAR_IR: [ChanField.NEAR_IR], - CloudMode.RGB: - [ChanField.CUSTOM0, ChanField.CUSTOM1, ChanField.CUSTOM2] + CloudMode.NEAR_IR: [ChanField.NEAR_IR] } class ImagesLayout(Enum): @@ -365,31 +362,6 @@ def _draw(self) -> None: self._clouds[idx].set_key(key_data) self._images[idx].set_image( client.destagger(self._metas[idx], key_data)) - elif self._cloud_mode == MultiLidarScanViz.CloudMode.RGB: - r = ls.field(ChanField.CUSTOM0) - g = ls.field(ChanField.CUSTOM1) - b = ls.field(ChanField.CUSTOM2) - - normalizer = 255 - - # for types other than uint8 for RED, GREEN, BLUE channels - # we try to check are there really value bigger than 255 - if (r.dtype != np.uint8 or g.dtype != np.uint8 - or b.dtype != np.uint8): - max_rgb = np.max((np.max(r), np.max(g), np.max(b))) - if max_rgb > 255: - normalizer = 65535 - - r = (r / normalizer).clip(0, 1.0).astype(np.float32) - g = (g / normalizer).clip(0, 1.0).astype(np.float32) - b = (b / normalizer).clip(0, 1.0).astype(np.float32) - - rgb_data = np.dstack((r, g, b)) - - self._clouds[idx].set_key(rgb_data) - self._images[idx].set_image( - client.destagger(self._metas[idx], rgb_data)) - else: key_zeros = np.zeros( (self._metas[idx].format.pixels_per_column, @@ -432,7 +404,6 @@ def _update_multi_viz_osd(self): if (self._ae_enabled and self._cloud_mode not in [ MultiLidarScanViz.CloudMode.REFLECTIVITY, - MultiLidarScanViz.CloudMode.RGB ]): ae_str = '(AE)' else: diff --git a/python/src/ouster/sdk/viz/scans_accum.py b/python/src/ouster/sdk/viz/scans_accum.py index cb258c6c..b12019da 100644 --- a/python/src/ouster/sdk/viz/scans_accum.py +++ b/python/src/ouster/sdk/viz/scans_accum.py @@ -27,7 +27,7 @@ from ouster.sdk.client import ChanField import ouster.sdk.util.pose_util as pu -from .view_mode import (CloudMode, ReflMode, SimpleMode, +from .view_mode import (CloudMode, ReflMode, SimpleMode, RGBMode, is_norm_reflectivity_mode, CloudPaletteItem) logger = logging.getLogger("viz-accum-logger") @@ -176,11 +176,13 @@ def __init__(self, SimpleMode(ChanField.RANGE, info=self._metas[self._sensor_idx]), ] - # index of available modes "pens" to get colors for point clouds - # start with all _cloud_modes and then check on every seen scan - # that is used for map/accum that we can still use available - # modes on such scans - self._available_modes: List[int] = list(range(len(self._cloud_modes))) + self._amended_fields: List[str] = [ChanField.RANGE, ChanField.RANGE2, + ChanField.SIGNAL, ChanField.SIGNAL2, + ChanField.REFLECTIVITY, ChanField.REFLECTIVITY2, + ChanField.NEAR_IR, + ChanField.FLAGS, ChanField.FLAGS2] + + self._populate_cloud_modes() # init view cloud mode toggle self._cloud_mode_ind = 0 @@ -211,6 +213,7 @@ def __init__(self, # initialize MAP structs map_init_points_num = MAP_INIT_POINTS_NUM if self._map_enabled else 0 + map_init_points_num = min(self._map_max_points, map_init_points_num) self._map_xyz = np.zeros((map_init_points_num, 3), dtype=np.float32, order='F') @@ -267,6 +270,13 @@ def __init__(self, if point_viz: self.set_point_viz(point_viz) + def _populate_cloud_modes(self) -> None: + # index of available modes "pens" to get colors for point clouds + # start with all _cloud_modes and then check on every seen scan + # that is used for map/accum that we can still use available + # modes on such scans + self._available_modes: List[int] = list(range(len(self._cloud_modes))) + def set_point_viz(self, point_viz: PointViz): """Initialize point viz and cloud components.""" assert self._viz is None, "Can't set viz to ScansAccumulator again" @@ -702,6 +712,33 @@ def _draw_osd(self): else: self._osd.set_text("") + def _amend_view_modes(self, scan: client.LidarScan) -> None: + # Look for any new field that doesn't have a view mode associated with yet + # NOTE: we don't currently handle edge cases like removing a view mode if + # the field associated with it was dropped or deleted. + # Another situation that is not currently handled if there are more than one + # field that share the same name but could have different dimensions. In the + # current code the first field to appear will acquire the view mode. + # Will handle all these edge cases in "LidarScanViz Reforms" work. + amended_fields_length = len(self._amended_fields) # save the value + for field_name in scan.fields: + if field_name not in self._amended_fields: + # we encountered new field + field = scan.field(field_name) + # for now only consider 2D + 3D + if np.ndim(field) == 2: + self._cloud_modes.extend([ + SimpleMode(field_name, info=self.metadata[0])]) + self._amended_fields.append(field_name) + elif np.ndim(field) == 3 and field.shape[2] == 3: + self._cloud_modes.extend([ + RGBMode(field_name, info=self.metadata[0])]) + self._amended_fields.append(field_name) + # else any other shape we simply skip over + + if amended_fields_length != len(self._amended_fields): + self._populate_cloud_modes() + @no_type_check def update(self, scan: Union[client.LidarScan, @@ -716,6 +753,8 @@ def update(self, else: self._scan_num += 1 + self._amend_view_modes(scan) + logger.debug("update_scan: scan_num = %d", self._scan_num) # NOTE: copy() is very very important, without it the whole LidarScan @@ -911,7 +950,7 @@ def _update_map(self) -> None: sel_flag = ls.field(client.ChanField.RANGE) != 0 nzi, nzj = np.nonzero(sel_flag) nzc = np.random.choice(len(nzi), - int(self._map_select_ratio * len(nzi)), + min(int(self._map_select_ratio * len(nzi)), self._map_max_points), replace=False) row_sel, col_sel = nzi[nzc], nzj[nzc] xyz = self._xyzlut[0](ls.field(client.ChanField.RANGE)) diff --git a/python/src/ouster/sdk/viz/view_mode.py b/python/src/ouster/sdk/viz/view_mode.py index cba16957..e96b70db 100644 --- a/python/src/ouster/sdk/viz/view_mode.py +++ b/python/src/ouster/sdk/viz/view_mode.py @@ -72,7 +72,7 @@ class ImageCloudMode(ImageMode, CloudMode, Protocol): pass -def _second_chan_field(field: client.ChanField) -> Optional[client.ChanField]: +def _second_chan_field(field: str) -> Optional[str]: """Get the second return field name.""" # yapf: disable second_fields = dict({ @@ -93,8 +93,9 @@ class SimpleMode(ImageCloudMode): When AutoExposure is enabled its state updates only for return_num=0 but applies for both returns. """ + def __init__(self, - field: client.ChanField, + field: str, *, info: Optional[client.SensorInfo] = None, prefix: Optional[str] = "", @@ -104,7 +105,7 @@ def __init__(self, """ Args: info: sensor metadata used mainly for destaggering here - field: ChanField to process, second return is handled automatically + field: name of field to process, second return is handled automatically prefix: name prefix suffix: name suffix use_ae: if True, use AutoExposure for the field @@ -136,7 +137,9 @@ def _prepare_data(self, return None f = self._fields[return_num] - key_data = ls.field(f).astype(np.float32) + field = ls.field(f) + key_data = field if field.dtype == np.float32 else field.astype( + np.float32) if self._buc: self._buc(key_data) @@ -174,6 +177,74 @@ def enabled(self, ls: client.LidarScan, return_num: int = 0): if return_num < len(self._fields) else False) +class RGBMode(ImageCloudMode): + """RGB view mode + """ + + def __init__(self, + field: str, + *, + info: Optional[client.SensorInfo] = None) -> None: + """ + Args: + info: sensor metadata used mainly for destaggering here + field: channel field to process + """ + self._info = info + self._field = field + + @property + def name(self) -> str: + return self._field + + @property + def names(self) -> List[str]: + return [self._field] + + def _prepare_data(self, + ls: client.LidarScan, + return_num: int = 0) -> Optional[np.ndarray]: + + field = ls.field(self._field) + if np.ndim(field) != 3 and field.shape != 3: + raise TypeError(f"Unsupport field shape: {field.shape}") + if field.dtype == np.uint8: + key_data = (field / (2**8 - 1)).astype(np.float32) + elif field.dtype == np.uint16: + key_data = (field / (2**16 - 1)).astype(np.float32) + elif field.dtype == np.float32: + key_data = field + elif field.dtype == np.float64: + key_data = field.astype(np.float32) + else: + raise TypeError(f"Unsupport field type {field.dtype}") + + return key_data.clip(0, 1.0) + + def set_image(self, + img: Image, + ls: client.LidarScan, + return_num: int = 0) -> None: + if self._info is None: + raise ValueError( + f"VizMode[{self.name}] requires metadata to make a 2D image") + key_data = self._prepare_data(ls) + if key_data is not None: + img.set_image(client.destagger(self._info, key_data)) + + def set_cloud_color(self, + cloud: Cloud, + ls: client.LidarScan, + return_num: int = 0) -> None: + key_data = self._prepare_data(ls) + if key_data is not None: + cloud.set_key(key_data) + + def enabled(self, ls: client.LidarScan, return_num: int = 0): + field = ls.field(self._field) + return np.ndim(field) == 3 + + class ReflMode(SimpleMode, ImageCloudMode): """Prepares image/cloud data for REFLECTIVITY channel""" diff --git a/python/src/ouster/viz/__init__.py b/python/src/ouster/viz/__init__.py deleted file mode 100644 index 45ccb5bb..00000000 --- a/python/src/ouster/viz/__init__.py +++ /dev/null @@ -1,5 +0,0 @@ -# flake8: noqa (unused imports) - -print("warning: the ouster.viz module has been moved to ouster.sdk.viz, " - "please use the new path to avoid this warning.") -from ouster.sdk.viz import * diff --git a/python/tests/conftest.py b/python/tests/conftest.py index 692126fa..e1021ea5 100644 --- a/python/tests/conftest.py +++ b/python/tests/conftest.py @@ -10,16 +10,15 @@ from more_itertools import partition import pytest - from ouster.sdk import client, pcap pytest.register_assert_rewrite('ouster.sdk.client._digest') import ouster.sdk.client._digest as digest # noqa - _has_mapping = False try: from ouster.cli.plugins import cli_mapping # type: ignore # noqa: F401 # yes... it has to be in this order. + _has_mapping = False # NOTE: temporarily disabled due to CLI chaining -- Tim T. except ImportError: pass @@ -55,9 +54,10 @@ def pytest_collection_modifyitems(items, config) -> None: # test data # TODO: add OS-DOME-32/64 in 1024x10 mode pcap with digest -PCAPS_DATA_DIR = path.join(path.dirname(path.abspath(__file__)), "../../tests/pcaps") -METADATA_DATA_DIR = path.join(path.dirname(path.abspath(__file__)), "../../tests/metadata") -OSFS_DATA_DIR = path.join(path.dirname(path.abspath(__file__)), "../../tests/osfs") +CURRENT_DIR = path.dirname(path.abspath(__file__)) +PCAPS_DATA_DIR = path.join(CURRENT_DIR, "..", "..", "tests", "pcaps") +METADATA_DATA_DIR = path.join(CURRENT_DIR, "..", "..", "tests", "metadata") +OSFS_DATA_DIR = path.join(CURRENT_DIR, "..", "..", "tests", "osfs") TESTS = { 'legacy-2.0': 'OS-2-32-U0_v2.0.0_1024x10', diff --git a/python/tests/osf/test_osf_basics.py b/python/tests/osf/test_osf_basics.py index 3bfb8674..d768486a 100644 --- a/python/tests/osf/test_osf_basics.py +++ b/python/tests/osf/test_osf_basics.py @@ -26,23 +26,25 @@ def input_info(test_data_dir): def test_osf_scan_source_flags(input_osf_file): + """TWS 20240614: previously, the flags kwarg would affect whether + FLAGS/FLAGS2 were added to scans read from OSF. Now it has no effect.""" from ouster.sdk.client import ChanField - ss = open_source(str(input_osf_file), flags=False) - assert ss.fields.get(ChanField.FLAGS) is None - ss = open_source(str(input_osf_file)) - assert ss.fields.get(ChanField.FLAGS) is not None + scan_source = open_source(str(input_osf_file), flags=False) + assert ChanField.FLAGS not in scan_source.fields + scan_source = open_source(str(input_osf_file), flags=True) + assert ChanField.FLAGS not in scan_source.fields # Test that we can save a subset of scan fields and that it errors # if you try and save a scan missing fields in the metadata def test_writer_quick(tmp_path, input_info): file_name = tmp_path / "test.osf" - save_fields = {} - save_fields[client.ChanField.REFLECTIVITY] = np.uint32 - save_fields[client.ChanField.RANGE] = np.uint32 + save_fields = [ + client.ChanField.REFLECTIVITY, + client.ChanField.RANGE + ] - error_fields = {} - error_fields[client.ChanField.RANGE] = np.uint32 + error_fields = [client.FieldType(client.ChanField.RANGE, np.uint32)] with osf.Writer(str(file_name), input_info, save_fields) as writer: scan = client.LidarScan(128, 1024) scan.field(client.ChanField.REFLECTIVITY)[:] = 123 diff --git a/python/tests/test_batching.py b/python/tests/test_batching.py index edd2614e..9d57114f 100644 --- a/python/tests/test_batching.py +++ b/python/tests/test_batching.py @@ -5,20 +5,24 @@ """ from copy import deepcopy -from typing import Dict, Iterator +from typing import Iterator from more_itertools import take import numpy as np import pytest from ouster.sdk import client +from ouster.sdk.client import PacketFormat, PacketWriter, FieldType from ouster.sdk.client._client import ScanBatcher -from ouster.sdk.util import PacketFormat, ColHeader + + +client.ChanField.CUSTOM0 = 'custom0' +client.ChanField.CUSTOM8 = 'custom8' def _patch_frame_id(packet: client.LidarPacket, fid: int) -> None: """Rewrite the frame id of a non-legacy format lidar packet.""" - packet._data[2:4] = memoryview(fid.to_bytes(2, byteorder='little')) + packet.buf[2:4] = memoryview(fid.to_bytes(2, byteorder='little')) def _simple_scans(source: client.PacketSource) -> Iterator[client.LidarScan]: @@ -31,7 +35,7 @@ def _simple_scans(source: client.PacketSource) -> Iterator[client.LidarScan]: info.format.udp_profile_lidar) for p in source: - if batch(p._data, ls): + if batch(p.buf, ls): yield deepcopy(ls) @@ -49,11 +53,11 @@ def gen_packets(): for p in plist: if isinstance(p, client.LidarPacket): if next_ts < 0: - next_ts = p.capture_timestamp + next_ts = p.host_timestamp _patch_frame_id(p, frame_id) - p.capture_timestamp = next_ts + p.host_timestamp = int(next_ts) yield p - next_ts += dt + next_ts += dt * 1e9 frame_id += 1 return client.Packets(gen_packets(), packets.metadata) @@ -111,14 +115,14 @@ def non_zero_scan(scan: client.LidarScan): } drop_columns_num = sum([len(set(v)) for v in drop_columns.values()]) - pformat = PacketFormat.from_metadata(info) - # number of scans we want to get from packets num_scans = 3 # drop some packets, and +1 to ensure the "cut" of the scan by batcher packets = list(take(packets_per_frame * num_scans + 1, lidar_stream)) + writer = PacketWriter.from_info(info) + # parse the packets into a scan def scans(): # reusing the same lidar scan object over and over @@ -129,11 +133,10 @@ def scans(): if packet_ind in drop_columns: # invalidating columns # column is invalid if first bit of status != 1 - pformat.header( - p._data, - ColHeader.STATUS)[drop_columns[packet_ind]] = 0 + for col in drop_columns[packet_ind]: + writer.set_col_status(p, col, 0) - if batch(p._data, client.packet_ts(p), ls): + if batch(p.buf, client.packet_ts(p), ls): # when batch returns True it means # that the outstanding and missed packets # in the scan should be zeroed properly @@ -177,14 +180,15 @@ def test_batch_custom_fields(lidar_stream: client.PacketSource) -> None: info.format.columns_per_packet) batch = ScanBatcher(lidar_stream.metadata) + pf = PacketFormat(info) # create LidarScan with only 2 fields - fields: Dict[client.ChanField, client.FieldDType] = { - client.ChanField.RANGE: np.uint32, - client.ChanField.SIGNAL: np.uint16, - client.ChanField.CUSTOM0: np.uint8, - client.ChanField.CUSTOM8: np.uint16 - } + fields = [ + FieldType(client.ChanField.RANGE, np.uint32), + FieldType(client.ChanField.SIGNAL, np.uint16), + FieldType(client.ChanField.CUSTOM0, np.uint8), + FieldType(client.ChanField.CUSTOM8, np.uint16) + ] ls = client.LidarScan(info.format.pixels_per_column, info.format.columns_per_frame, fields) @@ -198,10 +202,10 @@ def test_batch_custom_fields(lidar_stream: client.PacketSource) -> None: # do batching into ls with a fields subset for p in take(packets_per_frame, lidar_stream): - batch(p._data, ls) + batch(p.buf, ls) if isinstance(p, client.LidarPacket): - assert p.shot_limiting == ls.shot_limiting() - assert p.thermal_shutdown == ls.thermal_shutdown() + assert pf.shot_limiting(p.buf) == ls.shot_limiting() + assert pf.thermal_shutdown(p.buf) == ls.thermal_shutdown() # it should contain the same num fields as we've added assert len(list(ls.fields)) == len(fields) @@ -228,22 +232,9 @@ def test_incompatible_profile(lidar_stream: client.PacketSource) -> None: batch = ScanBatcher(lidar_stream.metadata) - ls = client.LidarScan(info.format.pixels_per_column, - info.format.columns_per_frame, - client.UDPProfileLidar.PROFILE_LIDAR_RNG19_RFL8_SIG16_NIR16_DUAL) - assert ls.packet_timestamp.shape == (packets_per_frame,) - - # Try to decode a legacy packet into a dual returns scan - # TODO change exception thrown on the cpp side - with pytest.raises(IndexError): - for p in take(packets_per_frame, lidar_stream): - batch(p._data, ls) - - batch = ScanBatcher(lidar_stream.metadata) - - fields = { - client.ChanField.RANGE: np.uint8, - } + fields = [ + FieldType(client.ChanField.RANGE, np.uint8) + ] ls = client.LidarScan(info.format.pixels_per_column, info.format.columns_per_frame, fields) @@ -251,7 +242,7 @@ def test_incompatible_profile(lidar_stream: client.PacketSource) -> None: # Test for decoding scans to a bad dest buffer type with pytest.raises(ValueError): for p in take(packets_per_frame, lidar_stream): - batch(p._data, ls) + batch(p.buf, ls) @pytest.fixture diff --git a/python/tests/test_cli.py b/python/tests/test_cli.py index df124b12..0cc62ab4 100644 --- a/python/tests/test_cli.py +++ b/python/tests/test_cli.py @@ -3,6 +3,7 @@ from glob import glob from pathlib import Path import pytest +import subprocess import sys import json import tempfile @@ -219,7 +220,7 @@ def test_source_good_command(runner): # we don't expect this to succeed because there is no such sensor # so we should see exit code 1 result = runner.invoke(core.cli, ['source', '127.0.0.1', 'config']) - assert "Error: CurlClient::execute_get failed" in result.output + assert "Error: CurlClient::execute_request failed" in result.output assert result.exit_code == 1 @@ -245,7 +246,7 @@ def test_source_config(runner): # we don't expect this to succeed because there is no such sensor # so we should see exit code 1 result = runner.invoke(core.cli, ['source', '127.0.0.1', 'config']) - assert "Error: CurlClient::execute_get failed" in result.output + assert "Error: CurlClient::execute_request failed" in result.output assert result.exit_code == 1 @@ -253,7 +254,7 @@ def test_source_metadata(): """It should attempt to get metadata (and fail when there is no sensor)""" runner = CliRunner() result = runner.invoke(core.cli, ['source', '127.0.0.1', 'metadata']) - assert "Error: CurlClient::execute_get failed" in result.output + assert "Error: CurlClient::execute_request failed" in result.output assert result.exit_code == 1 @@ -299,27 +300,33 @@ def test_source_pcap_slice_help_2(test_pcap_file, runner): assert "Error: Invalid value for 'INDICES'" in result.output -def source_pcap_slice_impl(test_pcap_file, runner, command, packets): +def source_pcap_slice_impl(test_pcap_file, runner, command, packets, should_fail=False): try: + pcap_filename = None with tempfile.NamedTemporaryFile(delete=False) as f: pass result = runner.invoke(core.cli, ['source', test_pcap_file, 'slice', command, 'save', '-p', f.name, ".pcap"]) # FIXME! Written file paths should be logged in output. # assert f'Writing: {f.name}' in result.output + print(result.output) + if should_fail: + assert result.exit_code != 0 + return assert result.exit_code == 0 pcaps_generated = glob(f'{f.name}_*.pcap') assert len(pcaps_generated) == 1 pcap_filename = pcaps_generated[0] result2 = runner.invoke(core.cli, ['source', pcap_filename, 'info']) - assert result2.exit_code == 0 print(result2.output) + assert result2.exit_code == 0 assert "Packets read: " + packets in result2.output finally: - json_filename = pcap_filename[:-4] + 'json' os.unlink(f'{f.name}') - os.unlink(pcap_filename) - os.unlink(json_filename) + if pcap_filename is not None: + json_filename = pcap_filename[:-4] + 'json' + os.unlink(pcap_filename) + os.unlink(json_filename) def test_source_pcap_slice(test_pcap_file, runner): @@ -332,6 +339,17 @@ def test_source_pcap_slice(test_pcap_file, runner): source_pcap_slice_impl(test_pcap_file, runner, "2:", "0") source_pcap_slice_impl(test_pcap_file, runner, "1::", "0") source_pcap_slice_impl(test_pcap_file, runner, "1::1", "0") + source_pcap_slice_impl(test_pcap_file, runner, "0s::1", "64") + source_pcap_slice_impl(test_pcap_file, runner, "0min:1h:1", "64") + source_pcap_slice_impl(test_pcap_file, runner, "10ms:", "0") + source_pcap_slice_impl(test_pcap_file, runner, "0s::2", "64") + source_pcap_slice_impl(test_pcap_file, runner, "1s::1", "0") + source_pcap_slice_impl(test_pcap_file, runner, "1s::", "0") + + # add some bad cases that should error + fail_cases = [":1s:1", "1s:1:", "'-1s::'", "0:1s", "1.5:3.5", ".s:0.s", "0::0", ".:.:3", "::-1", "3:1"] + for case in fail_cases: + source_pcap_slice_impl(test_pcap_file, runner, case, "0", True) def test_source_pcap_save_no_filename(test_pcap_file, runner, tmp_path): @@ -426,6 +444,17 @@ def test_discover(runner): assert result.exit_code == 0 +def test_source_stats(test_pcap_file, runner): + """ouster-cli source ... stats should display correct stats.""" + result = subprocess.run(["ouster-cli", "source", test_pcap_file, "stats"], + capture_output=True, check=True, text=True) + assert "Count: 1" in result.stdout + assert "First Time: 1650408693" in result.stdout + assert "Sizes: 1024x128" in result.stdout + assert "Incomplete Scans" in result.stdout + assert result.returncode == 0 + + def test_source_osf(runner, has_mapping) -> None: """It should list the correct commands in the help depending on source type.""" diff --git a/python/tests/test_cli_util.py b/python/tests/test_cli_util.py index 424f319d..7e44ad81 100644 --- a/python/tests/test_cli_util.py +++ b/python/tests/test_cli_util.py @@ -1,11 +1,7 @@ import os -import json import tempfile import pytest -from ouster.cli import core import ouster.cli.core.util -from click.testing import CliRunner -from tests.conftest import METADATA_DATA_DIR def test_md5file(): @@ -21,24 +17,3 @@ def test_md5file(): def test_md5file_doesntexist(): with pytest.raises(FileNotFoundError): ouster.cli.core.util.md5file('doesntexist') - - -def test_convert_metadata_to_legacy(): - runner = CliRunner() - result = runner.invoke(core.cli, ['util', 'convert-metadata-to-legacy']) - assert result.exit_code == 2 - assert "Error: Missing argument 'META'." in result.output - - -def test_convert_metadata_to_legacy_2(): - try: - f = tempfile.NamedTemporaryFile(delete=False) - f.close() - runner = CliRunner() - test_metadata_file = os.path.join(METADATA_DATA_DIR, '3_0_1_os-122246000293-128.json') - legacy_test_metadata_file = os.path.join(METADATA_DATA_DIR, '3_0_1_os-122246000293-128_legacy.json') - result = runner.invoke(core.cli, ['util', 'convert-metadata-to-legacy', test_metadata_file]) - assert result.exit_code == 0 - assert json.loads(result.output) == json.load(open(legacy_test_metadata_file)) - finally: - os.unlink(f.name) diff --git a/python/tests/test_core.py b/python/tests/test_core.py index 26c7f6f5..ac062f0e 100644 --- a/python/tests/test_core.py +++ b/python/tests/test_core.py @@ -10,8 +10,9 @@ import pytest from ouster.sdk import client -from ouster.sdk.client import ChanField, _LidarPacket, _ImuPacket +from ouster.sdk.client import ChanField, LidarPacket, ImuPacket, PacketFormat, ColHeader, FieldType from ouster.sdk.client.core import ClientTimeout +from ouster.sdk.client._client import Version pytest.register_assert_rewrite('ouster.sdk.client._digest') import ouster.sdk.client._digest as digest # noqa @@ -68,24 +69,24 @@ def test_sensor_packet(default_meta: client.SensorInfo) -> None: metadata=default_meta, timeout=5.0, _flush_before_read=False)) as source: - + pf = PacketFormat(source.metadata) sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) data = np.random.randint(255, - size=source._pf.lidar_packet_size, + size=pf.lidar_packet_size, dtype=np.uint8) sock.sendto(data.tobytes(), ("localhost", source.lidar_port)) packet = next(iter(source)) - assert (packet._data == data).all() - assert isinstance(packet, _LidarPacket) + assert (packet.buf == data).all() + assert isinstance(packet, LidarPacket) data = np.random.randint(255, - size=source._pf.imu_packet_size, + size=pf.imu_packet_size, dtype=np.uint8) sock.sendto(data.tobytes(), ("localhost", source.imu_port)) packet = next(iter(source)) - assert (packet._data == data).all() - assert isinstance(packet, _ImuPacket) + assert (packet.buf == data).all() + assert isinstance(packet, ImuPacket) def test_sensor_flush(default_meta: client.SensorInfo) -> None: @@ -96,9 +97,10 @@ def test_sensor_flush(default_meta: client.SensorInfo) -> None: metadata=default_meta, timeout=1.0, _flush_before_read=False)) as source: + pf = client.PacketFormat(source.metadata) total_packets_sent = 5 for i in range(total_packets_sent): - data = np.zeros((source._pf.lidar_packet_size), dtype=np.uint8) + data = np.zeros((pf.lidar_packet_size), dtype=np.uint8) data[:] = i sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) sock.sendto(data.tobytes(), ("localhost", source.lidar_port)) @@ -106,7 +108,7 @@ def test_sensor_flush(default_meta: client.SensorInfo) -> None: source.flush(flushed_packets) for i in range(flushed_packets, total_packets_sent): packet = next(iter(source)) - assert (packet._data == i).all() + assert (packet.buf == i).all() def test_sensor_packet_bad_size(default_meta: client.SensorInfo) -> None: @@ -119,7 +121,7 @@ def test_sensor_packet_bad_size(default_meta: client.SensorInfo) -> None: timeout=1.0, _flush_before_read=False)) as source: sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) - + pf = client.PacketFormat(source.metadata) # send packet too small sock.sendto(b"hello", ("localhost", source.lidar_port)) with pytest.raises(client.ClientTimeout): @@ -127,7 +129,7 @@ def test_sensor_packet_bad_size(default_meta: client.SensorInfo) -> None: # send packet too big data = np.random.randint(255, - size=source._pf.lidar_packet_size + 10, + size=pf.lidar_packet_size + 10, dtype=np.uint8) sock.sendto(data.tobytes(), ("localhost", source.lidar_port)) with pytest.raises(client.ClientTimeout): @@ -146,10 +148,11 @@ def test_sensor_overflow(default_meta: client.SensorInfo) -> None: timeout=1.0, _overflow_err=True, _flush_before_read=False)) as source: + pf = client.PacketFormat(source.metadata) total_packets_sent = 20 for i in range(total_packets_sent): data = np.random.randint(255, - size=source._pf.lidar_packet_size, + size=pf.lidar_packet_size, dtype=np.uint8) sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) sock.sendto(data.tobytes(), ("localhost", source.lidar_port)) @@ -208,34 +211,35 @@ def test_scans_first_packet(packet: client.LidarPacket, scans = iter(client.Scans(packets)) scan = next(scans) - h = packet._pf.pixels_per_column - w = packet._pf.columns_per_packet + pf = PacketFormat(packets.metadata) + h = pf.pixels_per_column + w = pf.columns_per_packet is_low_data_rate_profile = (packets.metadata.format.udp_profile_lidar == client.UDPProfileLidar.PROFILE_LIDAR_RNG15_RFL8_NIR8) if not is_low_data_rate_profile: # low data rate profile RANGE is scaled up - assert np.array_equal(packet.field(ChanField.RANGE), + assert np.array_equal(pf.packet_field(ChanField.RANGE, packet.buf), scan.field(ChanField.RANGE)[:h, :w]) - assert np.array_equal(packet.field(ChanField.REFLECTIVITY), + assert np.array_equal(pf.packet_field(ChanField.REFLECTIVITY, packet.buf), scan.field(ChanField.REFLECTIVITY)[:h, :w]) if is_low_data_rate_profile: # low data rate profile has no SIGNAL assert ChanField.SIGNAL not in set(scan.fields) else: - assert np.array_equal(packet.field(ChanField.SIGNAL), + assert np.array_equal(pf.packet_field(ChanField.SIGNAL, packet.buf), scan.field(ChanField.SIGNAL)[:h, :w]) if not is_low_data_rate_profile: # low data rate profile NEAR_IR is scaled up - assert np.array_equal(packet.field(ChanField.NEAR_IR), + assert np.array_equal(pf.packet_field(ChanField.NEAR_IR, packet.buf), scan.field(ChanField.NEAR_IR)[:h, :w]) - assert packet.frame_id == scan.frame_id + assert pf.frame_id(packet.buf) == scan.frame_id - assert np.array_equal(packet.timestamp, scan.timestamp[:w]) + assert np.array_equal(pf.packet_header(ColHeader.TIMESTAMP, packet.buf), scan.timestamp[:w]) - assert np.array_equal(packet.status, scan.status[:w]) + assert np.array_equal(pf.packet_header(ColHeader.STATUS, packet.buf), scan.status[:w]) @pytest.mark.parametrize('test_key', ['legacy-2.0']) @@ -299,49 +303,40 @@ def test_scans_dual(packets: client.PacketSource) -> None: @pytest.mark.parametrize('test_key', ['dual-2.2']) def test_scans_empty_fields(packets: client.PacketSource) -> None: """Test batching scans with no fields.""" - scan = next(iter(client.Scans(packets, fields={}))) + scan = next(iter(client.Scans(packets, fields=[]))) assert set(scan.fields) == set() @pytest.mark.parametrize('test_key', ['dual-2.2']) def test_scans_one_field(packets: client.PacketSource) -> None: """Test batching scans with a single field.""" - fields = {ChanField.FLAGS: np.uint8} + fields = [FieldType(ChanField.FLAGS, np.uint8)] scan = next(iter(client.Scans(packets, fields=fields))) assert set(scan.fields) == {ChanField.FLAGS} assert scan.field(ChanField.FLAGS).dtype == np.uint8 - with pytest.raises(ValueError): + with pytest.raises(IndexError): scan.field(ChanField.RANGE) @pytest.mark.parametrize('test_key', ['dual-2.2']) def test_scans_bad_type(packets: client.PacketSource) -> None: """Test batching scans with a type too small for source data.""" - fields = {ChanField.RANGE: np.uint16} + fields = [FieldType(ChanField.RANGE, np.uint16)] with pytest.raises(ValueError): next(iter(client.Scans(packets, fields=fields))) -@pytest.mark.parametrize('test_key', ['legacy-2.0', 'legacy-2.1']) -def test_scans_bad_field(packets: client.PacketSource) -> None: - """Test batching scans with a field not present on source packets.""" - fields = {ChanField.RANGE2: np.uint32} - - with pytest.raises(IndexError): - next(iter(client.Scans(packets, fields=fields))) - - @pytest.mark.parametrize('test_key', ['legacy-2.0', 'legacy-2.1']) def test_scans_raw(packets: client.PacketSource) -> None: """Smoke test reading raw channel field data.""" - fields = { - ChanField.RAW32_WORD1: np.uint32, - ChanField.RAW32_WORD2: np.uint32, - ChanField.RAW32_WORD3: np.uint32 - } + fields = [ + FieldType(ChanField.RAW32_WORD1, np.uint32), + FieldType(ChanField.RAW32_WORD2, np.uint32), + FieldType(ChanField.RAW32_WORD3, np.uint32) + ] scans = client.Scans(packets, fields=fields) @@ -354,3 +349,69 @@ def test_scans_raw(packets: client.PacketSource) -> None: # just check that raw fields are populated? for f in ls[0].fields: assert np.count_nonzero(ls[0].field(f)) != 0 + + +def test_version_compare() -> None: + v1 = Version.from_string("1.2.3") + v2 = Version.from_string("1.3.3") + v3 = Version.from_string("v1.2.3") + + assert v1 == v3 + assert v1 != v2 + + +def test_version_parse() -> None: + v = Version.from_string("v1.2.3") + assert v.stage == "" + assert v.machine == "" + assert v.major == 1 + assert v.minor == 2 + assert v.patch == 3 + + v2 = Version.from_string("1.2.3") + assert v2.major == 1 + assert v2.minor == 2 + assert v2.patch == 3 + + v = Version.from_string("ousteros-prod-bootes-v1.2.3-rc1+123456") + assert v.major == 1 + assert v.minor == 2 + assert v.patch == 3 + assert v.stage == "prod" + assert v.machine == "bootes" + assert v.prerelease == "rc1" + assert v.build == "123456" + + v = Version.from_string("ousteros-prod-bootes-v1.2.3+123456") + assert v.major == 1 + assert v.minor == 2 + assert v.patch == 3 + assert v.stage == "prod" + assert v.machine == "bootes" + assert v.prerelease == "" + assert v.build == "123456" + + v = Version.from_string("ousteros-image-prod-aries-v2.0.0-rc.2+20201023140416.staging") + assert v.major == 2 + assert v.minor == 0 + assert v.patch == 0 + assert v.stage == "prod" + assert v.machine == "aries" + assert v.prerelease == "rc.2" + assert v.build == "20201023140416.staging" + + # Invalid version strings should parse as all zeros + v = Version.from_string("2.3") + assert v.major == 0 + assert v.minor == 0 + assert v.patch == 0 + + v = Version.from_string("a.2.3") + assert v.major == 0 + assert v.minor == 0 + assert v.patch == 0 + + v = Version.from_string("3") + assert v.major == 0 + assert v.minor == 0 + assert v.patch == 0 diff --git a/python/tests/test_data.py b/python/tests/test_data.py index 6dd58ae8..ce4ed71f 100644 --- a/python/tests/test_data.py +++ b/python/tests/test_data.py @@ -6,139 +6,117 @@ Checks that the output of parsing hasn't changed unexpectedly. """ -import os -import json from copy import deepcopy import numpy as np import pytest from ouster.sdk import client -from ouster.sdk.client import _client -from ouster.sdk.pcap import _pcap - -from tests.conftest import PCAPS_DATA_DIR +from ouster.sdk.client._client import scan_to_packets +from ouster.sdk.client import PacketValidationFailure, PacketFormat, PacketWriter, FieldType def test_make_packets(meta: client.SensorInfo) -> None: - pf = _client.PacketFormat.from_info(meta) - - client.ImuPacket(bytes(pf.imu_packet_size), meta) - client.ImuPacket(bytearray(pf.imu_packet_size), meta) - - with pytest.raises(ValueError): - client.ImuPacket(bytes(), meta) + pf = PacketFormat.from_info(meta) - with pytest.raises(ValueError): - client.ImuPacket(bytes(pf.imu_packet_size - 1), meta) + p1 = client.ImuPacket() + p2 = client.ImuPacket(pf.imu_packet_size) - client.LidarPacket(bytes(pf.lidar_packet_size), meta) - client.LidarPacket(bytearray(pf.lidar_packet_size), meta) + p3 = client.LidarPacket() + p4 = client.LidarPacket(pf.lidar_packet_size) - with pytest.raises(ValueError): - client.LidarPacket(bytes(), meta) + assert p1.validate(meta, pf) == PacketValidationFailure.PACKET_SIZE + assert p2.validate(meta, pf) == PacketValidationFailure.NONE - with pytest.raises(ValueError): - client.LidarPacket(bytes(pf.lidar_packet_size - 1), meta) + assert p3.validate(meta, pf) == PacketValidationFailure.PACKET_SIZE + assert p4.validate(meta, pf) == PacketValidationFailure.NONE def test_imu_packet(meta: client.SensorInfo) -> None: - pf = _client.PacketFormat.from_info(meta) - - p = client.ImuPacket(bytes(pf.imu_packet_size), meta) + pf = PacketFormat.from_info(meta) - assert p.sys_ts == 0 - assert p.accel_ts == 0 - assert p.gyro_ts == 0 - assert np.array_equal(p.accel, np.array([0.0, 0.0, 0.0])) - assert np.array_equal(p.angular_vel, np.array([0.0, 0.0, 0.0])) + p = client.ImuPacket(pf.imu_packet_size) - with pytest.raises(AttributeError): - p.accel_ts = 0 # type: ignore + assert pf.imu_sys_ts(p.buf) == 0 + assert pf.imu_accel_ts(p.buf) == 0 + assert pf.imu_gyro_ts(p.buf) == 0 + assert pf.imu_av_x(p.buf) == 0.0 + assert pf.imu_av_y(p.buf) == 0.0 + assert pf.imu_av_z(p.buf) == 0.0 + assert pf.imu_la_x(p.buf) == 0.0 + assert pf.imu_la_y(p.buf) == 0.0 + assert pf.imu_la_z(p.buf) == 0.0 def test_lidar_packet(meta: client.SensorInfo) -> None: """Test reading and writing values from empty packets.""" - pf = _client.PacketFormat.from_info(meta) - p = client.LidarPacket(bytes(pf.lidar_packet_size), meta) + pf = PacketFormat(meta) + p = client.LidarPacket(pf.lidar_packet_size) w = pf.columns_per_packet h = pf.pixels_per_column scan_has_signal = (meta.format.udp_profile_lidar != client.UDPProfileLidar.PROFILE_LIDAR_RNG15_RFL8_NIR8) - assert len( - client.ChanField.__members__) == 29, "Don't forget to update tests!" - assert np.array_equal(p.field(client.ChanField.RANGE), np.zeros((h, w))) - assert np.array_equal(p.field(client.ChanField.REFLECTIVITY), + assert np.array_equal(pf.packet_field(client.ChanField.RANGE, p.buf), np.zeros((h, w))) + assert np.array_equal(pf.packet_field(client.ChanField.REFLECTIVITY, p.buf), np.zeros((h, w))) - assert np.array_equal(p.field(client.ChanField.NEAR_IR), np.zeros((h, w))) + assert np.array_equal(pf.packet_field(client.ChanField.NEAR_IR, p.buf), np.zeros((h, w))) if scan_has_signal: - assert np.array_equal(p.field(client.ChanField.SIGNAL), np.zeros( + assert np.array_equal(pf.packet_field(client.ChanField.SIGNAL, p.buf), np.zeros( (h, w))) assert len( client.ColHeader.__members__) == 5, "Don't forget to update tests!" - assert np.array_equal(p.timestamp, np.zeros(w)) - assert np.array_equal(p.measurement_id, np.zeros(w)) - assert np.array_equal(p.status, np.zeros(w)) + assert np.array_equal(pf.packet_header(client.ColHeader.TIMESTAMP, p.buf), np.zeros(w)) + assert np.array_equal(pf.packet_header(client.ColHeader.MEASUREMENT_ID, p.buf), np.zeros(w)) + assert np.array_equal(pf.packet_header(client.ColHeader.STATUS, p.buf), np.zeros(w)) - assert p.frame_id == 0 - - # should not be able to modify packet data - with pytest.raises(ValueError): - p.field(client.ChanField.REFLECTIVITY)[0] = 1 - - with pytest.raises(ValueError): - p.measurement_id[0] = 1 - - with pytest.raises(ValueError): - p.status[:] = 1 - - with pytest.raises(AttributeError): - p.frame_id = 1 # type: ignore + assert pf.frame_id(p.buf) == 0 @pytest.mark.parametrize('test_key', ['legacy-2.0']) -def test_read_legacy_packet(packet: client.LidarPacket) -> None: +def test_read_legacy_packet(packet: client.LidarPacket, packets: client.PacketSource) -> None: """Read some arbitrary values from a packet and check header invariants.""" - assert packet.field(client.ChanField.RANGE)[-1, 0] == 12099 - assert packet.field(client.ChanField.REFLECTIVITY)[-1, 0] == 1017 - assert packet.field(client.ChanField.SIGNAL)[-1, 0] == 6 - assert packet.field(client.ChanField.NEAR_IR)[-1, 0] == 13 - - assert np.all(np.diff(packet.timestamp) > 0) - assert np.all(np.diff(packet.measurement_id) == 1) - assert packet.packet_type == 0 - assert packet.frame_id == 5424 - assert packet.init_id == 0 - assert packet.prod_sn == 0 - assert packet.shot_limiting == 0 - assert packet.thermal_shutdown == 0 + pf = client.PacketFormat(packets.metadata) + assert pf.packet_field(client.ChanField.RANGE, packet.buf)[-1, 0] == 12099 + assert pf.packet_field(client.ChanField.REFLECTIVITY, packet.buf)[-1, 0] == 1017 + assert pf.packet_field(client.ChanField.SIGNAL, packet.buf)[-1, 0] == 6 + assert pf.packet_field(client.ChanField.NEAR_IR, packet.buf)[-1, 0] == 13 + + assert np.all(np.diff(pf.packet_header(client.ColHeader.TIMESTAMP, packet.buf)) > 0) + assert np.all(np.diff(pf.packet_header(client.ColHeader.MEASUREMENT_ID, packet.buf)) == 1) + assert pf.packet_type(packet.buf) == 0 + assert pf.frame_id(packet.buf) == 5424 + assert pf.init_id(packet.buf) == 0 + assert pf.prod_sn(packet.buf) == 0 + assert pf.shot_limiting(packet.buf) == 0 + assert pf.thermal_shutdown(packet.buf) == 0 # in 1024xN mode, the angle between measurements is exactly 88 encoder ticks - assert np.all(packet.status == 0xffffffff) + assert np.all(pf.packet_header(client.ColHeader.STATUS, packet.buf) == 0xffffffff) @pytest.mark.parametrize('test_key', ['single-2.3']) -def test_read_single_return_packet(packet: client.LidarPacket) -> None: +def test_read_single_return_packet(packet: client.LidarPacket, packets: client.PacketSource) -> None: """Read some arbitrary values from packet and check header invariants.""" - assert packet.field(client.ChanField.RANGE)[-1, 0] == 11610 - assert packet.field(client.ChanField.REFLECTIVITY)[-1, 0] == 11 - assert packet.field(client.ChanField.SIGNAL)[-1, 0] == 34 - assert packet.field(client.ChanField.NEAR_IR)[-1, 0] == 393 - - assert np.all(np.diff(packet.timestamp) > 0) - assert np.all(np.diff(packet.measurement_id) == 1) - assert packet.packet_type == 1 - assert packet.frame_id == 1259 - assert packet.init_id == 5431293 - assert packet.prod_sn == 992210000957 - assert packet.shot_limiting == 0 - assert packet.thermal_shutdown == 0 + pf = client.PacketFormat(packets.metadata) + assert pf.packet_field(client.ChanField.RANGE, packet.buf)[-1, 0] == 11610 + assert pf.packet_field(client.ChanField.REFLECTIVITY, packet.buf)[-1, 0] == 11 + assert pf.packet_field(client.ChanField.SIGNAL, packet.buf)[-1, 0] == 34 + assert pf.packet_field(client.ChanField.NEAR_IR, packet.buf)[-1, 0] == 393 + + assert np.all(np.diff(pf.packet_header(client.ColHeader.TIMESTAMP, packet.buf)) > 0) + assert np.all(np.diff(pf.packet_header(client.ColHeader.MEASUREMENT_ID, packet.buf)) == 1) + assert pf.packet_type(packet.buf) == 1 + assert pf.frame_id(packet.buf) == 1259 + assert pf.init_id(packet.buf) == 5431293 + assert pf.prod_sn(packet.buf) == 992210000957 + assert pf.shot_limiting(packet.buf) == 0 + assert pf.thermal_shutdown(packet.buf) == 0 # Changes from LEGACY - assert np.all(packet.status == 0x01) + assert np.all(pf.packet_header(client.ColHeader.STATUS, packet.buf) == 0x01) def test_lidarscan_init() -> None: @@ -358,32 +336,27 @@ def test_scan_single_return() -> None: def test_scan_empty() -> None: """Sanity check scan with no fields.""" - ls = client.LidarScan(32, 1024, {}) - - assert set(ls.fields) == set() - - for f in client.ChanField.values: - with pytest.raises(ValueError): - ls.field(f) + ls = client.LidarScan(32, 1024, []) + assert ls.fields == [] def test_scan_custom() -> None: """Sanity check scan with a custom set of fields.""" ls = client.LidarScan( - 32, 1024, { - client.ChanField.SIGNAL: np.uint16, - client.ChanField.FLAGS: np.uint8, - client.ChanField.CUSTOM0: np.uint32 - }) + 32, 1024, [ + FieldType(client.ChanField.SIGNAL, np.uint16), + FieldType(client.ChanField.FLAGS, np.uint8), + FieldType("custom0", np.uint32) + ]) assert set(ls.fields) == { client.ChanField.SIGNAL, client.ChanField.FLAGS, - client.ChanField.CUSTOM0 + "custom0" } assert ls.field(client.ChanField.SIGNAL).dtype == np.uint16 - assert ls.field(client.ChanField.CUSTOM0).dtype == np.uint32 + assert ls.field("custom0").dtype == np.uint32 - with pytest.raises(ValueError): + with pytest.raises(IndexError): ls.field(client.ChanField.RANGE) @@ -395,9 +368,9 @@ def test_scan_eq_fields() -> None: ls2 = client.LidarScan( 32, 1024, client.UDPProfileLidar.PROFILE_LIDAR_RNG19_RFL8_SIG16_NIR16_DUAL) - ls3 = client.LidarScan(32, 1024, {client.ChanField.SIGNAL: np.uint32}) - ls4 = client.LidarScan(32, 1024, {client.ChanField.SIGNAL: np.uint16}) - ls5 = client.LidarScan(32, 1024, {}) + ls3 = client.LidarScan(32, 1024, [FieldType(client.ChanField.SIGNAL, np.uint32)]) + ls4 = client.LidarScan(32, 1024, [FieldType(client.ChanField.SIGNAL, np.uint16)]) + ls5 = client.LidarScan(32, 1024, []) assert ls0 == ls1 assert not (ls0 != ls1) # should be implemented using __eq__ @@ -472,20 +445,20 @@ def test_scan_copy_eq() -> None: def test_scan_eq_with_custom_fields() -> None: """Test equality with custom fields.""" - ls0 = client.LidarScan(32, 512, { - client.ChanField.CUSTOM0: np.uint32, - client.ChanField.CUSTOM4: np.uint8 - }) + ls0 = client.LidarScan(32, 512, [ + FieldType("custom0", np.uint32), + FieldType("custom4", np.uint8) + ]) ls1 = deepcopy(ls0) - ls0.field(client.ChanField.CUSTOM0)[:] = 100 + ls0.field("custom0")[:] = 100 ls2 = deepcopy(ls0) assert np.count_nonzero( - ls2.field(client.ChanField.CUSTOM0) == 100) == ls0.h * ls0.w - assert np.count_nonzero(ls2.field(client.ChanField.CUSTOM4) == 100) == 0 + ls2.field("custom0") == 100) == ls0.h * ls0.w + assert np.count_nonzero(ls2.field("custom4") == 100) == 0 assert ls1 is not ls0 assert ls1 != ls0 @@ -494,82 +467,82 @@ def test_scan_eq_with_custom_fields() -> None: def test_scan_copy_extension() -> None: """ Verify we can clone a scan and null pad missing desired fields """ - ls0 = client.LidarScan(32, 512, { - client.ChanField.CUSTOM4: np.uint8 - }) + ls0 = client.LidarScan(32, 512, [ + FieldType("custom4", np.uint8) + ]) - ls0.field(client.ChanField.CUSTOM4)[:] = 123 + ls0.field("custom4")[:] = 123 - ls1 = client.LidarScan(ls0, { - client.ChanField.CUSTOM0: np.uint32, - client.ChanField.CUSTOM4: np.uint8 - }) + ls1 = client.LidarScan(ls0, [ + FieldType("custom0", np.uint32), + FieldType("custom4", np.uint8) + ]) - assert len(list(ls1.fields)) == 2 - assert np.count_nonzero(ls1.field(client.ChanField.CUSTOM0)[0, 0]) == 0 + assert len(list(ls1.fields)) == 2, ls1.fields + assert np.count_nonzero(ls1.field("custom0")[0, 0]) == 0 assert np.count_nonzero( - ls1.field(client.ChanField.CUSTOM4) == 123) == ls1.h * ls1.w + ls1.field("custom4") == 123) == ls1.h * ls1.w def test_scan_copy_retraction() -> None: """ Verify we can clone a scan and remove undesired fields """ - ls0 = client.LidarScan(32, 512, { - client.ChanField.CUSTOM0: np.uint32, - client.ChanField.CUSTOM4: np.uint8 - }) + ls0 = client.LidarScan(32, 512, [ + FieldType("custom0", np.uint32), + FieldType("custom4", np.uint8) + ]) - ls0.field(client.ChanField.CUSTOM0)[:] = 100 - ls0.field(client.ChanField.CUSTOM4)[:] = 123 + ls0.field("custom0")[:] = 100 + ls0.field("custom4")[:] = 123 - ls1 = client.LidarScan(ls0, { - client.ChanField.CUSTOM0: np.uint32, - }) + ls1 = client.LidarScan(ls0, [ + FieldType("custom0", np.uint32), + ]) assert ls0.h == ls1.h assert ls0.w == ls1.w assert len(list(ls1.fields)) == 1 assert np.count_nonzero( - ls1.field(client.ChanField.CUSTOM0) == 100) == ls1.h * ls1.w - with pytest.raises(ValueError): - ls1.field(client.ChanField.CUSTOM4)[0, 0] == 100 + ls1.field("custom0") == 100) == ls1.h * ls1.w + with pytest.raises(IndexError): + ls1.field("custom4")[0, 0] == 100 def test_scan_copy_cast() -> None: """ Verify we can clone a scan and cast between field types """ - ls0 = client.LidarScan(32, 512, { - client.ChanField.CUSTOM0: np.uint32, - client.ChanField.CUSTOM4: np.uint8 - }) + ls0 = client.LidarScan(32, 512, [ + FieldType("custom0", np.uint32), + FieldType("custom4", np.uint8) + ]) - ls0.field(client.ChanField.CUSTOM0)[:] = 2 ** 16 - 1 - ls0.field(client.ChanField.CUSTOM4)[:] = 255 + ls0.field("custom0")[:] = 2 ** 16 - 1 + ls0.field("custom4")[:] = 255 - ls1 = client.LidarScan(ls0, { - client.ChanField.CUSTOM0: np.uint8, - client.ChanField.CUSTOM4: np.uint16 - }) + ls1 = client.LidarScan(ls0, [ + FieldType("custom0", np.uint8), + FieldType("custom4", np.uint16) + ]) assert ls0.h == ls1.h assert ls0.w == ls1.w assert len(list(ls1.fields)) == 2 - assert ls1.field(client.ChanField.CUSTOM0).dtype == np.uint8 - assert ls1.field(client.ChanField.CUSTOM4).dtype == np.uint16 + assert ls1.field("custom0").dtype == np.uint8 + assert ls1.field("custom4").dtype == np.uint16 assert np.count_nonzero( - ls1.field(client.ChanField.CUSTOM0) == 255) == ls1.h * ls1.w + ls1.field("custom0") == 255) == ls1.h * ls1.w assert np.count_nonzero( - ls1.field(client.ChanField.CUSTOM4) == 255) == ls1.h * ls1.w + ls1.field("custom4") == 255) == ls1.h * ls1.w def test_scan_copy() -> None: - ls0 = client.LidarScan(32, 512, { - client.ChanField.CUSTOM0: np.uint32, - client.ChanField.CUSTOM4: np.uint8 - }) + ls0 = client.LidarScan(32, 512, [ + FieldType("custom0", np.uint32), + FieldType("custom4", np.uint8) + ]) - ls0.field(client.ChanField.CUSTOM0)[:] = 100 - ls0.field(client.ChanField.CUSTOM4)[:] = 123 + ls0.field("custom0")[:] = 100 + ls0.field("custom4")[:] = 123 ls1 = client.LidarScan(ls0) @@ -578,131 +551,18 @@ def test_scan_copy() -> None: assert len(list(ls1.fields)) == 2 assert np.count_nonzero( - ls1.field(client.ChanField.CUSTOM0) == 100) == ls1.h * ls1.w + ls1.field("custom0") == 100) == ls1.h * ls1.w assert np.count_nonzero( - ls1.field(client.ChanField.CUSTOM4) == 123) == ls1.h * ls1.w - - -def test_error_eq() -> None: - assert client.PacketSizeError("abc") == client.PacketSizeError("abc") - - -def test_lidar_packet_validator() -> None: - """LidarPacketValidator should be capable of the same init_id/sn check as LidarPacket.""" - meta_file_path = os.path.join(PCAPS_DATA_DIR, 'OS-0-128-U1_v2.3.0_1024x10.json') - pcap_file_path = os.path.join(PCAPS_DATA_DIR, 'OS-0-128-U1_v2.3.0_1024x10.pcap') - metadata = client.SensorInfo(open(meta_file_path).read()) - - metadata2 = deepcopy(metadata) - metadata2.init_id = 1234 - metadata2.sn = "5678" - - validator = client.LidarPacketValidator(metadata2) - pcap_handle = _pcap.replay_initialize(pcap_file_path) - buf = bytearray(2**16) - info = _pcap.packet_info() - _pcap.next_packet_info(pcap_handle, info) - n_bytes = _pcap.read_packet(pcap_handle, buf) - errors = validator.check_packet(buf, n_bytes) - assert n_bytes == 8448 - try: - # LidarPacket constructor should throw the same error if _raise_on_id_check is True - client.LidarPacket(buf, metadata2, None, _raise_on_id_check=True) - except client.data.PacketIdError as e: - assert type(errors) is list - assert len(errors) == 1 - assert str(e) == str(errors[0]) - assert str(e) == "Metadata init_id/sn does not match: expected by metadata \ -- 1234/5678, but got from packet buffer - 5431292/122150000150" - pass - else: - assert False, "Expected an exception to be raised by client.LidarPacket." - finally: - _pcap.replay_uninitialize(pcap_handle) - - -def test_lidar_packet_validator_2() -> None: - """LidarPacketValidator check_packet should return None if there are no issues.""" - meta_file_path = os.path.join(PCAPS_DATA_DIR, 'OS-0-128-U1_v2.3.0_1024x10.json') - pcap_file_path = os.path.join(PCAPS_DATA_DIR, 'OS-0-128-U1_v2.3.0_1024x10.pcap') - metadata = client.SensorInfo(open(meta_file_path).read()) - - metadata2 = deepcopy(metadata) - - validator = client.LidarPacketValidator(metadata2) - pcap_handle = _pcap.replay_initialize(pcap_file_path) - buf = bytearray(2**16) - info = _pcap.packet_info() - _pcap.next_packet_info(pcap_handle, info) - n_bytes = _pcap.read_packet(pcap_handle, buf) - assert n_bytes == 8448 - assert validator.check_packet(buf, n_bytes) == [] - _pcap.replay_uninitialize(pcap_handle) - - -def test_lidar_packet_validator_3() -> None: - """LidarPacketValidator check_packet should identify a packet - that's the wrong size according to the lidar UDP profile.""" - meta_file_path = os.path.join(PCAPS_DATA_DIR, 'OS-0-128-U1_v2.3.0_1024x10.json') - pcap_file_path = os.path.join(PCAPS_DATA_DIR, 'OS-0-128-U1_v2.3.0_1024x10.pcap') - metadata = client.SensorInfo(open(meta_file_path).read()) - - metadata2 = deepcopy(metadata) - metadata2.format.udp_profile_lidar = client.UDPProfileLidar.PROFILE_LIDAR_FIVE_WORD_PIXEL - - validator = client.LidarPacketValidator(metadata2) - pcap_handle = _pcap.replay_initialize(pcap_file_path) - buf = bytearray(2**16) - info = _pcap.packet_info() - _pcap.next_packet_info(pcap_handle, info) - n_bytes = _pcap.read_packet(pcap_handle, buf) - assert n_bytes == 8448 - errors = validator.check_packet(buf, n_bytes) - assert len(errors) == 1 - assert type(errors[0]) is client.PacketSizeError - assert str(errors[0]) == 'Expected a packet of size 41216 but got a buffer of size 8448' - _pcap.replay_uninitialize(pcap_handle) - - -def test_lidar_packet_validator_4() -> None: - """LidarPacketValidator check_packet should identify a packet - that's the wrong size according to the data format.""" - meta_file_path = os.path.join(PCAPS_DATA_DIR, 'OS-0-128-U1_v2.3.0_1024x10.json') - pcap_file_path = os.path.join(PCAPS_DATA_DIR, 'OS-0-128-U1_v2.3.0_1024x10.pcap') - meta_json = json.load(open(meta_file_path)) - meta_json['data_format']['columns_per_frame'] = 4096 - meta_json['data_format']['pixels_per_column'] = 16 - meta_json['data_format']['pixel_shift_by_row'] = [0] * 16 - meta_json['beam_altitude_angles'] = [1] * 16 - meta_json['beam_azimuth_angles'] = [1] * 16 - meta_json['prod_sn'] = '1234' - metadata2 = client.SensorInfo(json.dumps(meta_json)) - - validator = client.LidarPacketValidator(metadata2) - pcap_handle = _pcap.replay_initialize(pcap_file_path) - buf = bytearray(2**16) - info = _pcap.packet_info() - _pcap.next_packet_info(pcap_handle, info) - n_bytes = _pcap.read_packet(pcap_handle, buf) - assert n_bytes == 8448 - errors = validator.check_packet(buf, n_bytes) - assert len(errors) == 2 - assert type(errors[0]) is client.PacketIdError - assert str( - errors[0]) == 'Metadata init_id/sn does not match: expected by metadata - \ -5431292/1234, but got from packet buffer - 5431292/122150000150' - assert type(errors[1]) is client.PacketSizeError - assert str(errors[1]) == 'Expected a packet of size 1280 but got a buffer of size 8448' - _pcap.replay_uninitialize(pcap_handle) + ls1.field("custom4") == 123) == ls1.h * ls1.w def test_packet_writer_bindings(meta: client.SensorInfo) -> None: - pf = _client.PacketFormat.from_info(meta) - packet = client.LidarPacket(packet_format=pf) + pf = PacketFormat.from_info(meta) + packet = client.LidarPacket(pf.lidar_packet_size) - pw = _client.PacketWriter.from_info(meta) + pw = PacketWriter.from_info(meta) pw.set_frame_id(packet, 700) - assert pf.frame_id(packet._data) == 700 + assert pf.frame_id(packet.buf) == 700 with pytest.raises(ValueError): pw.set_col_timestamp(packet, pw.columns_per_packet, 100) @@ -720,12 +580,12 @@ def test_packet_writer_bindings(meta: client.SensorInfo) -> None: assert False, "setting cols up to columns_per_packet should not raise" for dt in [np.uint8, np.uint16, np.uint32, np.uint64]: - p = client.LidarPacket(packet_format=pf) - for chan in p.fields: - if chan in [_client.ChanField.RAW32_WORD1, - _client.ChanField.RAW32_WORD2, - _client.ChanField.RAW32_WORD3, - _client.ChanField.RAW32_WORD4]: + p = client.LidarPacket(pf.lidar_packet_size) + for chan in pf.fields: + if chan in [client.ChanField.RAW32_WORD1, + client.ChanField.RAW32_WORD2, + client.ChanField.RAW32_WORD3, + client.ChanField.RAW32_WORD4]: continue # mypy is going nuts with mask notation value_mask = pw.field_value_mask(chan) & np.iinfo(dt).max # type: ignore @@ -737,16 +597,150 @@ def test_packet_writer_bindings(meta: client.SensorInfo) -> None: s = hex(value_mask) assert np.any(field > 0), f"{chan}, {dt}, {s}" pw.set_field(p, chan, field) - assert np.all(pw.packet_field(chan, p._data) == field), f"{chan}, {dt}, {s}" + assert np.all(pw.packet_field(chan, p.buf) == field), f"{chan}, {dt}, {s}" columns_per_frame = meta.format.columns_per_frame ls = client.LidarScan(pf.pixels_per_column, columns_per_frame, pf.udp_profile_lidar, pf.columns_per_packet) # all fields are invalid, expect zero packets - packets = _client.scan_to_packets(ls, pw, 0, 0) + packets = scan_to_packets(ls, pw, 0, 0) assert len(packets) == 0 expected_packets = columns_per_frame / pf.columns_per_packet ls.status[:] = 0x1 - packets = _client.scan_to_packets(ls, pw, 0, 0) + packets = scan_to_packets(ls, pw, 0, 0) assert len(packets) == expected_packets + + +def test_to_string_doesnt_cause_fp_exception(): + """It shouldn't crash with a floating point exception when std::to_string(LidarScan&) is called.""" + str(client.LidarScan(1024, 128, client.UDPProfileLidar.PROFILE_LIDAR_RNG19_RFL8_SIG16_NIR16_DUAL)) + + +def test_scan_float_double() -> None: + """Test that we can add float fields and that setting floats in them works.""" + ls = client.LidarScan( + 64, 1024, + client.UDPProfileLidar.PROFILE_LIDAR_RNG19_RFL8_SIG16_NIR16_DUAL) + + ls.add_field("f32", np.float32, (), client.FieldClass.PIXEL_FIELD) + ls.add_field("f64", np.float64, (), client.FieldClass.PIXEL_FIELD) + + ls.field("f32")[:] = 3.3 + ls.field("f64")[:] = 6.6 + + assert ls.field("f32")[1, 1] == np.float32(3.3) + assert ls.field("f64")[1, 1] == 6.6 + + +def test_scan_int() -> None: + """Test that we can add int fields and that setting ints in them works.""" + ls = client.LidarScan( + 64, 1024, + client.UDPProfileLidar.PROFILE_LIDAR_RNG19_RFL8_SIG16_NIR16_DUAL) + + ls.add_field("i8", np.int8, (), client.FieldClass.PIXEL_FIELD) + ls.add_field("i16", np.int16, (), client.FieldClass.PIXEL_FIELD) + ls.add_field("i32", np.int32, (), client.FieldClass.PIXEL_FIELD) + ls.add_field("i64", np.int64, (), client.FieldClass.PIXEL_FIELD) + + ls.field("i8")[:] = 0x7FF + ls.field("i8")[1, 2] = -2 + ls.field("i16")[:] = 0x7FFFF + ls.field("i16")[1, 2] = -2 + ls.field("i32")[:] = 0x7FFFFFFF + ls.field("i32")[1, 2] = -2 + ls.field("i64")[:] = 0x7FFFFFFFFFFFFFFF + ls.field("i64")[1, 2] = -2 + + assert ls.field("i8")[1, 1] == -1 + assert ls.field("i8")[1, 2] == -2 + assert ls.field("i16")[1, 1] == -1 + assert ls.field("i16")[1, 2] == -2 + assert ls.field("i32")[1, 1] == 0x7FFFFFFF + assert ls.field("i32")[1, 2] == -2 + assert ls.field("i64")[1, 1] == 0x7FFFFFFFFFFFFFFF + assert ls.field("i64")[1, 2] == -2 + + +def test_lidarscan_3d_field() -> None: + """It should allow adding a 3d field.""" + h, w, d = 64, 1024, 3 + field_types = [ + client.FieldType(client.ChanField.RANGE, np.uint32, (d,), client.FieldClass.PIXEL_FIELD) + ] + ls = client.LidarScan(h, w, field_types) + assert ls.fields == [client.ChanField.RANGE] + range_field = ls.field(client.ChanField.RANGE) + assert range_field.dtype == np.uint32 + assert range_field.shape == (h, w, d) + + +def test_lidarscan_add_field_3d_field() -> None: + """It should allow adding a 3d field.""" + h, w, d = 64, 1024, 3 + ls = client.LidarScan(h, w, []) + ls.add_field(client.ChanField.RANGE, np.uint32, (d,), client.FieldClass.PIXEL_FIELD) + assert ls.fields == [client.ChanField.RANGE] + range_field = ls.field(client.ChanField.RANGE) + assert range_field.dtype == np.uint32 + assert range_field.shape == (h, w, 3) + + +def test_lidarscan_add_field_default_pixel() -> None: + """FieldType flags should be PIXEL by default.""" + ft = client.FieldType(client.ChanField.RANGE, np.uint32, ()) + assert ft.field_class == client.FieldClass.PIXEL_FIELD + # it's mutable + ft.field_class = client.FieldClass.COLUMN_FIELD + assert ft.field_class == client.FieldClass.COLUMN_FIELD + + # the constructor sets it + ft = client.FieldType(client.ChanField.RANGE, np.uint32, (), client.FieldClass.COLUMN_FIELD) + assert ft.field_class == client.FieldClass.COLUMN_FIELD + + +def test_lidarscan_fieldtype_name() -> None: + """FieldType name should be accessible.""" + ft = client.FieldType(client.ChanField.RANGE, np.uint32, ()) + assert ft.name == client.ChanField.RANGE + + # it's mutable + newname = 'foobar' + ft.name = newname + assert ft.name == newname + + +def test_lidarscan_fieldtype_extra_dims() -> None: + """FieldType extra_dims should be accessible.""" + extra_dims = (1, 2, 3) + ft = client.FieldType(client.ChanField.RANGE, np.uint32, extra_dims) + print(ft.extra_dims) + assert ft.extra_dims == extra_dims + + # it's mutable + extra_dims = (4, 5, 6) + ft.extra_dims = extra_dims + assert ft.extra_dims == extra_dims + + +def test_lidarscan_fieldtype_dtype() -> None: + """FieldType element_type should be accessible""" + ft = client.FieldType(client.ChanField.RANGE, np.uint32, ()) + assert ft.element_type is np.dtype(np.uint32) + + # it's mutable + ft.element_type = np.dtype(np.uint8) + assert ft.element_type is np.dtype(np.uint8) + + +def test_lidarscan_add_field_with_value() -> None: + """LidarScan.add_field should accept an array to be used as the field's + initial value""" + h, w = 64, 1024 + ls = client.LidarScan(h, w, []) + assert client.ChanField.RANGE not in ls.fields + with pytest.raises(IndexError): + ls.field(client.ChanField.RANGE) + ls.add_field(client.ChanField.RANGE, np.ones((h, w), np.int16)) + assert ls.field(client.ChanField.RANGE).all() diff --git a/python/tests/test_extended_profiles.py b/python/tests/test_extended_profiles.py index 78d9c89d..312c4731 100644 --- a/python/tests/test_extended_profiles.py +++ b/python/tests/test_extended_profiles.py @@ -6,8 +6,8 @@ import numpy as np from ouster.sdk import client -from ouster.sdk.client import _client -from ouster.sdk.client._client import (ChanField, FieldInfo) +from ouster.sdk.client import _client, ChanField +from ouster.sdk.client._client import FieldInfo def test_create_field_info() -> None: @@ -23,8 +23,8 @@ def test_add_custom_profile() -> None: profile_nr = 1000 profile_name = "CUSTOM_PROF" fields = [ - (0, FieldInfo(np.uint16, 0, 0xdeadbeef, 0)), - (1, FieldInfo(np.uint16, 1, 0xff, 2)) + (ChanField.RANGE, FieldInfo(np.uint16, 0, 0xdeadbeef, 0)), + (ChanField.REFLECTIVITY, FieldInfo(np.uint16, 1, 0xff, 2)) ] chan_data_size = 16 @@ -37,13 +37,13 @@ def test_add_custom_profile() -> None: def test_custom_copycat_profile_matches_original(packets: client.PacketSource) -> None: """Check that custom profile parses the same as original with the same fields""" custom_fields = [ - (ChanField.RANGE.value, FieldInfo(np.uint32, 0, 0x0007ffff, 0)), - (ChanField.REFLECTIVITY.value, FieldInfo(np.uint8, 3, 0, 0)), - (ChanField.RANGE2.value, FieldInfo(np.uint32, 4, 0x0007ffff, 0)), - (ChanField.REFLECTIVITY2.value, FieldInfo(np.uint8, 7, 0, 0)), - (ChanField.SIGNAL.value, FieldInfo(np.uint16, 8, 0, 0)), - (ChanField.SIGNAL2.value, FieldInfo(np.uint16, 10, 0, 0)), - (ChanField.NEAR_IR.value, FieldInfo(np.uint16, 12, 0, 0)), + (ChanField.RANGE, FieldInfo(np.uint32, 0, 0x0007ffff, 0)), + (ChanField.REFLECTIVITY, FieldInfo(np.uint8, 3, 0, 0)), + (ChanField.RANGE2, FieldInfo(np.uint32, 4, 0x0007ffff, 0)), + (ChanField.REFLECTIVITY2, FieldInfo(np.uint8, 7, 0, 0)), + (ChanField.SIGNAL, FieldInfo(np.uint16, 8, 0, 0)), + (ChanField.SIGNAL2, FieldInfo(np.uint16, 10, 0, 0)), + (ChanField.NEAR_IR, FieldInfo(np.uint16, 12, 0, 0)), ] _client.add_custom_profile(11, "DUAL_COPYCAT", custom_fields, 16) diff --git a/python/tests/test_forward_slicer.py b/python/tests/test_forward_slicer.py index e9eb19b7..715ec6b0 100644 --- a/python/tests/test_forward_slicer.py +++ b/python/tests/test_forward_slicer.py @@ -99,7 +99,23 @@ def sliceable_fixture(): (None, None, -2), (None, None, -3), ]) -def test_sliceable(sliceable_fixture, start, stop, step): +def test_forward_slicer_valid(sliceable_fixture, start, stop, step): ref_sliceable, test_sliceable = sliceable_fixture ss = slice(start, stop, step) - assert ref_sliceable[ss] == test_sliceable[ss], f"Failed test case with the slice [{start}:{stop}:{step}]" + assert ref_sliceable[ss] == test_sliceable[ss], \ + f"Failed test case with the slice [{start}:{stop}:{step}]" + + +@pytest.mark.parametrize("start, stop, step", [ + (None, None, 0), + (None, 8, 0), + (3, None, 0), + (3, 8, 0), + (-3, -8, 0), +]) +def test_forward_slicer_zero_step_throws(sliceable_fixture, start, stop, step): + _, test_sliceable = sliceable_fixture + ss = slice(start, stop, step) + with pytest.raises(ValueError) as ex: + _ = test_sliceable[ss] + assert str(ex.value) == "slice step cannot be zero" diff --git a/python/tests/test_metadata.py b/python/tests/test_metadata.py index 3857f327..12f35936 100644 --- a/python/tests/test_metadata.py +++ b/python/tests/test_metadata.py @@ -10,11 +10,10 @@ import pytest import inspect from pathlib import Path -from os import path from ouster.sdk import client -from tests.conftest import METADATA_DATA_DIR, PCAPS_DATA_DIR +from tests.conftest import METADATA_DATA_DIR @pytest.mark.parametrize("mode, string", [ @@ -39,23 +38,6 @@ def test_timestamp_mode_misc() -> None: client.TimestampMode(0) == client.TimestampMode.TIME_FROM_UNSPEC -@pytest.mark.parametrize("mode, cols, frequency, string", [ - (client.LidarMode.MODE_512x10, 512, 10, "512x10"), - (client.LidarMode.MODE_512x20, 512, 20, "512x20"), - (client.LidarMode.MODE_1024x10, 1024, 10, "1024x10"), - (client.LidarMode.MODE_1024x20, 1024, 20, "1024x20"), - (client.LidarMode.MODE_2048x10, 2048, 10, "2048x10"), - (client.LidarMode.MODE_4096x5, 4096, 5, "4096x5"), -]) -def test_lidar_mode(mode, cols, frequency, string) -> None: - """Check lidar mode (un)parsing and cols/frequency.""" - int(mode) # make sure nothing is raised - assert str(mode) == string - assert client.LidarMode.from_string(string) == mode - assert mode.cols == cols - assert mode.frequency == frequency - - def test_lidar_mode_misc() -> None: """Check some misc properties of lidar mode.""" assert len( @@ -63,23 +45,14 @@ def test_lidar_mode_misc() -> None: assert client.LidarMode.from_string('foo') == client.LidarMode.MODE_UNSPEC assert client.LidarMode(0) == client.LidarMode.MODE_UNSPEC assert str(client.LidarMode.MODE_UNSPEC) == "UNKNOWN" - with pytest.raises(ValueError): - client.LidarMode.MODE_UNSPEC.cols - with pytest.raises(ValueError): - client.LidarMode.MODE_UNSPEC.frequency - with pytest.raises(AttributeError): - client.LidarMode.MODE_1024x10.cols = 1025 # type: ignore - with pytest.raises(AttributeError): - client.LidarMode.MODE_1024x10.frequency = 0 # type: ignore @pytest.mark.parametrize('test_key', ['legacy-2.0']) def test_read_info(meta: client.SensorInfo) -> None: """Check the particular values in the test data.""" - assert meta.hostname == "os-992029000352.local" assert meta.sn == "992029000352" assert meta.fw_rev == "v2.0.0-rc.2" - assert meta.mode == client.LidarMode.MODE_1024x20 + assert meta.config.lidar_mode == client.LidarMode.MODE_1024x20 assert meta.prod_line == "OS-2-32-U0" assert meta.format.columns_per_frame == 1024 assert meta.format.columns_per_packet == 16 @@ -101,23 +74,32 @@ def test_read_info(meta: client.SensorInfo) -> None: assert numpy.array_equal(meta.extrinsic, numpy.identity(4)) assert meta.init_id == 0 - assert meta.udp_port_lidar == 0 - assert meta.udp_port_imu == 0 + assert meta.config.udp_port_lidar is None + assert meta.config.udp_port_imu is None assert meta.build_date == "2020-10-23T14:05:18Z" assert meta.prod_pn == "840-102146-C" assert meta.image_rev == "ousteros-image-prod-aries-v2.0.0-rc.2+20201023140416.staging" assert meta.status == "RUNNING" assert meta.cal == client.SensorCalibration() - assert meta.config == client.SensorConfig() + + ref_config = client.SensorConfig() + ref_config.lidar_mode = client.LidarMode.MODE_1024x20 + assert meta.config == ref_config + assert meta.get_version().major == 2 + + product_info = meta.get_product_info() + assert product_info.full_product_info == meta.prod_line + assert product_info.form_factor == "OS2" + assert product_info.short_range is False + assert product_info.beam_config == "U0" + assert product_info.beam_count == 32 def test_write_info(meta: client.SensorInfo) -> None: """Check modifying metadata.""" - meta.hostname = "" meta.sn = "" meta.fw_rev = "" - meta.mode = client.LidarMode.MODE_UNSPEC meta.prod_line = "" meta.format.columns_per_frame = 0 meta.format.columns_per_packet = 0 @@ -135,18 +117,21 @@ def test_write_info(meta: client.SensorInfo) -> None: meta.lidar_origin_to_beam_origin_mm = 0.0 meta.beam_to_lidar_transform = numpy.zeros((4, 4)) meta.init_id = 0 - meta.udp_port_lidar = 0 - meta.udp_port_imu = 0 meta.build_date = "" meta.image_rev = "" meta.prod_pn = "" meta.status = "" + meta.user_data = "" meta.cal = client.SensorCalibration() meta.config = client.SensorConfig() + meta.config.udp_port_lidar = None + meta.config.udp_port_imu = None + meta.config.lidar_mode = None - assert meta != client.SensorInfo() + assert meta == client.SensorInfo() assert meta.has_fields_equal(client.SensorInfo()) - assert meta.original_string() != client.SensorInfo().original_string() + client.SensorInfo().to_json_string() + assert meta.to_json_string() == client.SensorInfo().to_json_string() with pytest.raises(TypeError): meta.mode = 1 # type: ignore @@ -161,6 +146,18 @@ def test_write_info(meta: client.SensorInfo) -> None: with pytest.raises(TypeError): meta.beam_azimuth_angles = ["foo"] # type: ignore + product_info = meta.get_product_info() + with pytest.raises(AttributeError): + product_info.full_product_info = "" + with pytest.raises(AttributeError): + product_info.form_factor = "" + with pytest.raises(AttributeError): + product_info.short_range = True + with pytest.raises(AttributeError): + product_info.beam_config = "" + with pytest.raises(AttributeError): + product_info.beam_count = 1 + def test_copy_info(meta: client.SensorInfo) -> None: """Check that copy() works.""" @@ -173,10 +170,18 @@ def test_copy_info(meta: client.SensorInfo) -> None: assert meta1 != meta meta2 = copy(meta) - meta2.hostname = "foo" + meta2.sn = "foo" assert meta2 != meta +def test_userdata(meta: client.SensorInfo) -> None: + meta.user_data = "test" + + meta2 = client.SensorInfo(meta.to_json_string()) + + assert meta2.user_data == meta.user_data + + def test_parse_info() -> None: """Sanity check parsing from json.""" with pytest.raises(RuntimeError): @@ -256,27 +261,21 @@ def test_skip_metadata_beam_validation() -> None: client.SensorInfo(f.read(), skip_beam_validation = True) -@pytest.mark.parametrize('test_key', ['legacy-2.0', 'single-2.3']) -def test_original_string(meta: client.SensorInfo, base_name) -> None: - meta_path = path.join(PCAPS_DATA_DIR, f"{base_name}.json") - with open(meta_path, 'r') as f: - assert meta.original_string() == f.read() - - @pytest.mark.parametrize('metadata_key', ['1_12', '1_12_legacy', '1_13', '1_13_legacy', '1_14_128_legacy', '2_0', '2_0_legacy', '2_1', '2_1_legacy', '2_2', '2_2_legacy', '2_3', '2_3_legacy', '2_4', '2_4_legacy', '2_5', '2_5_legacy', '3_0']) def test_updated_string(metadata_base_name) -> None: meta_path = str(Path(METADATA_DATA_DIR) / f"{metadata_base_name}") - + original_string = "" with open(meta_path, 'r') as f: - meta = client.SensorInfo(f.read()) + original_string = f.read() + meta = client.SensorInfo(original_string) meta.format.columns_per_packet = 40 meta.format.fps = 50 # crucial check - fps is a value that is filled in with a default value at parsing time - updated_metadata_string = meta.updated_metadata_string() + updated_metadata_string = meta.to_json_string() - assert updated_metadata_string != meta.original_string() + assert updated_metadata_string != original_string meta2 = client.SensorInfo(updated_metadata_string, skip_beam_validation=True) assert meta2.format.columns_per_packet == 40 diff --git a/python/tests/test_packet_iter.py b/python/tests/test_packet_iter.py index 48d8a71f..4e46e70f 100644 --- a/python/tests/test_packet_iter.py +++ b/python/tests/test_packet_iter.py @@ -139,7 +139,7 @@ def test_recording_packet_source_bad_packet_format(tmp_path) -> None: meta_file_path = os.path.join(PCAPS_DATA_DIR, 'OS-0-128-U1_v2.3.0_1024x10.json') pcap_file_path = os.path.join(PCAPS_DATA_DIR, 'VLI-16-one-packet.pcap') source = PcapMultiPacketReader(pcap_file_path, [meta_file_path]) - source.metadata[0].udp_port_lidar = 2368 + source.metadata[0].config.udp_port_lidar = 2368 recording_iter = packet_iter.RecordingPacketSource(source, str(tmp_path) + "/test", n_frames=1) emitted_packets = 0 for (idx, packet) in recording_iter: diff --git a/python/tests/test_parsing.py b/python/tests/test_parsing.py index c15d25f7..12c65d4c 100644 --- a/python/tests/test_parsing.py +++ b/python/tests/test_parsing.py @@ -2,10 +2,10 @@ import os import numpy as np import ouster.sdk.pcap._pcap as _pcap -from ouster.sdk.client import LidarMode, SensorInfo, UDPProfileLidar, ChanField +from ouster.sdk.client import LidarMode, SensorInfo, UDPProfileLidar, ChanField, PacketFormat import ouster.sdk.client as client import ouster.sdk.pcap as pcap -from ouster.sdk.util import (FusaDualFormat, PacketFormat, default_scan_fields, +from ouster.sdk.util import (default_scan_fields, resolve_metadata) from tests.conftest import PCAPS_DATA_DIR @@ -14,8 +14,8 @@ def test_fusa_parsing_profile(): dataset_name = 'OS-1-128_767798045_1024x10_20230712_120049' meta = open(os.path.join(PCAPS_DATA_DIR, f'{dataset_name}.json')).read() si = SensorInfo(meta) - packet_format = PacketFormat.from_metadata(si) - assert type(packet_format) is FusaDualFormat + packet_format = PacketFormat(si) + assert packet_format.udp_profile_lidar == UDPProfileLidar.PROFILE_LIDAR_FUSA_RNG15_RFL8_NIR8_DUAL def test_fusa_fields(): @@ -23,7 +23,7 @@ def test_fusa_fields(): meta = open(os.path.join(PCAPS_DATA_DIR, f'{dataset_name}.json')).read() si = SensorInfo(meta) pcap = _pcap.replay_initialize(os.path.join(PCAPS_DATA_DIR, f'{dataset_name}.pcap')) - packet_format = PacketFormat.from_metadata(si) + packet_format = PacketFormat(si) # check preconditions assert si.format.columns_per_packet == 16 diff --git a/python/tests/test_pcap.py b/python/tests/test_pcap.py index c75432de..5ead4d03 100644 --- a/python/tests/test_pcap.py +++ b/python/tests/test_pcap.py @@ -14,10 +14,11 @@ import pytest import time +import numpy as np from ouster.sdk import client, pcap, open_source from ouster.sdk.pcap import _pcap -from ouster.sdk.client import _client +from ouster.sdk.client import _client, PacketValidationFailure from tests.conftest import PCAPS_DATA_DIR, TESTS from tests.test_batching import _patch_frame_id @@ -43,10 +44,18 @@ def fake_packets(metadata: client.SensorInfo, if is_lidar: buf = bytearray( getrandbits(8) for _ in range(pf.lidar_packet_size)) - yield client.LidarPacket(buf, metadata, packet_ts) + packet = client.LidarPacket(len(buf)) + packet.buf[:] = np.frombuffer(buf, dtype=np.uint8, count=len(buf)) + if timestamped: + packet.host_timestamp = int(packet_ts * 1e9) + yield packet else: buf = bytearray(getrandbits(8) for _ in range(pf.imu_packet_size)) - yield client.ImuPacket(buf, metadata, packet_ts) + packet = client.ImuPacket(len(buf)) + packet.buf[:] = np.frombuffer(buf, dtype=np.uint8, count=len(buf)) + if timestamped: + packet.host_timestamp = int(packet_ts * 1e9) + yield packet def set_init_id(data: bytearray, init_id: int) -> None: @@ -56,7 +65,7 @@ def set_init_id(data: bytearray, init_id: int) -> None: def set_prod_sn(data: bytearray, prod_sn: int) -> None: """Rewrite the sn of a non-legacy format lidar packet.""" - data[7:11] = memoryview(prod_sn.to_bytes(5, byteorder='little')) + data[7:12] = memoryview(prod_sn.to_bytes(5, byteorder='little')) def fake_packet_stream_with_frame_id(metadata: client.SensorInfo, @@ -73,19 +82,22 @@ def fake_packet_stream_with_frame_id(metadata: client.SensorInfo, choices = [True] * n_lidar_packets_per_frame + [False] * n_imu_packets_per_frame shuffle(choices) for is_lidar in choices: - packet_ts = None if is_lidar: buf = bytearray( getrandbits(8) for _ in range(pf.lidar_packet_size)) set_init_id(buf, metadata.init_id) set_prod_sn(buf, int(metadata.sn)) - packet = client.LidarPacket(buf, metadata, packet_ts) - assert not packet.id_error + packet = client.LidarPacket(len(buf)) + packet.buf[:] = np.frombuffer(buf, dtype=np.uint8, count=len(buf)) + assert packet.validate(metadata, pf) == PacketValidationFailure.NONE _patch_frame_id(packet, frame_id_fn(frame)) yield packet else: buf = bytearray(getrandbits(8) for _ in range(pf.imu_packet_size)) - yield client.ImuPacket(buf, metadata, packet_ts) + packet = client.ImuPacket(len(buf)) + packet.buf[:] = np.frombuffer(buf, dtype=np.uint8, count=len(buf)) + assert packet.validate(metadata, pf) == PacketValidationFailure.NONE + yield packet @pytest.fixture @@ -131,20 +143,23 @@ def fake_pcap(fake_meta, fake_pcap_path) -> Iterable[pcap.Pcap]: @pytest.mark.parametrize('n_packets', [0]) def test_pcap_read_empty(fake_meta, fake_pcap_path) -> None: """Check reading an empty pcap doesn't fail.""" + fake_meta = copy(fake_meta) + fake_meta.config.udp_port_lidar = 7505 + fake_meta.config.udp_port_imu = 7506 fake_pcap = pcap.Pcap(fake_pcap_path, fake_meta) - assert fake_pcap.ports == (0, 0) + assert fake_pcap.ports == (7505, 7506) assert list(fake_pcap) == [] @pytest.mark.parametrize('n_packets', [10]) def test_pcap_read_wrong_ports(fake_meta, fake_pcap_path) -> None: """Check specifying wrong ports.""" + fake_meta = copy(fake_meta) + fake_meta.config.udp_port_lidar = 7505 + fake_meta.config.udp_port_imu = 7506 fake_pcap = pcap.Pcap(fake_pcap_path, - fake_meta, - lidar_port=7505, - imu_port=7506) + fake_meta) assert fake_pcap.ports == (7505, 7506) - assert fake_pcap._guesses == [] assert list(fake_pcap) == [] @@ -154,11 +169,11 @@ def test_pcap_guess_one_port_invalid(fake_meta, fake_pcap_path) -> None: Capture contains data on (7502, 7503), but user expects lidar data on 7505. """ + fake_meta = copy(fake_meta) + fake_meta.config.udp_port_lidar = 7505 + fake_meta.config.udp_port_imu = 0 fake_pcap = pcap.Pcap(fake_pcap_path, - fake_meta, - lidar_port=7505, - imu_port=0) - assert fake_pcap._guesses == [] + fake_meta) assert fake_pcap.ports == (7505, 0) assert list(fake_pcap) == [] @@ -170,11 +185,11 @@ def test_pcap_guess_one_port_valid(fake_meta, fake_pcap_path) -> None: Capture contains just lidar data on 7502, and user expects imu on 7503. """ + fake_meta = copy(fake_meta) + fake_meta.config.udp_port_lidar = 0 + fake_meta.config.udp_port_imu = 7503 fake_pcap = pcap.Pcap(fake_pcap_path, - fake_meta, - lidar_port=0, - imu_port=7503) - assert fake_pcap._guesses == [(7502, 0)] # no imu packets, no guess + fake_meta) assert fake_pcap.ports == (7502, 7503) assert len(list(fake_pcap)) == 10 @@ -189,11 +204,11 @@ def test_pcap_read_10(fake_pcap) -> None: @pytest.mark.parametrize('n_packets', [10]) def test_pcap_infer_one_port(fake_meta, fake_pcap_path) -> None: """Check that a matching port is inferred when one is specified. """ + fake_meta = copy(fake_meta) + fake_meta.config.udp_port_lidar = 0 + fake_meta.config.udp_port_imu = 7503 fake_pcap = pcap.Pcap(fake_pcap_path, - fake_meta, - lidar_port=0, - imu_port=7503) - assert fake_pcap._guesses == [(7502, 7503)] + fake_meta) assert fake_pcap.ports == (7502, 7503) assert len(list(fake_pcap)) == 10 @@ -236,8 +251,8 @@ def test_pcap_reset(fake_pcap) -> None: fake_pcap.reset() packets2 = list(fake_pcap) - bufs1 = [bytes(p._data) for p in packets1] - bufs2 = [bytes(p._data) for p in packets2] + bufs1 = [bytes(p.buf) for p in packets1] + bufs2 = [bytes(p.buf) for p in packets2] assert bufs1 == bufs2 @@ -263,8 +278,8 @@ def test_read_write_lidar_imu(n_lidar, n_imu, fake_meta, tmpdir) -> None: pcap.record(in_packets, file_path) out_packets = list(pcap.Pcap(file_path, fake_meta)) - out_bufs = [bytes(p._data) for p in out_packets] - in_bufs = [bytes(p._data) for p in in_packets] + out_bufs = [bytes(p.buf) for p in out_packets] + in_bufs = [bytes(p.buf) for p in in_packets] assert in_bufs == out_bufs @@ -277,8 +292,8 @@ def test_timestamp_read_write(fake_meta, tmpdir) -> None: pcap.record(in_packets, file_path) out_packets = list(pcap.Pcap(file_path, fake_meta)) - out_timestamps = [p.capture_timestamp for p in out_packets] - in_timestamps = [p.capture_timestamp for p in in_packets] + out_timestamps = [p.host_timestamp for p in out_packets] + in_timestamps = [p.host_timestamp for p in in_packets] assert len(in_timestamps) == len(out_timestamps) assert in_timestamps == pytest.approx(out_timestamps) @@ -293,12 +308,12 @@ def test_no_timestamp_read_write(fake_meta, tmpdir) -> None: current_timestamp = time.time() pcap.record(in_packets, file_path) out_packets = list(pcap.Pcap(file_path, fake_meta)) - out_timestamps = [p.capture_timestamp for p in out_packets] - in_timestamps = [p.capture_timestamp for p in in_packets] + out_timestamps = [p.host_timestamp for p in out_packets] + in_timestamps = [p.host_timestamp for p in in_packets] assert len(in_timestamps) == len(out_timestamps) - assert all(ts is None for ts in in_timestamps) - assert all(ts == pytest.approx(current_timestamp, abs=1.0) + assert all(ts == 0 for ts in in_timestamps) + assert all(ts == pytest.approx(current_timestamp * 1e9, abs=1e9) for ts in out_timestamps) @@ -367,14 +382,13 @@ def test_lidar_guess_ambiguous(fake_meta, tmpdir) -> None: handle = _pcap.record_initialize(file_path, buf_size) try: _pcap.record_packet(handle, "127.0.0.1", "127.0.0.1", 7502, 7502, - (next(packets))._data, 1) + (next(packets)).buf, 1) _pcap.record_packet(handle, "127.0.0.1", "127.0.0.1", 7503, 7503, - (next(packets))._data, 2) + (next(packets)).buf, 2) finally: _pcap.record_uninitialize(handle) source = pcap.Pcap(file_path, fake_meta) - assert len(source._guesses) > 1 assert source.ports == (7503, 0) # arbitrary but deterministic assert len(list(source)) == 1 @@ -388,14 +402,13 @@ def test_imu_guess_ambiguous(fake_meta, tmpdir) -> None: handle = _pcap.record_initialize(file_path, buf_size) try: _pcap.record_packet(handle, "127.0.0.1", "127.0.0.1", 7502, 7502, - (next(packets))._data, 1) + (next(packets)).buf, 1) _pcap.record_packet(handle, "127.0.0.1", "127.0.0.1", 7503, 7503, - (next(packets))._data, 2) + (next(packets)).buf, 2) finally: _pcap.record_uninitialize(handle) source = pcap.Pcap(file_path, fake_meta) - assert len(source._guesses) > 1 assert source.ports == (0, 7503) # arbitrary but deterministic assert len(list(source)) == 1 @@ -410,18 +423,17 @@ def test_lidar_imu_guess_ambiguous(fake_meta, tmpdir) -> None: handle = _pcap.record_initialize(file_path, buf_size) try: _pcap.record_packet(handle, "127.0.0.1", "127.0.0.1", 7502, 7502, - (next(lidar_packets))._data, 1) + (next(lidar_packets)).buf, 1) _pcap.record_packet(handle, "127.0.0.1", "127.0.0.1", 7503, 7503, - (next(lidar_packets))._data, 2) + (next(lidar_packets)).buf, 2) _pcap.record_packet(handle, "127.0.0.1", "127.0.0.1", 7504, 7504, - (next(imu_packets))._data, 3) + (next(imu_packets)).buf, 3) _pcap.record_packet(handle, "127.0.0.1", "127.0.0.1", 7505, 7505, - (next(imu_packets))._data, 4) + (next(imu_packets)).buf, 4) finally: _pcap.record_uninitialize(handle) source = pcap.Pcap(file_path, fake_meta) - assert len(source._guesses) > 1 assert source.ports == (7503, 7505) # arbitrary but deterministic assert len(list(source)) == 2 @@ -429,8 +441,6 @@ def test_lidar_imu_guess_ambiguous(fake_meta, tmpdir) -> None: def test_pcap_read_real(real_pcap: pcap.Pcap) -> None: """Sanity check reading pcaps with real data.""" - assert len(real_pcap._guesses) == 1 - # lidar data should be on 7502 assert real_pcap.ports[0] == 7502 @@ -442,15 +452,15 @@ def test_pcap_read_real(real_pcap: pcap.Pcap) -> None: assert len(lidar_packets) == 64 # and should have timestamps - assert all(p.capture_timestamp is not None for p in packets) + assert all(p.capture_timestamp != 0 for p in packets) def test_pcap_guess_real(meta: client.SensorInfo, real_pcap_path: str) -> None: """Check that lidar port for real data can inferred.""" meta_no_ports = copy(meta) - meta_no_ports.udp_port_lidar = 0 - meta_no_ports.udp_port_imu = 0 + meta_no_ports.config.udp_port_lidar = 0 + meta_no_ports.config.udp_port_imu = 0 real_pcap = pcap.Pcap(real_pcap_path, meta_no_ports) assert real_pcap.ports[0] == 7502 @@ -475,7 +485,7 @@ def test_record_packet_info(fake_meta, tmpdir) -> None: info.timestamp = i - _pcap.record_packet(record, info, next_packet._data) + _pcap.record_packet(record, info, next_packet.buf) i += 1 @@ -522,7 +532,7 @@ def test_indexed_pcap_reader(tmpdir): previous_frame_id = None while reader.next_packet(): info = reader.current_info() - if info.dst_port == sensor_info.udp_port_lidar: + if info.dst_port == sensor_info.config.udp_port_lidar: packet_frame_id = reader.current_frame_id() if previous_frame_id is None or packet_frame_id > previous_frame_id: assert reader.get_index().frame_indices[0][frame_num] == info.file_offset @@ -614,14 +624,14 @@ def test_current_data(fake_meta, tmpdir): frame_ids = [] while reader.next_packet(): info = reader.current_info() - if info.dst_port == sensor_info.udp_port_lidar: + if info.dst_port == sensor_info.config.udp_port_lidar: packet_data = reader.current_data() frame_ids.append(packet_format.frame_id(packet_data.tobytes())) assert frame_ids == [2, 1, 4, 3, 6, 5, 8, 7, 10, 9] def test_validation(): - """The Pcap class should accumlate errors detected by LidarPacketValidator""" + """The Pcap class should accumlate errors detected""" meta_file_path = path.join(PCAPS_DATA_DIR, 'OS-0-128-U1_v2.3.0_1024x10.json') pcap_file_path = path.join(PCAPS_DATA_DIR, 'OS-0-128-U1_v2.3.0_1024x10.pcap') metadata = client.SensorInfo(open(meta_file_path).read()) @@ -629,11 +639,16 @@ def test_validation(): metadata.format.udp_profile_lidar = client.UDPProfileLidar.PROFILE_LIDAR_FIVE_WORD_PIXEL reader = pcap.Pcap(pcap_file_path, metadata) consume(reader) - assert reader._errors == { - client.PacketIdError('Metadata init_id/sn does not match: expected by metadata - \ -123/122150000150, but got from packet buffer - 5431292/122150000150'): 64, - client.PacketSizeError('Expected a packet of size 41216 but got a buffer of size 8448'): 64 - } + # size error takes precedent + assert reader.size_error_count == 64 + assert reader.id_error_count == 0 + + metadata = client.SensorInfo(open(meta_file_path).read()) + metadata.init_id = 123 + reader = pcap.Pcap(pcap_file_path, metadata) + consume(reader) + assert reader.size_error_count == 0 + assert reader.id_error_count == 64 def test_legacy_reduced_json_data(): @@ -646,6 +661,20 @@ def test_legacy_reduced_json_data(): assert 1 == sum(1 for _ in scans) +def test_packet_reassembly(): + # make sure we get 1 valid packet/scan out of this file, ignoring the first fragment + meta_file_path = path.join(PCAPS_DATA_DIR, 'duplicate_id.json') + pcap_file_path = path.join(PCAPS_DATA_DIR, 'duplicate_id.pcap') + metadata = client.SensorInfo(open(meta_file_path).read()) + packet_source = pcap.Pcap(pcap_file_path, metadata) + scans = client.Scans(packet_source) + count = 0 + for s in scans: + count = count + 1 + assert s.frame_id == 1778 + assert count == 1 + + def test_empty_pcap_loop(): pcap_file_path = path.join(PCAPS_DATA_DIR, 'empty_pcap.pcap') diff --git a/python/tests/test_plugins.py b/python/tests/test_plugins.py index f76bba14..6eef98d5 100644 --- a/python/tests/test_plugins.py +++ b/python/tests/test_plugins.py @@ -20,7 +20,7 @@ def test_find_plugins(capsys, has_mapping): if has_mapping: built_in_plugins.extend([ 'ouster.cli.plugins.cli_mapping', - 'ouster.cli.plugins.cli_source_mapping', + 'ouster.cli.plugins.source_mapping', ]) assert set(built_in_plugins).issubset(plugin_names) diff --git a/python/tests/test_scan_source_slice.py b/python/tests/test_scan_source_slice.py new file mode 100644 index 00000000..886e9e89 --- /dev/null +++ b/python/tests/test_scan_source_slice.py @@ -0,0 +1,205 @@ +""" +Copyright (c) 2024, Ouster, Inc. +All rights reserved. + +This module verifies that a sliced ScanSource matches the original ScanSource +but limits the interaction with the source to the scope. +""" + +import os +import pytest +from ouster.sdk import open_source + +from tests.conftest import OSFS_DATA_DIR +from tests.conftest import PCAPS_DATA_DIR + +paths = [os.path.join(OSFS_DATA_DIR, "OS-1-128_v2.3.0_1024x10_lb_n3.osf"), + os.path.join(PCAPS_DATA_DIR, 'OS-1-128_v2.3.0_1024x10_lb_n3.pcap')] + +L = 3 # BOTH FILES USED ARE STRICTLY 3 + + +@pytest.fixture(params=paths) +def scan_source_path(request): + return request.param + + +@pytest.fixture +def sliceable_fixture(scan_source_path): + scan_source = open_source(scan_source_path, index=True, cycle=False) + ref_ids = [s.frame_id for s in scan_source] # register ref ids + return scan_source, ref_ids + + +@pytest.mark.parametrize("start, stop, step", [ + (0, L, None), + (0, L - 1, None), + (0, L - 2, None), + (0, L - 3, None), + (1, L, None), + (2, L, None), + (3, L, None), + (0, L + 1, None), + (0, L + L, None), + (0, -1, None), + (0, -2, None), + (0, -L, None), + (-1, L, None), + (-2, L, None), + (-L, L, None), + # step=1 is covered by None and ForwardSlicer tests + (0, L, 2), + (0, L, 3), + (0, L - 1, 2), + (0, L - 1, 3), + (1, L, 2), + (1, L, 3), + (1, L - 1, 2), + (1, L - 1, 3), +]) +def test_slicing_level_1(sliceable_fixture, start, stop, step) -> None: + scan_source, ref_ids = sliceable_fixture + s = slice(start, stop, step) + ss_slice = scan_source[s] + assert len(ss_slice) == len(ref_ids[s]) + sliced_ids = [s.frame_id for s in ss_slice] + assert sliced_ids == ref_ids[s] + + +@pytest.mark.parametrize("start, stop, step", [ + (0, L - 1, None), + (0, L - 2, None), + (0, L - 3, None), + (1, L, None), + (2, L, None), + (3, L, None), +]) +def test_slicing_level_2_from_full(sliceable_fixture, start, stop, step) -> None: + scan_source, ref_ids = sliceable_fixture + s1 = slice(None) + ss_l1 = scan_source[s1] + assert len(ss_l1) == L + s2 = slice(start, stop, step) + ss_l2 = ss_l1[s2] + assert len(ss_l2) == len(ref_ids[s1][s2]) + sliced_ids = [s.frame_id for s in ss_l2] + assert sliced_ids == ref_ids[s1][s2] + + +@pytest.mark.parametrize("start, stop, step", [ + (0, L - 1, None), + (0, L - 2, None), + (0, L - 3, None), + (1, L, None), + (2, L, None), + (3, L, None), +]) +def test_slicing_level_2_from_subset(sliceable_fixture, start, stop, step) -> None: + scan_source, ref_ids = sliceable_fixture + s1 = slice(1, L) + ss_l1 = scan_source[s1] + assert len(ss_l1) == L - 1 + s2 = slice(start, stop, step) + ss_l2 = ss_l1[s2] + assert len(ss_l2) == len(ref_ids[s1][s2]) + sliced_ids = [s.frame_id for s in ss_l2] + assert sliced_ids == ref_ids[s1][s2] + + +@pytest.mark.parametrize("start, stop, step", [ + (0, L, -1), + (L, 0, -1), + (L, 0, -2), +]) +def test_slicing_negative_step_throws(sliceable_fixture, start, stop, step) -> None: + scan_source, _ = sliceable_fixture + s = slice(start, stop, step) + with pytest.raises(TypeError) as ex: + _ = scan_source.slice(s) + assert str(ex.value) == "slice() can't work with negative step" + + +@pytest.mark.parametrize("start, stop, step", [ + (None, None, 0), + (None, 8, 0), + (3, None, 0), + (3, 8, 0), + (-3, -8, 0), +]) +def test_slicing_zero_step_throws(sliceable_fixture, start, stop, step): + scan_source, _ = sliceable_fixture + s = slice(start, stop, step) + with pytest.raises(ValueError) as ex: + _ = scan_source.slice(s) + assert str(ex.value) == "slice step cannot be zero" + + +def test_slicing_dual_slice(sliceable_fixture) -> None: + scan_source, ref_ids = sliceable_fixture + s1 = slice(0, 2) + ss_slice1 = scan_source.slice(s1) + assert len(ss_slice1) == len(ref_ids[s1]) + sliced_ids = [s.frame_id for s in ss_slice1] + assert sliced_ids == ref_ids[s1] + s2 = slice(1, 3) + ss_slice2 = scan_source.slice(s2) + assert len(ss_slice2) == len(ref_ids[s2]) + sliced_ids = [s.frame_id for s in ss_slice2] + assert sliced_ids == ref_ids[s2] + # revisit the first slice + sliced_ids = [s.frame_id for s in ss_slice1] + assert sliced_ids == ref_ids[s1] + + +@pytest.mark.parametrize("start, stop, step, idx", [ + (0, L, None, 0), + (0, L, None, 1), + (0, L, None, 2), + (0, L, None, -1), + (0, L, None, -2), + (1, L, None, 0), + (1, L, None, 1), + (1, L, None, -1), + (2, L, None, 0), +]) +def test_index_from_level_1_slice(sliceable_fixture, start, stop, step, idx) -> None: + scan_source, ref_ids = sliceable_fixture + s = slice(start, stop, step) + assert scan_source[s][idx].frame_id == ref_ids[s][idx] + + +@pytest.mark.parametrize("start, stop, step, idx", [ + (0, L, None, 0), + (0, L, None, 1), + (0, L, None, 2), + (0, L, None, -1), + (0, L, None, -2), + (1, L, None, 0), + (1, L, None, 1), + (1, L, None, -1), + (2, L, None, 0), +]) +def test_index_from_level_2_slice(sliceable_fixture, start, stop, step, idx) -> None: + scan_source, ref_ids = sliceable_fixture + s1 = slice(None) + ss_l1 = scan_source[s1] + assert len(ss_l1) == L + s2 = slice(start, stop, step) + ss_l2 = ss_l1[s2] + assert ss_l2[idx].frame_id == ref_ids[s1][s2][idx] + + +@pytest.mark.parametrize("start, stop, step, idx", [ + (0, L, None, L), + (0, L, None, L + 1), + (1, L, None, L - 1), + (1, L, None, L), + (2, L, None, L - 2), + (L, L, None, 0), +]) +def test_index_slicing_out_of_range_throws(sliceable_fixture, start, stop, step, idx): + scan_source, _ = sliceable_fixture + s = slice(start, stop, step) + with pytest.raises(IndexError) as ex: + _ = scan_source[s][idx] + assert str(ex.value) == "index is out of range" diff --git a/python/tests/test_sdk_utils.py b/python/tests/test_sdk_utils.py index cd9d71ff..ba9a2e9c 100644 --- a/python/tests/test_sdk_utils.py +++ b/python/tests/test_sdk_utils.py @@ -1,5 +1,8 @@ import pytest import tempfile +import os +from tests.conftest import PCAPS_DATA_DIR +from ouster.sdk.client import SensorInfo from os.path import commonprefix from pathlib import Path from ouster.sdk.util.metadata import resolve_metadata, \ @@ -74,16 +77,30 @@ def test_resolve_metadata_multi(): def test_resolve_metadata_multi_2(): """It should only files that exist and share a prefix with the data file.""" - with tempfile.TemporaryDirectory() as directory: - dir_path = Path(directory) - # create some test files (one representing data, one metadata) - - test_data_filename = 'tmpfile' - test_meta_filename = 'tmpfile' - open(dir_path / test_data_filename, 'a').close() - open(dir_path / f'{test_meta_filename}.json', 'a').close() - open(dir_path / f'{test_meta_filename}.2.json', 'a').close() - - assert set(resolve_metadata_multi(dir_path / test_data_filename)) == set([ - str(Path(dir_path) / f'{test_meta_filename}.json'), str(Path(dir_path) / f'{test_meta_filename}.2.json') - ]) + # read files with below prefix from PCAPS_DATA_DIR, the serial numbers for metadata on each is different + test_data_prefix = 'OS-0-128_v3.0.1_1024x10' + assert set(resolve_metadata_multi(os.path.join(PCAPS_DATA_DIR, f'{test_data_prefix}.pcap'))) == set( + [str(os.path.join(PCAPS_DATA_DIR, f'{test_data_prefix}.2.json')), + str(os.path.join(PCAPS_DATA_DIR, f'{test_data_prefix}.json'))]) + + +def test_resolve_metadata_multi_exception_raised(): + # read files with below prefix from PCAPS_DATA_DIR, the serial numbers for metadata on each is same + test_data_prefix = 'OS-0-128_v3.0.1_1024x10_20240321_125947' + test_data_filename = os.path.join(PCAPS_DATA_DIR, f'{test_data_prefix}.pcap') + + with open(os.path.join(PCAPS_DATA_DIR, f'{test_data_prefix}.json')) as file: + meta_content = file.read() + si = SensorInfo(meta_content) + metas = set([str(os.path.join(PCAPS_DATA_DIR, f'{test_data_prefix}.2.json')), + str(os.path.join(PCAPS_DATA_DIR, f'{test_data_prefix}.json'))]) + + with pytest.raises(RuntimeError) as excinfo: + resolve_metadata_multi(test_data_filename) + + s = ["The following metadata files identified for " + f"{test_data_filename} contain configuration for the same sensor {si.sn}. Files: " + f"{metas}", + "To resolve this, remove the extra metadata file(s) or specify the metadata " + "files manually using the --meta option."] + assert str(excinfo.value) == "\n".join(s) diff --git a/python/tox.ini b/python/tox.ini deleted file mode 100644 index 4d6bab84..00000000 --- a/python/tox.ini +++ /dev/null @@ -1,82 +0,0 @@ -[tox] -envlist = py{37,38,39,310,311} -isolated_build = true -skip_missing_interpreters = true -passenv = CCACHE_DIR, OUSTER_SDK_CMAKE_ARGS -parallel_show_output = true - -[testenv] -extras = test -allowlist_externals = mkdir -passenv = ARTIFACT_DIR, ID, VERSION_ID -setenv = - ARTIFACT_DIR = {env:ARTIFACT_DIR:./artifacts} - ID = {env:ID:none} - VERSION_ID = {env:VERSION_ID:none} -commands = - pytest tests/ -o junit_suite_name="ouster-sdk-{env:ID}-{env:VERSION_ID}-{envname}" \ - --junit-prefix="{env:ID}__{env:VERSION_ID}__{envname}" \ - --junitxml="{env:ARTIFACT_DIR}/tox-tests/ouster-sdk-{env:ID}-{env:VERSION_ID}-{envname}.xml" - -[testenv:py{37,38,39,310,311}-use_wheels] -description = installs ouster-sdk-python from wheels and runs tests -passenv = WHEELS_DIR -skipsdist = true -skip_install = true -parallel_show_output = true -commands = - pip install --force-reinstall --upgrade --pre -f {env:WHEELS_DIR} --no-index --no-cache-dir ouster-sdk[test] - pytest tests/ -o junit_suite_name="ouster-sdk-{env:ID}-{env:VERSION_ID}-{envname}" \ - --junit-prefix="{env:ID}__{env:VERSION_ID}__{envname}" \ - --junitxml="{env:ARTIFACT_DIR}/tox-tests/ouster-sdk-{env:ID}-{env:VERSION_ID}-{envname}.xml" - -[testenv:docs] -description = generating Ouster SDK documentaion html page (sphinx based) -extras = docs -commands = - sphinx-build --color -b html -d "{toxworkdir}/docs_doctree" "{toxinidir}/../docs" "{env:ARTIFACT_DIR}/docs" {posargs} - -[testenv:flake] -description = checking style with flake8 -skip_install = true -deps = - flake8 - flake8_formatter_junit_xml -commands = - mkdir -p {env:ARTIFACT_DIR}/lint - flake8 --format=junit-xml --output-file={env:ARTIFACT_DIR}/lint/flake.xml ./src ./tests - -[testenv:mypy] -description = check types with mypy -passenv = PIP_EXTRA_INDEX_URL -setenv = - PIP_EXTRA_INDEX_URL = {env:PIP_EXTRA_INDEX_URL:none} -deps = mypy - -commands = - mypy --junit-xml {env:ARTIFACT_DIR}/lint/mypy.xml ./src ./tests - -[flake8] -max-line-length = 120 -per-file-ignores = - tests/*: D - docs/*: D -ignore = - # E125, E126, E128 continuation line indentation, yapf doesn't fix this - E125, - E126, - E128, - # E251 newlines around equals in keywords, yapf again - E251, - # E731 assigning a lambda expression - E731, - # W503 line break before binary operator, yapf again - W503, - # W504 line break before binary operator, yapf again - W504, - # E741 "l" and "I" as variables names; not a problem with some static analysis - E741 - -[pydocstyle] -# used by flake8-docstrings plugin in flake env -convention = google diff --git a/test_package/CMakeLists.txt b/test_package/CMakeLists.txt new file mode 100644 index 00000000..1c710db7 --- /dev/null +++ b/test_package/CMakeLists.txt @@ -0,0 +1,9 @@ +cmake_minimum_required(VERSION 3.15) +project(PackageTest CXX) + +find_package(OusterSDK CONFIG REQUIRED) + + + +add_executable(example src/example.cpp) +target_link_libraries(example OusterSDK::ouster_client) diff --git a/test_package/conanfile.py b/test_package/conanfile.py new file mode 100644 index 00000000..a052d042 --- /dev/null +++ b/test_package/conanfile.py @@ -0,0 +1,26 @@ +import os + +from conan import ConanFile +from conan.tools.cmake import CMake, cmake_layout +from conan.tools.build import can_run + + +class ouster_sdkTestConan(ConanFile): + settings = "os", "compiler", "build_type", "arch" + generators = "CMakeDeps", "CMakeToolchain" + + def requirements(self): + self.requires(self.tested_reference_str) + + def build(self): + cmake = CMake(self) + cmake.configure() + cmake.build() + + def layout(self): + cmake_layout(self) + + def test(self): + if can_run(self): + cmd = os.path.join(self.cpp.build.bindir, "example") + self.run(cmd, env="conanrun") diff --git a/test_package/src/example.cpp b/test_package/src/example.cpp new file mode 100644 index 00000000..25fa63c2 --- /dev/null +++ b/test_package/src/example.cpp @@ -0,0 +1,4 @@ +#include +#include + +int main() { ouster::sensor::init_logger("info", "ouster.log"); } diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index faf7405a..a721c11c 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -169,3 +169,14 @@ set_tests_properties( ENVIRONMENT DATA_DIR=${CMAKE_CURRENT_LIST_DIR}/pcaps/ ) + +add_executable(array_view_test array_view_test.cpp) +target_link_libraries(array_view_test OusterSDK::ouster_client GTest::gtest GTest::gtest_main) +add_test(NAME array_view_test COMMAND array_view_test --gtest_output=xml:array_view_test.xml) + +add_executable(field_test field_test.cpp) +target_link_libraries(field_test OusterSDK::ouster_client GTest::gtest GTest::gtest_main) +add_test(NAME field_test COMMAND field_test --gtest_output=xml:field_test.xml) +if(NOT MSVC) + target_compile_options(field_test PRIVATE -Wno-unused-variable -Wno-unused-but-set-variable) +endif() diff --git a/tests/array_view_test.cpp b/tests/array_view_test.cpp new file mode 100644 index 00000000..4df4189c --- /dev/null +++ b/tests/array_view_test.cpp @@ -0,0 +1,458 @@ +/** + * Copyright (c) 2024, Ouster, Inc. + * All rights reserved. + */ + +#include "ouster/array_view.h" + +#include +#include + +#include "gtest/gtest.h" +#include "ouster/impl/idx_range.h" + +using namespace ouster; +using namespace ouster::impl; + +TEST(ArrayView_tests, args_restride_test) { + int32_t shape[4] = {1, 2, 3, 4}; + int32_t new_shape[2] = {0, 0}; + range_args_restride(shape, new_shape, 0, 0, idx_range{}, idx_range{}); + EXPECT_EQ(new_shape[0], shape[2]); + EXPECT_EQ(new_shape[1], shape[3]); + + range_args_restride(shape, new_shape, 0, idx_range{}, idx_range{}, 0); + EXPECT_EQ(new_shape[0], shape[1]); + EXPECT_EQ(new_shape[1], shape[2]); + + range_args_restride(shape, new_shape, 0, idx_range{}, 0, idx_range{}); + EXPECT_EQ(new_shape[0], shape[1]); + EXPECT_EQ(new_shape[1], shape[3]); + + range_args_restride(shape, new_shape, idx_range{}, 0, 0, idx_range{}); + EXPECT_EQ(new_shape[0], shape[0]); + EXPECT_EQ(new_shape[1], shape[3]); + + range_args_restride(shape, new_shape, 0, 0); + EXPECT_EQ(new_shape[0], shape[2]); + EXPECT_EQ(new_shape[1], shape[3]); + + range_args_restride(shape, new_shape, idx_range{}, 0, 0); + EXPECT_EQ(new_shape[0], shape[0]); + EXPECT_EQ(new_shape[1], shape[3]); + + int32_t n_shape[3] = {0, 0, 0}; + range_args_restride(shape, n_shape, idx_range{}, idx_range{}, idx_range{}, + 15); + EXPECT_EQ(n_shape[0], shape[0]); + EXPECT_EQ(n_shape[1], shape[1]); + EXPECT_EQ(n_shape[2], shape[2]); +} + +TEST(ArrayView_tests, args_restride_vector_test) { + std::vector shape = {1, 2, 3, 4}; + std::vector new_shape; + new_shape = range_args_restride(shape, 0, 0, idx_range{}, idx_range{}); + EXPECT_EQ(new_shape.size(), 2); + EXPECT_EQ(new_shape[0], shape[2]); + EXPECT_EQ(new_shape[1], shape[3]); + + new_shape = range_args_restride(shape, 0, idx_range{}, idx_range{}, 0); + EXPECT_EQ(new_shape.size(), 2); + EXPECT_EQ(new_shape[0], shape[1]); + EXPECT_EQ(new_shape[1], shape[2]); + + new_shape = range_args_restride(shape, 0, idx_range{}, 0, idx_range{}); + EXPECT_EQ(new_shape.size(), 2); + EXPECT_EQ(new_shape[0], shape[1]); + EXPECT_EQ(new_shape[1], shape[3]); + + new_shape = range_args_restride(shape, idx_range{}, 0, 0, idx_range{}); + EXPECT_EQ(new_shape.size(), 2); + EXPECT_EQ(new_shape[0], shape[0]); + EXPECT_EQ(new_shape[1], shape[3]); + + new_shape = range_args_restride(shape, 0, 0); + EXPECT_EQ(new_shape.size(), 2); + EXPECT_EQ(new_shape[0], shape[2]); + EXPECT_EQ(new_shape[1], shape[3]); + + new_shape = range_args_restride(shape, idx_range{}, 0, 0); + EXPECT_EQ(new_shape.size(), 2); + EXPECT_EQ(new_shape[0], shape[0]); + EXPECT_EQ(new_shape[1], shape[3]); + + new_shape = + range_args_restride(shape, idx_range{}, idx_range{}, idx_range{}, 15); + EXPECT_EQ(new_shape.size(), 3); + EXPECT_EQ(new_shape[0], shape[0]); + EXPECT_EQ(new_shape[1], shape[1]); + EXPECT_EQ(new_shape[2], shape[2]); +} + +TEST(ArrayView_tests, args_reshape_test) { + int32_t shape[4] = {10, 12, 14, 16}; + int32_t new_shape[2] = {0, 0}; + EXPECT_NO_THROW(range_args_reshape(shape, new_shape, idx_range{}, 0, 0)); + EXPECT_EQ(new_shape[0], shape[0]); + EXPECT_EQ(new_shape[1], shape[3]); + EXPECT_NO_THROW( + range_args_reshape(shape, new_shape, idx_range{2, 3}, 0, 0)); + EXPECT_EQ(new_shape[0], 1); + EXPECT_EQ(new_shape[1], shape[3]); + EXPECT_NO_THROW( + range_args_reshape(shape, new_shape, idx_range{2, 8}, 0, 0)); + EXPECT_EQ(new_shape[0], 6); + EXPECT_EQ(new_shape[1], shape[3]); + EXPECT_THROW(range_args_reshape(shape, new_shape, idx_range{2, 2}, 0, 0), + std::runtime_error); + EXPECT_THROW(range_args_reshape(shape, new_shape, idx_range{2, 1}, 0, 0), + std::runtime_error); + EXPECT_THROW(range_args_reshape(shape, new_shape, idx_range{2, 11}, 0, 0), + std::runtime_error); +} + +TEST(ArrayView_tests, args_reshape_vector_test) { + std::vector shape = {10, 12, 14, 16}; + std::vector new_shape; + EXPECT_NO_THROW(new_shape = range_args_reshape(shape, idx_range{}, 0, 0)); + EXPECT_EQ(new_shape.size(), 2); + EXPECT_EQ(new_shape[0], shape[0]); + EXPECT_EQ(new_shape[1], shape[3]); + EXPECT_NO_THROW(new_shape = + range_args_reshape(shape, idx_range{2, 3}, 0, 0)); + EXPECT_EQ(new_shape.size(), 2); + EXPECT_EQ(new_shape[0], 1); + EXPECT_EQ(new_shape[1], shape[3]); + EXPECT_NO_THROW(new_shape = + range_args_reshape(shape, idx_range{2, 8}, 0, 0)); + EXPECT_EQ(new_shape.size(), 2); + EXPECT_EQ(new_shape[0], 6); + EXPECT_EQ(new_shape[1], shape[3]); + EXPECT_THROW(new_shape = range_args_reshape(shape, idx_range{2, 2}, 0, 0), + std::runtime_error); + EXPECT_THROW(new_shape = range_args_reshape(shape, idx_range{2, 1}, 0, 0), + std::runtime_error); + EXPECT_THROW(new_shape = range_args_reshape(shape, idx_range{2, 11}, 0, 0), + std::runtime_error); +} + +TEST(ArrayView_tests, constructor_test) { + int v = 10; + { + auto a = ArrayView4(&v, {2, 3, 4, 5}); + + EXPECT_EQ(a.shape[0], 2); + EXPECT_EQ(a.shape[1], 3); + EXPECT_EQ(a.shape[2], 4); + EXPECT_EQ(a.shape[3], 5); + + EXPECT_EQ(a.strides[0], 3 * 4 * 5); + EXPECT_EQ(a.strides[1], 4 * 5); + EXPECT_EQ(a.strides[2], 5); + } + { + std::vector src = {2.f, 3.f, 4.f, 5.f}; + + auto a = ArrayView4(&v, src); + + EXPECT_EQ(a.shape[0], 2); + EXPECT_EQ(a.shape[1], 3); + EXPECT_EQ(a.shape[2], 4); + EXPECT_EQ(a.shape[3], 5); + + EXPECT_EQ(a.strides[0], 3 * 4 * 5); + EXPECT_EQ(a.strides[1], 4 * 5); + EXPECT_EQ(a.strides[2], 5); + } + { + std::list src = {2, 3, 4, 5}; + + auto a = ArrayView4(&v, src); + + EXPECT_EQ(a.shape[0], 2); + EXPECT_EQ(a.shape[1], 3); + EXPECT_EQ(a.shape[2], 4); + EXPECT_EQ(a.shape[3], 5); + + EXPECT_EQ(a.strides[0], 3 * 4 * 5); + EXPECT_EQ(a.strides[1], 4 * 5); + EXPECT_EQ(a.strides[2], 5); + } + { + std::vector shape = {2, 3, 4, 5}; + std::vector strides = {3 * 4 * 5, 4 * 5, 5, 1}; + + auto a = ArrayView4(&v, shape, strides); + + EXPECT_EQ(a.shape[0], 2); + EXPECT_EQ(a.shape[1], 3); + EXPECT_EQ(a.shape[2], 4); + EXPECT_EQ(a.shape[3], 5); + + EXPECT_EQ(a.strides[0], 3 * 4 * 5); + EXPECT_EQ(a.strides[1], 4 * 5); + EXPECT_EQ(a.strides[2], 5); + EXPECT_EQ(a.strides[3], 1); + } +} + +TEST(ArrayView_tests, indexing_test) { + std::vector shape = {5, 6, 7}; + + std::vector src(5 * 6 * 7, 0); + for (size_t i = 0; i < src.size(); ++i) src[i] = i; + + auto a = ArrayView3(src.data(), shape); + + int i = 0; + for (int x = 0; x < a.shape[0]; ++x) + for (int y = 0; y < a.shape[1]; ++y) + for (int z = 0; z < a.shape[2]; ++z) EXPECT_EQ(a(x, y, z), i++); + + auto b = ArrayView1(src.data(), {5 * 6 * 7}); + for (size_t i = 0; i < src.size(); ++i) EXPECT_EQ(b(i), i); +} + +TEST(ArrayView_tests, assignment_test) { + std::vector shape = {5, 6, 7}; + + std::vector src(5 * 6 * 7, 10); + + auto a = ArrayView3(src.data(), shape); + + int i = 0; + for (int x = 0; x < a.shape[0]; ++x) + for (int y = 0; y < a.shape[1]; ++y) + for (int z = 0; z < a.shape[2]; ++z) a(x, y, z) = i++; + + for (size_t j = 0; j < src.size(); ++j) EXPECT_EQ(src[j], j); +} + +TEST(ArrayView_tests, subview_test) { + // single subview test + std::vector src(100 * 100 * 2, 0); + + ArrayView3 a(src.data(), {2, 100, 100}); + for (size_t i = 0; i < src.size() / 2; ++i) { + src[i] = 1; + } + for (size_t i = src.size() / 2; i < src.size(); ++i) { + src[i] = 2; + } + + auto sub0 = a.subview(0); + auto sub1 = a.subview(1); + EXPECT_THROW(a.subview(2), std::invalid_argument); + EXPECT_THROW(a.subview(1, 145), std::invalid_argument); + EXPECT_THROW(a.subview(3, 200), std::invalid_argument); + EXPECT_THROW(a.subview(3, 80), std::invalid_argument); + + EXPECT_EQ(sub0.shape[0], a.shape[1]); + EXPECT_EQ(sub0.shape[1], a.shape[2]); + EXPECT_EQ(sub0.strides[0], a.strides[1]); + + for (int i = 0; i < 100; ++i) { + for (int j = 0; j < 100; ++j) { + EXPECT_EQ(sub0(i, j), 1); + EXPECT_EQ(sub1(i, j), 2); + } + } + + ArrayView3 b(src.data(), {100, 20, 10}); + for (int i = 0; i < 100; ++i) { + for (int j = 0; j < 20; ++j) { + auto c = b.subview(i, j); + for (int k = 0; k < 10; ++k) { + c(k) = i * 20 * 10 + j * 10 + k; + } + } + } + + for (size_t i = 0; i < src.size(); ++i) EXPECT_EQ(src[i], i); +} + +TEST(ArrayView_tests, window_subview_test) { + std::vector src(100 * 100); + + int i = 0; + for (auto& v : src) v = i++; + + ArrayView2 view{src.data(), {100, 100}}; + ArrayView2 subview = view.subview(keep(10, 20), keep(30, 45)); + + EXPECT_EQ(subview.shape[0], 10); + EXPECT_EQ(subview.shape[1], 15); + EXPECT_EQ(subview.strides[0], view.strides[0]); + EXPECT_EQ(subview.strides[1], view.strides[1]); + + for (int y = 10; y < 20; ++y) + for (int x = 30; x < 45; ++x) + EXPECT_EQ(view(y, x), subview(y - 10, x - 30)); +} + +TEST(ArrayView_tests, sparse_subview_test) { + std::vector v(20 * 320 * 456 * 18); + + for (size_t i = 0; i < v.size(); ++i) { + v[i] = i % 18; + } + + ArrayView4 view(v.data(), {20, 320, 456, 18}); + + ArrayView3 bin16 = view.subview(keep(), keep(), keep(), 15); + EXPECT_EQ(bin16.shape[0], view.shape[0]); + EXPECT_EQ(bin16.shape[1], view.shape[1]); + EXPECT_EQ(bin16.shape[2], view.shape[2]); + EXPECT_EQ(bin16.strides[0], view.strides[0]); + EXPECT_EQ(bin16.strides[1], view.strides[1]); + EXPECT_EQ(bin16.strides[2], view.strides[2]); + + for (int ss = 0; ss < 20; ++ss) { + for (int xx = 0; xx < 320; ++xx) { + for (int yy = 0; yy < 456; ++yy) { + EXPECT_EQ(bin16(ss, xx, yy), 15); + } + } + } + + for (auto&& val : v) { + val = 0; + } + + auto w310 = view.subview(keep(), keep(), 310, keep()); + EXPECT_EQ(w310.shape[0], view.shape[0]); + EXPECT_EQ(w310.shape[1], view.shape[1]); + EXPECT_EQ(w310.shape[2], view.shape[3]); + EXPECT_EQ(w310.strides[0], view.strides[0]); + EXPECT_EQ(w310.strides[1], view.strides[1]); + EXPECT_EQ(w310.strides[2], view.strides[3]); + + for (int ss = 0; ss < 20; ++ss) { + for (int xx = 0; xx < 320; ++xx) { + for (int bb = 0; bb < 18; ++bb) { + w310(ss, xx, bb) = -10; + EXPECT_EQ(w310(ss, xx, bb), view(ss, xx, 310, bb)); + } + } + } +} + +TEST(ArrayView_tests, sparse_test) { + ConstArrayView4 view{nullptr, {20, 320, 456, 18}}; + + EXPECT_EQ(view.sparse(), false); + EXPECT_EQ(view.subview(0).sparse(), false); + EXPECT_EQ(view.subview(1).sparse(), false); + EXPECT_EQ(view.subview(3).sparse(), false); + EXPECT_EQ(view.subview(3, 5).sparse(), false); + EXPECT_EQ(view.subview(3, 5, 10).sparse(), false); + + EXPECT_EQ(view.subview(keep(), keep(), keep(), 10).sparse(), true); + EXPECT_EQ(view.subview(keep(), 200, 134, 10).sparse(), true); + EXPECT_EQ(view.subview(keep(), 2, keep(), 10).sparse(), true); + EXPECT_EQ(view.subview(1, keep(), keep(), 10).sparse(), true); + EXPECT_EQ(view.subview(1, 2, keep(), 10).sparse(), true); + EXPECT_EQ(view.subview(keep(), keep(), 10).sparse(), true); + EXPECT_EQ(view.subview(1, keep(), 10).sparse(), true); + EXPECT_EQ(view.subview(keep(), 10).sparse(), true); + EXPECT_EQ(view.subview(keep(), 10, 5).sparse(), true); + EXPECT_EQ(view.subview(keep(), 10, 5, 3).sparse(), true); +} + +TEST(ArrayView_tests, reshape_test) { + std::vector vec(100 * 200 * 300); + ConstArrayView3 view{vec.data(), {100, 200, 300}}; + + EXPECT_THROW(view.reshape(3, 9, 12, 10), std::invalid_argument); + EXPECT_THROW(view.reshape(3, 10), std::invalid_argument); + EXPECT_THROW(view.subview(keep(), 2).reshape(50, 2, 300), + std::invalid_argument); + EXPECT_NO_THROW(view.subview(2).reshape(100, 2, 300)); + + { + int i = 0; + for (auto& v : vec) v = i++; + } + { + ConstArrayView2 reshaped = view.reshape(200 * 100, 300); + + int i = 0; + for (int a = 0; a < reshaped.shape[0]; ++a) + for (int b = 0; b < reshaped.shape[1]; ++b) + EXPECT_EQ(reshaped(a, b), i++); + } + { + ConstArrayView2 reshaped = view.reshape(2, 100 * 100 * 300); + + int i = 0; + for (int a = 0; a < reshaped.shape[0]; ++a) + for (int b = 0; b < reshaped.shape[1]; ++b) + EXPECT_EQ(reshaped(a, b), i++); + } + { + ConstArrayView2 reshaped = view.reshape(100 * 50, 4 * 300); + + int i = 0; + for (int a = 0; a < reshaped.shape[0]; ++a) + for (int b = 0; b < reshaped.shape[1]; ++b) + EXPECT_EQ(reshaped(a, b), i++); + } + { + ConstArrayView3 reshaped = view.reshape(200 * 10, 5, 2 * 300); + + int i = 0; + for (int a = 0; a < reshaped.shape[0]; ++a) + for (int b = 0; b < reshaped.shape[1]; ++b) + for (int c = 0; c < reshaped.shape[2]; ++c) + EXPECT_EQ(reshaped(a, b, c), i++); + } + { + ConstArrayView4 reshaped = view.reshape(2, 50, 200, 300); + + int i = 0; + for (int a = 0; a < reshaped.shape[0]; ++a) + for (int b = 0; b < reshaped.shape[1]; ++b) + for (int c = 0; c < reshaped.shape[2]; ++c) + for (int d = 0; d < reshaped.shape[3]; ++d) + EXPECT_EQ(reshaped(a, b, c, d), i++); + } + { + ConstArrayView4 reshaped = view.reshape(100, 50, 4, 300); + + int i = 0; + for (int a = 0; a < reshaped.shape[0]; ++a) + for (int b = 0; b < reshaped.shape[1]; ++b) + for (int c = 0; c < reshaped.shape[2]; ++c) + for (int d = 0; d < reshaped.shape[3]; ++d) + EXPECT_EQ(reshaped(a, b, c, d), i++); + } + { + ConstArrayView4 reshaped = view.reshape(100, 200, 100, 3); + + int i = 0; + for (int a = 0; a < reshaped.shape[0]; ++a) + for (int b = 0; b < reshaped.shape[1]; ++b) + for (int c = 0; c < reshaped.shape[2]; ++c) + for (int d = 0; d < reshaped.shape[3]; ++d) + EXPECT_EQ(reshaped(a, b, c, d), i++); + } +} + +TEST(ArrayView_tests, iterator_test) { + std::vector vec(20 * 320 * 456 * 18); + + ArrayView4 view{vec.data(), {20, 320, 456, 18}}; + ConstArrayView4 cview{vec.data(), {20, 320, 456, 18}}; + + EXPECT_THROW(view.subview(1, keep(), 10).begin(), std::logic_error); + EXPECT_THROW(view.subview(1, keep(), 10).end(), std::logic_error); + EXPECT_THROW(cview.subview(1, keep(), 10).begin(), std::logic_error); + EXPECT_THROW(cview.subview(1, keep(), 10).end(), std::logic_error); + + int i = 0; + for (auto& v : view) v = i++; + + i = 0; + for (auto& v : vec) EXPECT_EQ(v, i++); +} diff --git a/tests/bcompat_meta_json_test.cpp b/tests/bcompat_meta_json_test.cpp index 73262e22..5b784a34 100644 --- a/tests/bcompat_meta_json_test.cpp +++ b/tests/bcompat_meta_json_test.cpp @@ -23,7 +23,7 @@ inline std::string getenvs(const std::string& var) { return res ? std::string{res} : std::string{}; } -void sinfo_populator(sensor_info& info, const std::string& name, +void sinfo_populator(sensor_info& info, const std::string& /*name*/, const std::string& sn, const std::string& fw_rev, const lidar_mode mode, const std::string& prod_line, const data_format& format, @@ -34,15 +34,14 @@ void sinfo_populator(sensor_info& info, const std::string& name, const ouster::mat4d& imu_to_sensor_transform, const ouster::mat4d& lidar_to_sensor_transform, const ouster::mat4d& extrinsic, const int init_id, - const int udp_port_lidar, const int udp_port_imu, + const nonstd::optional udp_port_lidar, + const nonstd::optional udp_port_imu, const std::string& build_date, const std::string& image_rev, const std::string& prod_pn, const std::string& status, const calibration_status& cal, const sensor_config& config) { - info.name = name; info.sn = sn; info.fw_rev = fw_rev; - info.mode = mode; info.prod_line = prod_line; info.format = format; info.beam_azimuth_angles = beam_azimuth_angles; @@ -53,14 +52,15 @@ void sinfo_populator(sensor_info& info, const std::string& name, info.lidar_to_sensor_transform = lidar_to_sensor_transform; info.extrinsic = extrinsic; info.init_id = init_id; - info.udp_port_lidar = udp_port_lidar; - info.udp_port_imu = udp_port_imu; info.build_date = build_date; info.image_rev = image_rev; info.prod_pn = prod_pn; info.status = status; info.cal = cal; info.config = config; + info.config.lidar_mode = mode; + info.config.udp_port_lidar = udp_port_lidar; + info.config.udp_port_imu = udp_port_imu; } static sensor_info si_1_12_os1_991913000010_64; @@ -148,8 +148,8 @@ class MetaJsonTest : public testing::TestWithParam { config.operating_mode = OperatingMode::OPERATING_NORMAL; config.multipurpose_io_mode = MultipurposeIOMode::MULTIPURPOSE_OFF; config.nmea_baud_rate = NMEABaudRate::BAUD_9600; - config.ld_mode = lidar_mode::MODE_1024x10; - config.ts_mode = timestamp_mode::TIME_FROM_INTERNAL_OSC; + config.lidar_mode = lidar_mode::MODE_1024x10; + config.timestamp_mode = timestamp_mode::TIME_FROM_INTERNAL_OSC; config.udp_port_imu = 60023; config.udp_port_lidar = 60823; config.sync_pulse_in_polarity = Polarity::POLARITY_ACTIVE_HIGH; @@ -200,8 +200,8 @@ class MetaJsonTest : public testing::TestWithParam { mkmat4d({-1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 1, 36.18, 0, 0, 0, 1, }), mkmat4d({1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, }), 0, - 0, - 0, + {}, + {}, "2019-05-02T20:17:32Z", "ousteros-image-prod-aries-v1.12.0-20190502211317", "840-101855-02", @@ -225,8 +225,8 @@ class MetaJsonTest : public testing::TestWithParam { mkmat4d({-1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 1, 36.18, 0, 0, 0, 1, }), mkmat4d({1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, }), 0, - 0, - 0, + {}, + {}, "2019-05-02T20:17:32Z", "ousteros-image-prod-aries-v1.12.0-20190502211317", "840-101855-02", @@ -238,7 +238,7 @@ class MetaJsonTest : public testing::TestWithParam { config = sensor_config{}; config.operating_mode = OperatingMode::OPERATING_NORMAL; config.azimuth_window = std::make_pair(0, 36000); - config.ld_mode = lidar_mode::MODE_1024x10; + config.lidar_mode = lidar_mode::MODE_1024x10; config.multipurpose_io_mode = MultipurposeIOMode::MULTIPURPOSE_OFF; config.nmea_baud_rate = NMEABaudRate::BAUD_9600; config.nmea_ignore_valid_char = false; @@ -249,7 +249,7 @@ class MetaJsonTest : public testing::TestWithParam { config.sync_pulse_out_frequency = 1; config.sync_pulse_out_polarity = Polarity::POLARITY_ACTIVE_HIGH; config.sync_pulse_out_pulse_width = 10; - config.ts_mode = timestamp_mode::TIME_FROM_INTERNAL_OSC; + config.timestamp_mode = timestamp_mode::TIME_FROM_INTERNAL_OSC; config.udp_dest="169.254.91.92"; config.udp_port_imu = 7503; config.udp_port_lidar = 7502; @@ -293,8 +293,8 @@ class MetaJsonTest : public testing::TestWithParam { mkmat4d({-1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 1, 36.18, 0, 0, 0, 1, }), mkmat4d({1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, }), 0, - 0, - 0, + {}, + {}, "2019-11-04T23:58:36Z", "ousteros-image-prod-aries-v1.13.0-20191105025459", "840-101855-02", @@ -318,8 +318,8 @@ class MetaJsonTest : public testing::TestWithParam { mkmat4d({-1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 1, 36.18, 0, 0, 0, 1, }), mkmat4d({1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, }), 0, - 0, - 0, + {}, + {}, "2019-11-04T23:58:36Z", "ousteros-image-prod-aries-v1.13.0-20191105025459", "840-101855-02", @@ -343,8 +343,8 @@ class MetaJsonTest : public testing::TestWithParam { mkmat4d({-1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 1, 36.18, 0, 0, 0, 1, }), mkmat4d({1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, }), 0, - 0, - 0, + {}, + {}, "2020-02-13T17:22:02Z", "ousteros-image-prod-aries-v1.14.0-beta.1+ci+git+beta2@6cccd783ea+dev_fw_PR-884-20200213171907-staging", "840-102144-A", @@ -368,8 +368,8 @@ class MetaJsonTest : public testing::TestWithParam { mkmat4d({-1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 1, 36.18, 0, 0, 0, 1, }), mkmat4d({1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, }), 0, - 0, - 0, + {}, + {}, "2020-02-13T17:22:02Z", "ousteros-image-prod-aries-v1.14.0-beta.1+ci+git+beta2@6cccd783ea+dev_fw_PR-884-20200213171907-staging", "840-102144-A", @@ -393,8 +393,8 @@ class MetaJsonTest : public testing::TestWithParam { mkmat4d({-1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 1, 36.18, 0, 0, 0, 1, }), mkmat4d({1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, }), 0, - 0, - 0, + {}, + {}, "2020-02-11T00:14:48Z", "ousteros-image-prod-aries-v1.14.0-beta.1+ci+git+master@de6f92cb16-20200211001242-staging", "840-101855-02", @@ -418,8 +418,8 @@ class MetaJsonTest : public testing::TestWithParam { mkmat4d({-1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 1, 36.18, 0, 0, 0, 1, }), mkmat4d({1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, }), 0, - 0, - 0, + {}, + {}, "2020-02-11T00:14:48Z", "ousteros-image-prod-aries-v1.14.0-beta.1+ci+git+master@de6f92cb16-20200211001242-staging", "840-101855-02", @@ -443,8 +443,8 @@ class MetaJsonTest : public testing::TestWithParam { mkmat4d({-1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 1, 36.18, 0, 0, 0, 1, }), mkmat4d({1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, }), 0, - 0, - 0, + {}, + {}, "", "", "", @@ -468,8 +468,8 @@ class MetaJsonTest : public testing::TestWithParam { mkmat4d({-1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 1, 36.18, 0, 0, 0, 1, }), mkmat4d({1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, }), 0, - 0, - 0, + {}, + {}, "2020-10-23T14:05:18Z", "ousteros-image-prod-aries-v2.0.0-rc.2+20201023140416.staging", "840-102145-B", @@ -493,8 +493,8 @@ class MetaJsonTest : public testing::TestWithParam { mkmat4d({-1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 1, 36.18, 0, 0, 0, 1, }), mkmat4d({1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, }), 0, - 0, - 0, + {}, + {}, "2020-11-24T06:51:26Z", "ousteros-image-prod-aries-v2.0.0+20201124065024", "840-102145-A", @@ -506,7 +506,7 @@ class MetaJsonTest : public testing::TestWithParam { config = sensor_config{}; config.operating_mode = OperatingMode::OPERATING_NORMAL; config.azimuth_window = std::make_pair(0, 360000); - config.ld_mode = lidar_mode::MODE_1024x10; + config.lidar_mode = lidar_mode::MODE_1024x10; config.multipurpose_io_mode = MultipurposeIOMode::MULTIPURPOSE_OFF; config.nmea_baud_rate = NMEABaudRate::BAUD_9600; config.nmea_ignore_valid_char = false; @@ -519,7 +519,7 @@ class MetaJsonTest : public testing::TestWithParam { config.sync_pulse_out_frequency = 1; config.sync_pulse_out_polarity = Polarity::POLARITY_ACTIVE_HIGH; config.sync_pulse_out_pulse_width = 10; - config.ts_mode = timestamp_mode::TIME_FROM_INTERNAL_OSC; + config.timestamp_mode = timestamp_mode::TIME_FROM_INTERNAL_OSC; config.udp_dest="169.254.91.92"; config.udp_port_imu = 55824; config.udp_port_lidar = 55426; @@ -563,8 +563,8 @@ class MetaJsonTest : public testing::TestWithParam { mkmat4d({ -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 1, 36.18, 0, 0, 0, 1,}), mkmat4d({1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, }), 0, - 0, - 0, + {}, + {}, "2021-07-27T22:34:53Z", "ousteros-image-prod-aries-v2.1.2+20210727223313.patch-v2.1.2", "840-101396-03", @@ -576,7 +576,7 @@ class MetaJsonTest : public testing::TestWithParam { config = sensor_config{}; config.operating_mode = OperatingMode::OPERATING_NORMAL; config.azimuth_window = std::make_pair(0, 360000); - config.ld_mode = lidar_mode::MODE_1024x10; + config.lidar_mode = lidar_mode::MODE_1024x10; config.multipurpose_io_mode = MultipurposeIOMode::MULTIPURPOSE_OFF; config.nmea_baud_rate = NMEABaudRate::BAUD_9600; config.nmea_ignore_valid_char = false; @@ -590,7 +590,7 @@ class MetaJsonTest : public testing::TestWithParam { config.sync_pulse_out_frequency = 1; config.sync_pulse_out_polarity = Polarity::POLARITY_ACTIVE_HIGH; config.sync_pulse_out_pulse_width = 10; - config.ts_mode = timestamp_mode::TIME_FROM_INTERNAL_OSC; + config.timestamp_mode = timestamp_mode::TIME_FROM_INTERNAL_OSC; config.udp_dest=""; config.udp_port_imu = 7503; config.udp_port_lidar = 7502; @@ -648,7 +648,7 @@ class MetaJsonTest : public testing::TestWithParam { config.operating_mode = OperatingMode::OPERATING_NORMAL; config.azimuth_window = std::make_pair(0, 360000); config.columns_per_packet = 16; - config.ld_mode = lidar_mode::MODE_1024x10; + config.lidar_mode = lidar_mode::MODE_1024x10; config.multipurpose_io_mode = MultipurposeIOMode::MULTIPURPOSE_OFF; config.nmea_baud_rate = NMEABaudRate::BAUD_9600; config.nmea_ignore_valid_char = false; @@ -662,7 +662,7 @@ class MetaJsonTest : public testing::TestWithParam { config.sync_pulse_out_frequency = 1; config.sync_pulse_out_polarity = Polarity::POLARITY_ACTIVE_HIGH; config.sync_pulse_out_pulse_width = 10; - config.ts_mode = timestamp_mode::TIME_FROM_INTERNAL_OSC; + config.timestamp_mode = timestamp_mode::TIME_FROM_INTERNAL_OSC; config.udp_dest="192.168.88.254"; config.udp_port_imu = 7503; config.udp_port_lidar = 7502; @@ -722,7 +722,7 @@ class MetaJsonTest : public testing::TestWithParam { config.operating_mode = OperatingMode::OPERATING_NORMAL; config.azimuth_window = std::make_pair(0, 360000); config.columns_per_packet = 16; - config.ld_mode = lidar_mode::MODE_1024x10; + config.lidar_mode = lidar_mode::MODE_1024x10; config.multipurpose_io_mode = MultipurposeIOMode::MULTIPURPOSE_OFF; config.nmea_baud_rate = NMEABaudRate::BAUD_9600; config.nmea_ignore_valid_char = false; @@ -736,7 +736,7 @@ class MetaJsonTest : public testing::TestWithParam { config.sync_pulse_out_frequency = 1; config.sync_pulse_out_polarity = Polarity::POLARITY_ACTIVE_HIGH; config.sync_pulse_out_pulse_width = 10; - config.ts_mode = timestamp_mode::TIME_FROM_INTERNAL_OSC; + config.timestamp_mode = timestamp_mode::TIME_FROM_INTERNAL_OSC; config.udp_dest="192.168.1.80"; config.udp_port_imu = 7503; config.udp_port_lidar = 7502; @@ -770,7 +770,7 @@ class MetaJsonTest : public testing::TestWithParam { config = sensor_config{}; config.azimuth_window = std::make_pair(0, 360000); config.columns_per_packet = 16; - config.ld_mode = lidar_mode::MODE_1024x10; + config.lidar_mode = lidar_mode::MODE_1024x10; config.multipurpose_io_mode = MultipurposeIOMode::MULTIPURPOSE_INPUT_NMEA_UART; config.nmea_baud_rate = NMEABaudRate::BAUD_115200; config.nmea_ignore_valid_char = false; @@ -785,7 +785,7 @@ class MetaJsonTest : public testing::TestWithParam { config.sync_pulse_out_frequency = 10; config.sync_pulse_out_polarity = Polarity::POLARITY_ACTIVE_HIGH; config.sync_pulse_out_pulse_width = 10; - config.ts_mode = timestamp_mode::TIME_FROM_SYNC_PULSE_IN; + config.timestamp_mode = timestamp_mode::TIME_FROM_SYNC_PULSE_IN; config.udp_dest="10.0.0.167"; config.udp_port_imu = 7503; config.udp_port_lidar = 7502; @@ -869,7 +869,7 @@ class MetaJsonTest : public testing::TestWithParam { config = sensor_config{}; config.azimuth_window = std::make_pair(0, 360000); config.columns_per_packet = 16; - config.ld_mode = lidar_mode::MODE_1024x10; + config.lidar_mode = lidar_mode::MODE_1024x10; config.multipurpose_io_mode = MultipurposeIOMode::MULTIPURPOSE_OFF; config.nmea_baud_rate = NMEABaudRate::BAUD_9600; config.nmea_ignore_valid_char = false; @@ -884,7 +884,7 @@ class MetaJsonTest : public testing::TestWithParam { config.sync_pulse_out_frequency = 1; config.sync_pulse_out_polarity = Polarity::POLARITY_ACTIVE_HIGH; config.sync_pulse_out_pulse_width = 10; - config.ts_mode = timestamp_mode::TIME_FROM_INTERNAL_OSC; + config.timestamp_mode = timestamp_mode::TIME_FROM_INTERNAL_OSC; config.udp_dest="10.0.0.167"; config.udp_port_imu = 7503; config.udp_port_lidar = 7502; @@ -943,7 +943,7 @@ class MetaJsonTest : public testing::TestWithParam { config = sensor_config{}; config.azimuth_window = std::make_pair(583, 39402); config.columns_per_packet = 16; - config.ld_mode = lidar_mode::MODE_1024x10; + config.lidar_mode = lidar_mode::MODE_1024x10; config.multipurpose_io_mode = MultipurposeIOMode::MULTIPURPOSE_OFF; config.nmea_baud_rate = NMEABaudRate::BAUD_9600; config.nmea_ignore_valid_char = false; @@ -958,7 +958,7 @@ class MetaJsonTest : public testing::TestWithParam { config.sync_pulse_out_frequency = 1; config.sync_pulse_out_polarity = Polarity::POLARITY_ACTIVE_HIGH; config.sync_pulse_out_pulse_width = 10; - config.ts_mode = timestamp_mode::TIME_FROM_INTERNAL_OSC; + config.timestamp_mode = timestamp_mode::TIME_FROM_INTERNAL_OSC; config.udp_dest="10.0.0.167"; config.udp_port_imu = 7503; config.udp_port_lidar = 7502; @@ -1033,15 +1033,14 @@ TEST_P(MetaJsonTest, MetadataFromJson) { auto data_dir = getenvs("DATA_DIR"); // parse json file - const sensor_info si = metadata_from_json(data_dir + "/" + param + ".json"); + sensor_info si = metadata_from_json(data_dir + "/" + param + ".json"); // compare with previously parsed struct from bcompat_sensor_info_data.h const sensor_info si_expected = *(expected_sensor_infos.at(param)); - EXPECT_EQ(si.name, si_expected.name); EXPECT_EQ(si.sn, si_expected.sn); EXPECT_EQ(si.fw_rev, si_expected.fw_rev); - EXPECT_EQ(si.mode, si_expected.mode); + EXPECT_EQ(si.config.lidar_mode, si_expected.config.lidar_mode); EXPECT_EQ(si.prod_line, si_expected.prod_line); EXPECT_EQ(si.format.pixels_per_column, @@ -1065,8 +1064,8 @@ TEST_P(MetaJsonTest, MetadataFromJson) { si_expected.lidar_to_sensor_transform); EXPECT_EQ(si.extrinsic, si_expected.extrinsic); EXPECT_EQ(si.init_id, si_expected.init_id); - EXPECT_EQ(si.udp_port_lidar, si_expected.udp_port_lidar); - EXPECT_EQ(si.udp_port_imu, si_expected.udp_port_imu); + EXPECT_EQ(si.config.udp_port_lidar, si_expected.config.udp_port_lidar); + EXPECT_EQ(si.config.udp_port_imu, si_expected.config.udp_port_imu); EXPECT_EQ(si.build_date, si_expected.build_date); EXPECT_EQ(si.image_rev, si_expected.image_rev); EXPECT_EQ(si.prod_pn, si_expected.prod_pn); @@ -1074,8 +1073,7 @@ TEST_P(MetaJsonTest, MetadataFromJson) { EXPECT_EQ(si.cal, si_expected.cal); EXPECT_EQ(si.config, si_expected.config); - EXPECT_TRUE(si != - si_expected); // si_expected won't have original string set + EXPECT_TRUE(si == si_expected); EXPECT_TRUE(si.has_fields_equal(si_expected)); // but the rest is the same } diff --git a/tests/field_test.cpp b/tests/field_test.cpp new file mode 100644 index 00000000..f0058266 --- /dev/null +++ b/tests/field_test.cpp @@ -0,0 +1,647 @@ +/** + * Copyright (c) 2024, Ouster, Inc. + * All rights reserved. + */ + +#include "ouster/field.h" + +#include +#include +#include +#include +#include + +#include "gtest/gtest.h" + +using namespace ouster; + +TEST(Field_tests, comparison_test) { + Field a(fd_array(100)); + int* a_p = a; + std::iota(a_p, a_p + 100, int{0}); + + Field b(fd_array(100)); + int* b_p = b; + std::iota(b_p, b_p + 100, int{0}); + + EXPECT_TRUE(a == b); + + *b_p = -1; + EXPECT_FALSE(a == b); + + Field c(fd_array(100)); + ASSERT_EQ(c.bytes(), a.bytes()); + float* c_p = c; + std::memcpy(c_p, a_p, a.bytes()); + EXPECT_FALSE(a == c); +} + +TEST(Field_tests, type_safety_test) { + // clang-format off + Field type_checking_ptr = Field{fd_array(128)}; + + EXPECT_NO_THROW({ uint8_t* u8ptr = type_checking_ptr; }); + EXPECT_NO_THROW({ void* vptr = type_checking_ptr; }); + EXPECT_NO_THROW({ type_checking_ptr.get(); }); + EXPECT_NO_THROW({ type_checking_ptr.get(); }); + + EXPECT_THROW({ uint16_t* u16ptr = type_checking_ptr; }, std::invalid_argument); + EXPECT_THROW({ uint32_t* u32ptr = type_checking_ptr; }, std::invalid_argument); + EXPECT_THROW({ int8_t* i8ptr = type_checking_ptr; }, std::invalid_argument); + EXPECT_THROW({ int16_t* i16ptr = type_checking_ptr; }, std::invalid_argument); + EXPECT_THROW({ int32_t* i32ptr = type_checking_ptr; }, std::invalid_argument); + EXPECT_THROW({ int64_t* i64ptr = type_checking_ptr; }, std::invalid_argument); + EXPECT_THROW({ float* fptr = type_checking_ptr; }, std::invalid_argument); + EXPECT_THROW({ double* dptr = type_checking_ptr; }, std::invalid_argument); + + EXPECT_THROW({ type_checking_ptr.get(); }, std::invalid_argument); + EXPECT_THROW({ type_checking_ptr.get(); }, std::invalid_argument); + EXPECT_THROW({ type_checking_ptr.get(); }, std::invalid_argument); + EXPECT_THROW({ type_checking_ptr.get(); }, std::invalid_argument); + EXPECT_THROW({ type_checking_ptr.get(); }, std::invalid_argument); + EXPECT_THROW({ type_checking_ptr.get(); }, std::invalid_argument); + EXPECT_THROW({ type_checking_ptr.get(); }, std::invalid_argument); + EXPECT_THROW({ type_checking_ptr.get(); }, std::invalid_argument); + + Field non_type_checking_ptr = Field{FieldDescriptor::memory(128)}; + + EXPECT_NO_THROW({ void* vptr = non_type_checking_ptr; }); + EXPECT_NO_THROW({ uint8_t* u8ptr = non_type_checking_ptr; }); + EXPECT_NO_THROW({ uint16_t* u16ptr = non_type_checking_ptr; }); + EXPECT_NO_THROW({ uint32_t* u32ptr = non_type_checking_ptr; }); + EXPECT_NO_THROW({ int8_t* i8ptr = non_type_checking_ptr; }); + EXPECT_NO_THROW({ int16_t* i16ptr = non_type_checking_ptr; }); + EXPECT_NO_THROW({ int32_t* i32ptr = non_type_checking_ptr; }); + EXPECT_NO_THROW({ int64_t* i64ptr = non_type_checking_ptr; }); + EXPECT_NO_THROW({ float* fptr = non_type_checking_ptr; }); + EXPECT_NO_THROW({ double* dptr = non_type_checking_ptr; }); + + auto convert_uint8 = [](uint8_t* ptr) { return ptr; }; + auto convert_uint16 = [](uint16_t* ptr) { return ptr; }; + auto convert_uint32 = [](uint32_t* ptr) { return ptr; }; + auto convert_uint64 = [](uint64_t* ptr) { return ptr; }; + auto convert_int8 = [](int8_t* ptr) { return ptr; }; + auto convert_int16 = [](int16_t* ptr) { return ptr; }; + auto convert_int32 = [](int32_t* ptr) { return ptr; }; + auto convert_int64 = [](int64_t* ptr) { return ptr; }; + auto convert_float = [](float* ptr) { return ptr; }; + auto convert_double = [](double* ptr) { return ptr; }; + auto convert_void = [](void* ptr) { return ptr; }; + + EXPECT_NO_THROW(convert_uint8(non_type_checking_ptr)); + EXPECT_NO_THROW(convert_uint16(non_type_checking_ptr)); + EXPECT_NO_THROW(convert_uint32(non_type_checking_ptr)); + EXPECT_NO_THROW(convert_uint64(non_type_checking_ptr)); + EXPECT_NO_THROW(convert_int8(non_type_checking_ptr)); + EXPECT_NO_THROW(convert_int16(non_type_checking_ptr)); + EXPECT_NO_THROW(convert_int32(non_type_checking_ptr)); + EXPECT_NO_THROW(convert_int64(non_type_checking_ptr)); + EXPECT_NO_THROW(convert_float(non_type_checking_ptr)); + EXPECT_NO_THROW(convert_double(non_type_checking_ptr)); + EXPECT_NO_THROW(convert_void(non_type_checking_ptr)); + + EXPECT_NO_THROW(convert_void(type_checking_ptr)); + EXPECT_NO_THROW(convert_uint8(type_checking_ptr)); + EXPECT_THROW(convert_uint16(type_checking_ptr), std::invalid_argument); + EXPECT_THROW(convert_uint32(type_checking_ptr), std::invalid_argument); + EXPECT_THROW(convert_uint64(type_checking_ptr), std::invalid_argument); + EXPECT_THROW(convert_int8(type_checking_ptr), std::invalid_argument); + EXPECT_THROW(convert_int16(type_checking_ptr), std::invalid_argument); + EXPECT_THROW(convert_int32(type_checking_ptr), std::invalid_argument); + EXPECT_THROW(convert_int64(type_checking_ptr), std::invalid_argument); + EXPECT_THROW(convert_float(type_checking_ptr), std::invalid_argument); + EXPECT_THROW(convert_double(type_checking_ptr), std::invalid_argument); + + auto convert_const_uint8 = [](const uint8_t* ptr) { return ptr; }; + auto convert_const_uint16 = [](const uint16_t* ptr) { return ptr; }; + auto convert_const_uint32 = [](const uint32_t* ptr) { return ptr; }; + auto convert_const_uint64 = [](const uint64_t* ptr) { return ptr; }; + auto convert_const_int8 = [](const int8_t* ptr) { return ptr; }; + auto convert_const_int16 = [](const int16_t* ptr) { return ptr; }; + auto convert_const_int32 = [](const int32_t* ptr) { return ptr; }; + auto convert_const_int64 = [](const int64_t* ptr) { return ptr; }; + auto convert_const_float = [](const float* ptr) { return ptr; }; + auto convert_const_double = [](const double* ptr) { return ptr; }; + auto convert_const_void = [](const void* ptr) { return ptr; }; + + EXPECT_NO_THROW(convert_const_uint8(non_type_checking_ptr)); + EXPECT_NO_THROW(convert_const_uint16(non_type_checking_ptr)); + EXPECT_NO_THROW(convert_const_uint32(non_type_checking_ptr)); + EXPECT_NO_THROW(convert_const_uint64(non_type_checking_ptr)); + EXPECT_NO_THROW(convert_const_int8(non_type_checking_ptr)); + EXPECT_NO_THROW(convert_const_int16(non_type_checking_ptr)); + EXPECT_NO_THROW(convert_const_int32(non_type_checking_ptr)); + EXPECT_NO_THROW(convert_const_int64(non_type_checking_ptr)); + EXPECT_NO_THROW(convert_const_float(non_type_checking_ptr)); + EXPECT_NO_THROW(convert_const_double(non_type_checking_ptr)); + EXPECT_NO_THROW(convert_const_void(non_type_checking_ptr)); + + EXPECT_NO_THROW(convert_const_void(type_checking_ptr)); + EXPECT_NO_THROW(convert_const_uint8(type_checking_ptr)); + EXPECT_THROW(convert_const_uint16(type_checking_ptr), std::invalid_argument); + EXPECT_THROW(convert_const_uint32(type_checking_ptr), std::invalid_argument); + EXPECT_THROW(convert_const_uint64(type_checking_ptr), std::invalid_argument); + EXPECT_THROW(convert_const_int8(type_checking_ptr), std::invalid_argument); + EXPECT_THROW(convert_const_int16(type_checking_ptr), std::invalid_argument); + EXPECT_THROW(convert_const_int32(type_checking_ptr), std::invalid_argument); + EXPECT_THROW(convert_const_int64(type_checking_ptr), std::invalid_argument); + EXPECT_THROW(convert_const_float(type_checking_ptr), std::invalid_argument); + EXPECT_THROW(convert_const_double(type_checking_ptr), std::invalid_argument); + // clang-format on +} + +TEST(Field_tests, array_view_test) { + Field p4d{fd_array(128, 128, 64, 3)}; + EXPECT_NO_THROW({ ArrayView4 a = p4d; }); + EXPECT_THROW({ ArrayView3 a = p4d; }, std::invalid_argument); + EXPECT_THROW({ ArrayView2 a = p4d; }, std::invalid_argument); + EXPECT_THROW({ ArrayView1 a = p4d; }, std::invalid_argument); + + EXPECT_THROW({ ArrayView4 a = p4d; }, std::invalid_argument); + + Field p3d{fd_array(128, 64, 3)}; + EXPECT_THROW({ ArrayView4 a = p3d; }, std::invalid_argument); + EXPECT_NO_THROW({ ArrayView3 a = p3d; }); + EXPECT_THROW({ ArrayView2 a = p3d; }, std::invalid_argument); + EXPECT_THROW({ ArrayView1 a = p3d; }, std::invalid_argument); + + Field p2d{fd_array(64, 3)}; + EXPECT_THROW({ ArrayView4 a = p2d; }, std::invalid_argument); + EXPECT_THROW({ ArrayView4 a = p2d; }, std::invalid_argument); + EXPECT_NO_THROW({ ArrayView2 a = p2d; }); + EXPECT_THROW({ ArrayView1 a = p2d; }, std::invalid_argument); + + Field p1d{fd_array(64)}; + EXPECT_THROW({ ArrayView4 a = p1d; }, std::invalid_argument); + EXPECT_THROW({ ArrayView3 a = p1d; }, std::invalid_argument); + EXPECT_THROW({ ArrayView2 a = p1d; }, std::invalid_argument); + EXPECT_NO_THROW({ ArrayView1 a = p1d; }); +} + +TEST(Field_tests, proxy_subview_test) { + Field ptr{fd_array(2, 100, 100)}; + + Field& p = ptr; + const Field& cp = ptr; + + EXPECT_NO_THROW({ ArrayView2 a = p.subview(1); }); + EXPECT_NO_THROW({ ArrayView1 a = p.subview(1, 50); }); + EXPECT_THROW({ ArrayView2 a = p.subview(3); }, + std::invalid_argument); + EXPECT_THROW({ ArrayView1 a = p.subview(3, 50); }, + std::invalid_argument); + EXPECT_THROW({ ArrayView1 a = p.subview(1, 110); }, + std::invalid_argument); + EXPECT_THROW({ ArrayView1 a = p.subview(3, 110); }, + std::invalid_argument); + + EXPECT_NO_THROW({ ConstArrayView2 a = cp.subview(1); }); + EXPECT_NO_THROW({ ConstArrayView1 a = cp.subview(1, 50); }); + EXPECT_THROW({ ConstArrayView2 a = cp.subview(3); }, + std::invalid_argument); + EXPECT_THROW({ ConstArrayView1 a = cp.subview(3, 50); }, + std::invalid_argument); + EXPECT_THROW({ ConstArrayView1 a = cp.subview(1, 110); }, + std::invalid_argument); + EXPECT_THROW({ ConstArrayView1 a = cp.subview(3, 110); }, + std::invalid_argument); + + EXPECT_NO_THROW({ ConstArrayView2 a = p.subview(1); }); + EXPECT_NO_THROW({ ConstArrayView1 a = p.subview(1, 50); }); + EXPECT_THROW({ ConstArrayView2 a = p.subview(3); }, + std::invalid_argument); + EXPECT_THROW({ ConstArrayView1 a = p.subview(3, 50); }, + std::invalid_argument); + EXPECT_THROW({ ConstArrayView1 a = p.subview(1, 110); }, + std::invalid_argument); + EXPECT_THROW({ ConstArrayView1 a = p.subview(3, 110); }, + std::invalid_argument); + + ArrayView3 view = ptr; + + int i = 0; + for (int ss = 0; ss < view.shape[0]; ++ss) { + for (int xx = 0; xx < view.shape[1]; ++xx) { + for (int yy = 0; yy < view.shape[2]; ++yy) { + view(ss, xx, yy) = i++; + } + } + } + + ArrayView1 subview1 = ptr.subview(1, 90); + ArrayView1 subview2 = view.subview(1, 90); + for (int j = 0; j < subview1.shape[0]; ++j) { + EXPECT_EQ(subview1(j), subview2(j)); + } +} + +TEST(Field_tests, bool_test) { + Field empty{}; + Field assigned = Field{FieldDescriptor::memory(128)}; + + bool should_be_false = false; + bool should_be_true = false; + + if (empty) should_be_false = true; + if (assigned) should_be_true = true; + + EXPECT_FALSE(should_be_false); + EXPECT_TRUE(should_be_true); +} + +TEST(Field_tests, bytes_test) { + Field v128 = Field{FieldDescriptor::memory(128)}; + Field v32 = Field{FieldDescriptor::memory(32)}; + Field f128 = Field{fd_array(128)}; + Field f32 = Field{fd_array(32)}; + Field u29 = Field{fd_array(29)}; + Field u32 = Field{fd_array(32)}; + + Field f32_16_64_20 = Field{fd_array(32, 16, 64, 20)}; + + EXPECT_EQ(v128.bytes(), 128); + EXPECT_EQ(v32.bytes(), 32); + EXPECT_EQ(f128.bytes(), 128 * sizeof(float)); + EXPECT_EQ(f32.bytes(), 32 * sizeof(float)); + EXPECT_EQ(u29.bytes(), 29 * sizeof(uint8_t)); + EXPECT_EQ(u32.bytes(), 32 * sizeof(uint8_t)); + EXPECT_EQ(f32_16_64_20.bytes(), 32 * 16 * 64 * 20 * sizeof(float)); +} + +TEST(Field_tests, sparse_memory_check_test) { + Field contiguous_ptr = Field{fd_array(128, 64, 32, 16)}; + FieldView sparse_view = contiguous_ptr.subview(keep(), keep(), keep(), 12); + + std::vector v(128 * 64 * 32 * 16); + + EXPECT_TRUE(sparse_view.sparse()); + + { + Field ptr{FieldDescriptor::memory(128)}; + EXPECT_FALSE(ptr.sparse()); + } + { + Field ptr{fd_array(128)}; + EXPECT_FALSE(ptr.sparse()); + } +} + +TEST(Field_tests, container_test) { + // test vector + { + std::vector v; + EXPECT_NO_THROW({ + v.push_back(Field{FieldDescriptor::memory(128)}); + v.push_back(Field{FieldDescriptor::memory(96)}); + v.push_back(Field{FieldDescriptor::memory(64)}); + v.push_back(Field{FieldDescriptor::memory(32)}); + }); + + void* ptrs[4]; + int i = 0; + for (auto&& p : v) { + ptrs[i] = p; + ++i; + } + + EXPECT_NO_THROW({ + std::sort(v.begin(), v.end(), + [](auto&& a, auto&& b) { return a.bytes() < b.bytes(); }); + }); + + for (auto&& p : v) { + --i; + EXPECT_EQ(ptrs[i], p.get()); + } + } + + // test list + { + std::list l; + EXPECT_NO_THROW({ + l.push_back(Field{FieldDescriptor::memory(128)}); + l.push_back(Field{FieldDescriptor::memory(96)}); + l.push_back(Field{FieldDescriptor::memory(64)}); + l.push_back(Field{FieldDescriptor::memory(32)}); + }); + + void* ptrs[4]; + int i = 0; + for (auto&& p : l) { + ptrs[i] = p; + ++i; + } + + EXPECT_NO_THROW( + l.sort([](auto&& a, auto&& b) { return a.bytes() < b.bytes(); })); + + for (auto&& p : l) { + --i; + EXPECT_EQ(ptrs[i], p.get()); + } + } + + // test unordered_map + { + std::unordered_map m; + m.emplace(std::make_pair("0", Field{FieldDescriptor::memory(128)})); + m.emplace(std::make_pair("1", Field{FieldDescriptor::memory(96)})); + m.emplace(std::make_pair("2", Field{FieldDescriptor::memory(64)})); + m.emplace(std::make_pair("3", Field{FieldDescriptor::memory(32)})); + + EXPECT_NO_THROW(m["0"]); + EXPECT_NO_THROW(m["1"]); + EXPECT_NO_THROW(m["2"]); + EXPECT_NO_THROW(m["3"]); + } +} + +TEST(Field_tests, matches_test) { + Field ptr{fd_array(32)}; + + EXPECT_TRUE(ptr.matches(fd_array(32))); + + EXPECT_FALSE(ptr.matches(fd_array(8, 4))); + EXPECT_FALSE( + ptr.matches(FieldDescriptor::memory(32 * sizeof(float)))); + EXPECT_FALSE(ptr.matches(fd_array(64))); + EXPECT_FALSE(ptr.matches(fd_array(32))); + EXPECT_FALSE(ptr.matches(FieldDescriptor::memory(32 * sizeof(int)))); +} + +TEST(Field_tests, descriptor_strides_test) { + auto a = fd_array(2, 3, 4, 5); + + EXPECT_EQ(a.shape.size(), 4); + EXPECT_EQ(a.shape[0], 2); + EXPECT_EQ(a.shape[1], 3); + EXPECT_EQ(a.shape[2], 4); + EXPECT_EQ(a.shape[3], 5); + + EXPECT_EQ(a.strides.size(), 4); + EXPECT_EQ(a.strides[0], 3 * 4 * 5); + EXPECT_EQ(a.strides[1], 4 * 5); + EXPECT_EQ(a.strides[2], 5); + EXPECT_EQ(a.strides[3], 1); +} + +TEST(Field_tests, view_sparse_test) { + Field ptr{fd_array(20, 320, 456, 18)}; + EXPECT_EQ(ptr.sparse(), false); + EXPECT_EQ(ptr.subview(0).sparse(), false); + EXPECT_EQ(ptr.subview(1).sparse(), false); + EXPECT_EQ(ptr.subview(3).sparse(), false); + EXPECT_EQ(ptr.subview(3, 5).sparse(), false); + EXPECT_EQ(ptr.subview(3, 5, 10).sparse(), false); + + EXPECT_EQ(ptr.subview(keep(), keep(), keep(), 10).sparse(), true); + EXPECT_EQ(ptr.subview(keep(), 200, 134, 10).sparse(), true); + EXPECT_EQ(ptr.subview(keep(), 2, keep(), 10).sparse(), true); + EXPECT_EQ(ptr.subview(1, keep(), keep(), 10).sparse(), true); + EXPECT_EQ(ptr.subview(1, 2, keep(), 10).sparse(), true); + EXPECT_EQ(ptr.subview(keep(), keep(), 10).sparse(), true); + EXPECT_EQ(ptr.subview(1, keep(), 10).sparse(), true); + EXPECT_EQ(ptr.subview(keep(), 10).sparse(), true); + EXPECT_EQ(ptr.subview(keep(), 10, 5).sparse(), true); + EXPECT_EQ(ptr.subview(keep(), 10, 5, 3).sparse(), true); +} + +// TODO: would be good to move LidarScan::Header to types.h +template +using Header = Eigen::Array; + +TEST(Field_tests, eigen_conversion_test) { + Field ptr2d{fd_array(100, 200)}; + + ArrayView2 view2d = ptr2d; + for (int y = 0; y < view2d.shape[0]; ++y) + for (int x = 0; x < view2d.shape[1]; ++x) { + view2d(y, x) = y * 1000 + x; + } + + { + Eigen::Ref> ref = ptr2d; + EXPECT_EQ(ref.data(), ptr2d.get()); + EXPECT_EQ(ref.rows(), 100); + EXPECT_EQ(ref.cols(), 200); + + for (int y = 0; y < ref.rows(); ++y) + for (int x = 0; x < ref.cols(); ++x) + EXPECT_EQ(ref(y, x), y * 1000 + x); + } + + { + Eigen::Ref> ref = ptr2d; + EXPECT_EQ(ref.data(), ptr2d.get()); + EXPECT_EQ(ref.rows(), 100); + EXPECT_EQ(ref.cols(), 200); + + for (int y = 0; y < ref.rows(); ++y) + for (int x = 0; x < ref.cols(); ++x) + EXPECT_EQ(ref(y, x), y * 1000 + x); + } + + Field ptr1d{fd_array(100)}; + ArrayView1 view1d = ptr1d; + + for (int x = 0; x < view1d.shape[0]; ++x) { + view1d(x) = x * 15; + } + + { + Eigen::Ref> ref = ptr1d; + EXPECT_EQ(ref.data(), ptr1d.get()); + EXPECT_EQ(ref.rows(), 100); + + for (int x = 0; x < ref.cols(); ++x) EXPECT_EQ(ref(x), x * 15); + } + + { + Eigen::Ref> ref = ptr1d; + EXPECT_EQ(ref.data(), ptr1d.get()); + EXPECT_EQ(ref.rows(), 100); + + for (int x = 0; x < ref.cols(); ++x) EXPECT_EQ(ref(x), x * 15); + } + + // clang-format off + EXPECT_THROW({ Eigen::Ref> r = ptr2d; }, std::invalid_argument); + EXPECT_THROW({ Eigen::Ref> r = ptr2d; }, std::invalid_argument); + EXPECT_THROW({ Eigen::Ref> r = ptr2d; }, std::invalid_argument); + EXPECT_THROW({ Eigen::Ref> r = ptr2d; }, std::invalid_argument); + EXPECT_THROW({ Eigen::Ref> r = ptr2d; }, std::invalid_argument); + EXPECT_THROW({ Eigen::Ref> r = ptr2d; }, std::invalid_argument); + + EXPECT_THROW({ Eigen::Ref> r = ptr1d; }, std::invalid_argument); + EXPECT_THROW({ Eigen::Ref> r = ptr1d; }, std::invalid_argument); + EXPECT_THROW({ Eigen::Ref> r = ptr1d; }, std::invalid_argument); + EXPECT_THROW({ Eigen::Ref> r = ptr1d; }, std::invalid_argument); + EXPECT_THROW({ Eigen::Ref> r = ptr1d; }, std::invalid_argument); + EXPECT_THROW({ Eigen::Ref> r = ptr1d; }, std::invalid_argument); + + EXPECT_THROW({ Eigen::Ref> r = ptr1d; }, std::invalid_argument); + EXPECT_THROW({ Eigen::Ref> r = ptr2d; }, std::invalid_argument); + + FieldView sparse1d = ptr2d.subview(keep(), 10); + FieldView sparse2d = ptr2d.subview(keep(), keep(2, 5)); + ASSERT_TRUE(sparse1d.sparse()); + ASSERT_TRUE(sparse2d.sparse()); + EXPECT_THROW({ Eigen::Ref> r = sparse2d; }, std::invalid_argument); + EXPECT_THROW({ Eigen::Ref> r = sparse1d; }, std::invalid_argument); + // clang-format on +} + +TEST(Field_tests, reshape_test) { + std::vector vec(100 * 200 * 300); + auto view = FieldView{vec.data(), fd_array(100, 200, 300)}; + EXPECT_THROW(view.reshape(3, 9, 12, 10), std::invalid_argument); + EXPECT_THROW(view.reshape(3, 10), std::invalid_argument); + EXPECT_THROW(view.subview(keep(), 2).reshape(50, 2, 300), + std::invalid_argument); + EXPECT_NO_THROW(view.subview(2).reshape(100, 2, 300)); + + /** + * Below are copied directly from ArrayView tests and may be secondary, + * but we are effectively testing matching runtime logic + */ + { + int i = 0; + for (auto& v : vec) v = i++; + } + { + ConstArrayView2 reshaped = view.reshape(200 * 100, 300); + + int i = 0; + for (int a = 0; a < reshaped.shape[0]; ++a) + for (int b = 0; b < reshaped.shape[1]; ++b) + EXPECT_EQ(reshaped(a, b), i++); + } + { + ConstArrayView2 reshaped = view.reshape(2, 100 * 100 * 300); + + int i = 0; + for (int a = 0; a < reshaped.shape[0]; ++a) + for (int b = 0; b < reshaped.shape[1]; ++b) + EXPECT_EQ(reshaped(a, b), i++); + } + { + ConstArrayView2 reshaped = view.reshape(100 * 50, 4 * 300); + + int i = 0; + for (int a = 0; a < reshaped.shape[0]; ++a) + for (int b = 0; b < reshaped.shape[1]; ++b) + EXPECT_EQ(reshaped(a, b), i++); + } + { + ConstArrayView3 reshaped = view.reshape(200 * 10, 5, 2 * 300); + + int i = 0; + for (int a = 0; a < reshaped.shape[0]; ++a) + for (int b = 0; b < reshaped.shape[1]; ++b) + for (int c = 0; c < reshaped.shape[2]; ++c) + EXPECT_EQ(reshaped(a, b, c), i++); + } + { + ConstArrayView4 reshaped = view.reshape(2, 50, 200, 300); + + int i = 0; + for (int a = 0; a < reshaped.shape[0]; ++a) + for (int b = 0; b < reshaped.shape[1]; ++b) + for (int c = 0; c < reshaped.shape[2]; ++c) + for (int d = 0; d < reshaped.shape[3]; ++d) + EXPECT_EQ(reshaped(a, b, c, d), i++); + } + { + ConstArrayView4 reshaped = view.reshape(100, 50, 4, 300); + + int i = 0; + for (int a = 0; a < reshaped.shape[0]; ++a) + for (int b = 0; b < reshaped.shape[1]; ++b) + for (int c = 0; c < reshaped.shape[2]; ++c) + for (int d = 0; d < reshaped.shape[3]; ++d) + EXPECT_EQ(reshaped(a, b, c, d), i++); + } + { + ConstArrayView4 reshaped = view.reshape(100, 200, 100, 3); + + int i = 0; + for (int a = 0; a < reshaped.shape[0]; ++a) + for (int b = 0; b < reshaped.shape[1]; ++b) + for (int c = 0; c < reshaped.shape[2]; ++c) + for (int d = 0; d < reshaped.shape[3]; ++d) + EXPECT_EQ(reshaped(a, b, c, d), i++); + } +} + +TEST(Field_tests, tag_tests) { + EXPECT_EQ(FieldDescriptor::memory(128).tag(), + sensor::ChanFieldType::VOID); + EXPECT_EQ(fd_array(10).tag(), sensor::ChanFieldType::UINT8); + EXPECT_EQ(fd_array(10).tag(), sensor::ChanFieldType::UINT16); + EXPECT_EQ(fd_array(10).tag(), sensor::ChanFieldType::UINT32); + EXPECT_EQ(fd_array(10).tag(), sensor::ChanFieldType::UINT64); + EXPECT_EQ(fd_array(10).tag(), sensor::ChanFieldType::INT8); + EXPECT_EQ(fd_array(10).tag(), sensor::ChanFieldType::INT16); + EXPECT_EQ(fd_array(10).tag(), sensor::ChanFieldType::INT32); + EXPECT_EQ(fd_array(10).tag(), sensor::ChanFieldType::INT64); + EXPECT_EQ(fd_array(10).tag(), sensor::ChanFieldType::FLOAT32); + EXPECT_EQ(fd_array(10).tag(), sensor::ChanFieldType::FLOAT64); + + EXPECT_EQ(fd_array(sensor::ChanFieldType::UINT8, 10).tag(), + sensor::ChanFieldType::UINT8); + EXPECT_EQ(fd_array(sensor::ChanFieldType::UINT16, 10).tag(), + sensor::ChanFieldType::UINT16); + EXPECT_EQ(fd_array(sensor::ChanFieldType::UINT32, 10).tag(), + sensor::ChanFieldType::UINT32); + EXPECT_EQ(fd_array(sensor::ChanFieldType::UINT64, 10).tag(), + sensor::ChanFieldType::UINT64); + EXPECT_EQ(fd_array(sensor::ChanFieldType::INT8, 10).tag(), + sensor::ChanFieldType::INT8); + EXPECT_EQ(fd_array(sensor::ChanFieldType::INT16, 10).tag(), + sensor::ChanFieldType::INT16); + EXPECT_EQ(fd_array(sensor::ChanFieldType::INT32, 10).tag(), + sensor::ChanFieldType::INT32); + EXPECT_EQ(fd_array(sensor::ChanFieldType::INT64, 10).tag(), + sensor::ChanFieldType::INT64); +} + +template +void uint_view_test() { + std::vector vec(100 * 200 * 300); + FieldView view{vec.data(), fd_array(100, 200, 300)}; + + FieldView uint_v = uint_view(view); + EXPECT_EQ(view.shape(), uint_v.shape()); + switch (sizeof(T)) { + case 1: + EXPECT_EQ(uint_v.tag(), sensor::ChanFieldType::UINT8); + break; + case 2: + EXPECT_EQ(uint_v.tag(), sensor::ChanFieldType::UINT16); + break; + case 4: + EXPECT_EQ(uint_v.tag(), sensor::ChanFieldType::UINT32); + break; + case 8: + EXPECT_EQ(uint_v.tag(), sensor::ChanFieldType::UINT64); + break; + } +} + +TEST(Field_tests, uint_view_tests) { + EXPECT_NO_THROW(uint_view_test()); + EXPECT_NO_THROW(uint_view_test()); + EXPECT_NO_THROW(uint_view_test()); + + EXPECT_NO_THROW(uint_view_test()); + EXPECT_NO_THROW(uint_view_test()); + EXPECT_NO_THROW(uint_view_test()); + EXPECT_NO_THROW(uint_view_test()); + + struct my_struct { + double x, y; + }; + std::vector vec(100); + FieldView view{vec.data(), fd_array(100)}; + EXPECT_THROW(uint_view(view), std::invalid_argument); +} diff --git a/tests/fusa_profile_test.cpp b/tests/fusa_profile_test.cpp index 08c4b5c0..0255a660 100644 --- a/tests/fusa_profile_test.cpp +++ b/tests/fusa_profile_test.cpp @@ -52,15 +52,17 @@ TEST(FusaProfileTest, fields) { // check preconditions for the test constexpr int pixels_per_column = 128u; - ASSERT_EQ(info.mode, ouster::sensor::MODE_1024x10); + ASSERT_EQ(info.config.lidar_mode, ouster::sensor::MODE_1024x10); ASSERT_EQ(info.format.udp_profile_lidar, ouster::sensor::PROFILE_FUSA_RNG15_RFL8_NIR8_DUAL); ASSERT_EQ(pf.pixels_per_column, pixels_per_column); // check field widths int expected_cols = 16; - ASSERT_EQ(pf.field_type(ouster::sensor::RANGE), ouster::sensor::UINT32); - ASSERT_EQ(pf.field_type(ouster::sensor::RANGE2), ouster::sensor::UINT32); + ASSERT_EQ(pf.field_type(ouster::sensor::ChanField::RANGE), + ouster::sensor::UINT32); + ASSERT_EQ(pf.field_type(ouster::sensor::ChanField::RANGE2), + ouster::sensor::UINT32); ASSERT_EQ(pf.columns_per_packet, expected_cols); // check packet header values @@ -105,11 +107,11 @@ TEST(FusaProfileTest, fields) { 480, 0, 0, 0, 448, 0, 0, 0}; uint32_t range_array[pixels_per_column]; - pf.col_field(col, ouster::sensor::RANGE, range_array); + pf.col_field(col, ouster::sensor::ChanField::RANGE, range_array); for (int range_idx = 0; range_idx < pixels_to_test; range_idx++) { EXPECT_EQ(range_array[range_idx], expected_range[range_idx]); } - pf.col_field(col, ouster::sensor::RANGE2, range_array); + pf.col_field(col, ouster::sensor::ChanField::RANGE2, range_array); for (int range_idx = 0; range_idx < pixels_to_test; range_idx++) { EXPECT_EQ(range_array[range_idx], expected_range2[range_idx]); } @@ -117,7 +119,7 @@ TEST(FusaProfileTest, fields) { uint16_t expected_nearir[] = {320, 544, 528, 496, 400, 464, 496, 560, 368, 512, 640, 640, 400, 672, 688, 608}; uint16_t nearir_array[pixels_per_column]; - pf.col_field(col, ouster::sensor::NEAR_IR, nearir_array); + pf.col_field(col, ouster::sensor::ChanField::NEAR_IR, nearir_array); for (int nearir_idx = 0; nearir_idx < pixels_to_test; nearir_idx++) { EXPECT_EQ(nearir_array[nearir_idx], expected_nearir[nearir_idx]); } @@ -126,19 +128,19 @@ TEST(FusaProfileTest, fields) { 1, 58, 60, 61, 1, 67, 68, 72}; uint8_t expected_refl2[] = {0, 0, 0, 0, 0, 0, 0, 0, 6, 0, 0, 0, 3, 0, 0, 0}; uint8_t refl_array[pixels_per_column]; - pf.col_field(col, ouster::sensor::REFLECTIVITY, refl_array); + pf.col_field(col, ouster::sensor::ChanField::REFLECTIVITY, refl_array); for (int refl_idx = 0; refl_idx < pixels_to_test; refl_idx++) { EXPECT_EQ(refl_array[refl_idx], expected_refl[refl_idx]); } - pf.col_field(col, ouster::sensor::REFLECTIVITY2, refl_array); + pf.col_field(col, ouster::sensor::ChanField::REFLECTIVITY2, refl_array); for (int refl_idx = 0; refl_idx < pixels_to_test; refl_idx++) { EXPECT_EQ(refl_array[refl_idx], expected_refl2[refl_idx]); } uint32_t raw_words[pixels_per_column]; uint32_t raw_words2[pixels_per_column]; - pf.col_field(col, ouster::sensor::RAW32_WORD1, raw_words); - pf.col_field(col, ouster::sensor::RAW32_WORD2, raw_words2); + pf.col_field(col, ouster::sensor::ChanField::RAW32_WORD1, raw_words); + pf.col_field(col, ouster::sensor::ChanField::RAW32_WORD2, raw_words2); // check raw words against known range values // to confirm raw word offsets are correct diff --git a/tests/lidar_scan_test.cpp b/tests/lidar_scan_test.cpp index b002fffe..6a70f68d 100644 --- a/tests/lidar_scan_test.cpp +++ b/tests/lidar_scan_test.cpp @@ -25,15 +25,15 @@ template using Table = std::array, N>; using namespace ouster::sensor; -static const Table empty_field_slots{}; +static const std::vector empty_field_slots{}; -static const Table legacy_field_slots{ +static const std::vector legacy_field_slots{ {{ChanField::RANGE, ChanFieldType::UINT32}, {ChanField::SIGNAL, ChanFieldType::UINT32}, {ChanField::NEAR_IR, ChanFieldType::UINT32}, {ChanField::REFLECTIVITY, ChanFieldType::UINT32}}}; -static const Table dual_field_slots{ +static const std::vector dual_field_slots{ {{ChanField::RANGE, ChanFieldType::UINT32}, {ChanField::RANGE2, ChanFieldType::UINT32}, {ChanField::SIGNAL, ChanFieldType::UINT16}, @@ -42,13 +42,13 @@ static const Table dual_field_slots{ {ChanField::REFLECTIVITY2, ChanFieldType::UINT8}, {ChanField::NEAR_IR, ChanFieldType::UINT16}}}; -static const Table contrived_slots{ +static const std::vector contrived_slots{ {{ChanField::RANGE2, ChanFieldType::UINT32}, {ChanField::SIGNAL2, ChanFieldType::UINT16}, {ChanField::REFLECTIVITY, ChanFieldType::UINT8}, {ChanField::NEAR_IR, ChanFieldType::UINT16}}}; -static const Table duplicated_slots{ +static const std::vector duplicated_slots{ {{ChanField::RANGE2, ChanFieldType::UINT32}, {ChanField::RANGE2, ChanFieldType::UINT32}, {ChanField::REFLECTIVITY, ChanFieldType::UINT8}, @@ -73,11 +73,29 @@ struct check_field_data { }; void zero_check_fields(ouster::LidarScan& scan) { - for (auto it = scan.begin(); it != scan.end(); it++) { - ouster::impl::visit_field(scan, it->first, check_field_data(), 0); + for (const auto& field : scan.fields()) { + ouster::impl::visit_field(scan, field.first, check_field_data(), 0); } } +TEST(LidarScanSanityTests, get_field_types_test) { + ouster::LidarScanFieldTypes ft; + ft.emplace_back(ChanField::RANGE, ChanFieldType::UINT32); + ft.emplace_back(ChanField::SIGNAL, ChanFieldType::UINT16); + ft.emplace_back(ChanField::RANGE2, ChanFieldType::UINT32); + ft.emplace_back(ChanField::SIGNAL2, ChanFieldType::UINT16); + ft.emplace_back(ChanField::REFLECTIVITY, ChanFieldType::UINT8); + ft.emplace_back(ChanField::NEAR_IR, ChanFieldType::UINT16); + ft.emplace_back(ChanField::FLAGS, ChanFieldType::UINT8); + ft.emplace_back(ChanField::FLAGS2, ChanFieldType::UINT8); + ft.emplace_back("CUSTOM0", ChanFieldType::UINT64); + ft.emplace_back("CUSTOM7", ChanFieldType::UINT16); + std::sort(ft.begin(), ft.end()); + + auto ls = ouster::LidarScan(1028, 128, ft.begin(), ft.end()); + EXPECT_EQ(ft, ls.field_types()); +} + TEST(LidarScan, EmptyConstructorInit) { auto scan = ouster::LidarScan(); EXPECT_EQ(scan.w, 0); @@ -85,7 +103,7 @@ TEST(LidarScan, EmptyConstructorInit) { EXPECT_EQ(scan.frame_id, -1); - EXPECT_EQ(scan.end() - scan.begin(), 0); + EXPECT_EQ(scan.fields().size(), 0); zero_check_fields(scan); } @@ -100,13 +118,13 @@ TEST(LidarScan, LegacyConstructorInit) { size_t count = 0; size_t hit_count = 0; - std::vector field_copy; + std::vector field_copy; for (auto item : legacy_field_slots) { - field_copy.push_back(std::get<0>(item)); + field_copy.push_back(item.name); } - for (auto it = scan.begin(); it != scan.end(); it++) { + for (const auto& field : scan.fields()) { auto result = - std::find(field_copy.begin(), field_copy.end(), it->first); + std::find(field_copy.begin(), field_copy.end(), field.first); if (result != field_copy.end()) { hit_count++; field_copy.erase(result); @@ -135,13 +153,13 @@ TEST(LidarScan, DualReturnConstructorInit) { size_t count = 0; size_t hit_count = 0; - std::vector field_copy; + std::vector field_copy; for (auto item : dual_field_slots) { - field_copy.push_back(std::get<0>(item)); + field_copy.push_back(item.name); } - for (auto it = scan.begin(); it != scan.end(); it++) { + for (const auto& field : scan.fields()) { auto result = - std::find(field_copy.begin(), field_copy.end(), it->first); + std::find(field_copy.begin(), field_copy.end(), field.first); if (result != field_copy.end()) { hit_count++; field_copy.erase(result); @@ -170,14 +188,14 @@ TEST(LidarScan, CustomFieldConstructorInit) { size_t count = 0; size_t hit_count = 0; - std::vector field_copy; + std::vector field_copy; for (auto item : contrived_slots) { - field_copy.push_back(std::get<0>(item)); + field_copy.push_back(item.name); } - for (auto it = scan.begin(); it != scan.end(); it++) { + for (const auto& field : scan.fields()) { auto result = - std::find(field_copy.begin(), field_copy.end(), it->first); + std::find(field_copy.begin(), field_copy.end(), field.first); if (result != field_copy.end()) { hit_count++; field_copy.erase(result); @@ -200,19 +218,11 @@ TEST(LidarScan, EmptyEquality) { auto scan2 = ouster::LidarScan(); auto scan3 = ouster::LidarScan(); auto scan4 = ouster::LidarScan(10, 20); - auto scan5 = ouster::LidarScan(0, 0); - auto scan6 = ouster::LidarScan(0, 0, empty_field_slots.begin(), - empty_field_slots.end()); - auto scan7 = ouster::LidarScan( - 0, 0, UDPProfileLidar::PROFILE_RNG19_RFL8_SIG16_NIR16_DUAL); scan3.frame_id = 1; EXPECT_TRUE(scan1 == scan2); EXPECT_TRUE(scan1 != scan3); EXPECT_TRUE(scan1 != scan4); - EXPECT_TRUE(scan1 != scan5); - EXPECT_TRUE(scan1 == scan6); - EXPECT_TRUE(scan1 != scan7); } TEST(LidarScan, LegacyEquality) { @@ -264,7 +274,7 @@ TEST(LidarScan, CustomEquality) { auto test_array1 = dual_field_slots; auto test_array2 = dual_field_slots; - auto test_array3 = std::vector>( + auto test_array3 = std::vector( dual_field_slots.begin() + 1, dual_field_slots.end()); std::random_shuffle(test_array1.begin(), test_array1.end()); @@ -284,7 +294,7 @@ TEST(LidarScan, CustomEquality) { scan4.frame_id = 1; EXPECT_TRUE(scan1 == scan2); - EXPECT_TRUE(scan1 != scan3); + EXPECT_TRUE(scan1 == scan3); EXPECT_TRUE(scan3 != scan4); EXPECT_TRUE(scan1 != scan5); } @@ -301,7 +311,7 @@ TEST(LidarScan, DataCheck) { int w = rand() % 1000 + 1; int h = rand() % 1000 + 1; - std::unordered_map expected_data; + std::unordered_map expected_data; auto scan1 = ouster::LidarScan( w, h, UDPProfileLidar::PROFILE_RNG19_RFL8_SIG16_NIR16_DUAL); @@ -310,14 +320,14 @@ TEST(LidarScan, DataCheck) { auto scan3 = ouster::LidarScan( w, h, UDPProfileLidar::PROFILE_RNG19_RFL8_SIG16_NIR16_DUAL); - for (auto it = scan1.begin(); it != scan1.end(); it++) { - expected_data[it->first] = rand() % 254 + 1; - ouster::impl::visit_field(scan1, it->first, set_field_data(), - expected_data[it->first]); - ouster::impl::visit_field(scan2, it->first, set_field_data(), - expected_data[it->first]); - ouster::impl::visit_field(scan1, it->first, check_field_data(), - expected_data[it->first]); + for (const auto& field : scan1.fields()) { + expected_data[field.first] = rand() % 254 + 1; + ouster::impl::visit_field(scan1, field.first, set_field_data(), + expected_data[field.first]); + ouster::impl::visit_field(scan2, field.first, set_field_data(), + expected_data[field.first]); + ouster::impl::visit_field(scan1, field.first, check_field_data(), + expected_data[field.first]); } EXPECT_TRUE(scan1 == scan2); @@ -326,17 +336,15 @@ TEST(LidarScan, DataCheck) { } TEST(LidarScan, CustomUserFields) { - using LidarScanFieldTypes = std::vector< - std::pair>; + using LidarScanFieldTypes = std::vector; - LidarScanFieldTypes user_fields{ - {ChanField::CUSTOM0, ChanFieldType::UINT8}, - {ChanField::CUSTOM3, ChanFieldType::UINT64}, - {ChanField::CUSTOM9, ChanFieldType::UINT16}}; + LidarScanFieldTypes user_fields{{"CUSTOM0", ChanFieldType::UINT8}, + {"CUSTOM3", ChanFieldType::UINT64}, + {"CUSTOM9", ChanFieldType::UINT16}}; ouster::LidarScan user_scan(10, 10, user_fields.begin(), user_fields.end()); - EXPECT_EQ(3, std::distance(user_scan.begin(), user_scan.end())); + EXPECT_EQ(3, user_scan.fields().size()); zero_check_fields(user_scan); } @@ -454,7 +462,7 @@ TEST(LidarScan, test_get_first_valid_packet_timestamp) { auto packet_ts = scan.packet_timestamp(); // fill in some default values - std::iota(packet_ts.begin(), packet_ts.end(), 1); + std::iota(packet_ts.data(), packet_ts.data() + packet_ts.size(), 1); ASSERT_TRUE((packet_ts == scan.packet_timestamp()).all()); // no packet found @@ -474,6 +482,16 @@ TEST(LidarScan, test_get_first_valid_packet_timestamp) { EXPECT_EQ(scan.get_first_valid_packet_timestamp(), 64); } +TEST(LidarScan, test_packet_timestamps_length) { + auto ls10 = ouster::LidarScan(10, 64, PROFILE_RNG19_RFL8_SIG16_NIR16, 32); + auto ls32 = ouster::LidarScan(32, 64, PROFILE_RNG19_RFL8_SIG16_NIR16, 32); + auto ls33 = ouster::LidarScan(33, 64, PROFILE_RNG19_RFL8_SIG16_NIR16, 32); + + EXPECT_EQ(ls10.packet_timestamp().size(), 1); + EXPECT_EQ(ls32.packet_timestamp().size(), 1); + EXPECT_EQ(ls33.packet_timestamp().size(), 2); +} + TEST(LidarScan, destagger) { // It raises std::invalid_argument when the image height doesn't match the // shift rows @@ -494,3 +512,13 @@ TEST(LidarScan, destagger) { }, std::invalid_argument); } + +TEST(LidarScan, lidar_scan_to_string_test) { + ouster::LidarScan ls(128, 1024, PROFILE_RNG19_RFL8_SIG16_NIR16); + ls.add_field("custom_field", ouster::fd_array(33, 44, 55), {}); + ls.field(ChanField::RANGE) = 10; + + std::string s; + EXPECT_NO_THROW({ s = to_string(ls); }); + std::cout << s << std::endl; +} diff --git a/tests/metadata_test.cpp b/tests/metadata_test.cpp index 51275417..2eb3156f 100644 --- a/tests/metadata_test.cpp +++ b/tests/metadata_test.cpp @@ -7,6 +7,9 @@ #include #include +#include +#include +#include #include "ouster/types.h" #include "ouster/util.h" @@ -105,27 +108,6 @@ INSTANTIATE_TEST_CASE_P( "ouster-studio-reduced-config-v1")); // clang-format on -TEST_P(MetaFiles, origStringTestMetadata) { - std::string param = GetParam(); - - auto data_dir = getenvs("DATA_DIR"); - auto json_file = data_dir + param + ".json"; - - ouster::sensor::sensor_info si = - ouster::sensor::metadata_from_json(json_file); - - std::stringstream buf{}; - std::ifstream ifs{}; - ifs.open(json_file); - buf << ifs.rdbuf(); - ifs.close(); - - si.mode = ouster::sensor::lidar_mode::MODE_4096x5; - si.build_date = "FAKEBUILDDATE"; - - EXPECT_EQ(si.original_string(), buf.str()); -} - TEST_P(MetaFiles, combinedTestMetadata) { std::string param = GetParam(); @@ -137,73 +119,202 @@ TEST_P(MetaFiles, combinedTestMetadata) { // Make si new -- change a few values auto si_new = si_orig; si_new.init_id = 5; - si_new.mode = ouster::sensor::lidar_mode::MODE_4096x5; + si_new.config.lidar_mode = ouster::sensor::lidar_mode::MODE_4096x5; si_new.format.fps = 47289; // fps is an addition instead of a replacement si_new.beam_altitude_angles[5] = 0.01; // Make sure they aren't somehow the same - EXPECT_NE(si_new.mode, si_orig.mode); + EXPECT_NE(si_new.config.lidar_mode, si_orig.config.lidar_mode); EXPECT_NE(si_new.init_id, si_orig.init_id); EXPECT_NE(si_new.format.fps, si_orig.format.fps); EXPECT_NE(si_new.beam_altitude_angles, si_orig.beam_altitude_angles); - auto si_new_updated_string = si_new.updated_metadata_string(); - auto si_roundtrip = ouster::sensor::parse_metadata(si_new_updated_string); + auto si_new_updated_string = si_new.to_json_string(); + auto si_roundtrip = ouster::sensor::sensor_info(si_new_updated_string); - EXPECT_EQ(si_new.mode, si_roundtrip.mode); + EXPECT_EQ(si_new.config.lidar_mode, si_roundtrip.config.lidar_mode); EXPECT_EQ(si_new.init_id, si_roundtrip.init_id); EXPECT_EQ(si_new.format.fps, si_roundtrip.format.fps); EXPECT_EQ(si_new.beam_altitude_angles, si_roundtrip.beam_altitude_angles); +} - Json::Value root{}; - Json::CharReaderBuilder builder{}; - std::string errors{}; - std::stringstream ss{si_new_updated_string}; - - Json::parseFromStream(builder, ss, &root, &errors); - - auto changed_json = root["ouster-sdk"]["changed_fields"]; - - auto find_changed = [&changed_json](auto str1, auto str2) { - return (std::find(changed_json.begin(), changed_json.end(), str1) != - changed_json.end() || - std::find(changed_json.begin(), changed_json.end(), str2) != - changed_json.end()); - }; - - EXPECT_TRUE( - find_changed("sensor_info.initialization_id", "initialization_id")); - - if (param.substr(0, 4) == "1_12" || param.substr(0, 4) == "1_13" || - param == "ouster-studio-reduced-config-v1") { - // 1_12 and 1_13 did not have data_format at all - EXPECT_TRUE(find_changed("data_format", "lidar_data_format")); - } else { - // 1.14+ have data_format so only fps changes - EXPECT_TRUE(find_changed("data_format.fps", "lidar_data_format.fps")); +class product_info_test : public ouster::sensor::product_info { + public: + product_info_test(std::string product_info_string, std::string form_factor, + bool short_range, std::string beam_config, int beam_count) + : ouster::sensor::product_info(product_info_string, form_factor, + short_range, beam_config, beam_count){}; +}; + +TEST(Util, TestProdlineDecoder) { + EXPECT_EQ(ouster::sensor::product_info(), ouster::sensor::product_info()); + EXPECT_EQ( + ouster::sensor::product_info::create_product_info("OS-0-128-BH02-SR"), + ouster::sensor::product_info::create_product_info("OS-0-128-BH02-SR")); + EXPECT_NE( + ouster::sensor::product_info::create_product_info("OS-0-128-BH02-SR"), + ouster::sensor::product_info()); + EXPECT_NE( + ouster::sensor::product_info::create_product_info("OS-0-128"), + ouster::sensor::product_info::create_product_info("OS-0-128-BH02-SR")); + EXPECT_NE( + ouster::sensor::product_info::create_product_info("OS-0-128-BH02"), + ouster::sensor::product_info::create_product_info("OS-0-128-BH02-SR")); + + bool error_recieved = false; + try { + ouster::sensor::product_info::create_product_info("DEADBEEF"); + } catch (const std::runtime_error& e) { + EXPECT_EQ(std::string(e.what()), + "Product Info \"DEADBEEF\" is not a recognized product info"); + error_recieved = true; } - EXPECT_TRUE(find_changed("lidar_mode", "config_params.lidar_mode")); - EXPECT_TRUE(find_changed("beam_intrinsics.beam_altitude_angles", - "beam_altitude_angles")); - - if (param == "ouster-studio-reduced-config-v1") { - EXPECT_TRUE(changed_json.size() == 12); - } else { - if (std::find(changed_json.begin(), changed_json.end(), "ouster-sdk") != - changed_json.end()) { - // check indicates non-legacy format - if (param.substr(0, 4) == "1_12" || param.substr(0, 4) == "1_13") { - // udp_dest and operating_mode are new - EXPECT_TRUE(changed_json.size() == 7); - } else { - // expect ouster-sdk along with the main 4 - EXPECT_TRUE(changed_json.size() == 5); - } - - } else { - // mode, init_id, data_format.fps (or data_format itself), - // beam_altitude_angles - EXPECT_TRUE(changed_json.size() == 4); - } + EXPECT_TRUE(error_recieved); + + auto bad_count = ouster::sensor::product_info::create_product_info( + "OS-0-STUFF HERE-BH02-SR"); + EXPECT_EQ(bad_count.beam_count, 0); + + std::vector> test_product_infos = + {std::make_pair( + "FOOBAR-1234", + product_info_test("FOOBAR-1234", "FOOBAR1234", false, "U", 0)), + std::make_pair("OS-0-128", + product_info_test("OS-0-128", "OS0", false, "U", 128)), + std::make_pair( + "OS-0-128-BH02-SR", + product_info_test("OS-0-128-BH02-SR", "OS0", true, "BH02", 128)), + std::make_pair("OS-0-32-AH02", product_info_test("OS-0-32-AH02", "OS0", + false, "AH02", 32)), + std::make_pair("OS-0-32-BH02", product_info_test("OS-0-32-BH02", "OS0", + false, "BH02", 32)), + std::make_pair("OS-0-32-G", + product_info_test("OS-0-32-G", "OS0", false, "G", 32)), + std::make_pair("OS-0-32-U0", product_info_test("OS-0-32-U0", "OS0", + false, "U0", 32)), + std::make_pair("OS-0-32-U1", product_info_test("OS-0-32-U1", "OS0", + false, "U1", 32)), + std::make_pair("OS-0-32-U2", product_info_test("OS-0-32-U2", "OS0", + false, "U2", 32)), + std::make_pair("OS-0-32-U3", product_info_test("OS-0-32-U3", "OS0", + false, "U3", 32)), + std::make_pair("OS-0-64-AH", product_info_test("OS-0-64-AH", "OS0", + false, "AH", 64)), + std::make_pair("OS-0-64-BH", product_info_test("OS-0-64-BH", "OS0", + false, "BH", 64)), + std::make_pair("OS-0-64-G", + product_info_test("OS-0-64-G", "OS0", false, "G", 64)), + std::make_pair("OS-0-64-U02", product_info_test("OS-0-64-U02", "OS0", + false, "U02", 64)), + std::make_pair("OS-0-64-U13", product_info_test("OS-0-64-U13", "OS0", + false, "U13", 64)), + std::make_pair("OS-1-128", + product_info_test("OS-1-128", "OS1", false, "U", 128)), + std::make_pair("OS-1-128-SR", product_info_test("OS-1-128-SR", "OS1", + true, "U", 128)), + std::make_pair("OS-1-16-A1", product_info_test("OS-1-16-A1", "OS1", + false, "A1", 16)), + std::make_pair("OS-1-16-U0", product_info_test("OS-1-16-U0", "OS1", + false, "U0", 16)), + std::make_pair("OS-1-32-A02", product_info_test("OS-1-32-A02", "OS1", + false, "A02", 32)), + std::make_pair("OS-1-32-BH02", product_info_test("OS-1-32-BH02", "OS1", + false, "BH02", 32)), + std::make_pair("OS-1-32-BH13", product_info_test("OS-1-32-BH13", "OS1", + false, "BH13", 32)), + std::make_pair("OS-1-32-C", + product_info_test("OS-1-32-C", "OS1", false, "C", 32)), + std::make_pair("OS-1-32-G", + product_info_test("OS-1-32-G", "OS1", false, "G", 32)), + std::make_pair("OS-1-32-U0", product_info_test("OS-1-32-U0", "OS1", + false, "U0", 32)), + std::make_pair("OS-1-32-U1", product_info_test("OS-1-32-U1", "OS1", + false, "U1", 32)), + std::make_pair("OS-1-32-U2", product_info_test("OS-1-32-U2", "OS1", + false, "U2", 32)), + std::make_pair("OS-1-32-U3", product_info_test("OS-1-32-U3", "OS1", + false, "U3", 32)), + std::make_pair("OS-1-64", + product_info_test("OS-1-64", "OS1", false, "U", 64)), + std::make_pair("OS-1-64-AH", product_info_test("OS-1-64-AH", "OS1", + false, "AH", 64)), + std::make_pair("OS-1-64-BH", product_info_test("OS-1-64-BH", "OS1", + false, "BH", 64)), + std::make_pair("OS-1-64-G", + product_info_test("OS-1-64-G", "OS1", false, "G", 64)), + std::make_pair("OS-1-64-U02", product_info_test("OS-1-64-U02", "OS1", + false, "U02", 64)), + std::make_pair("OS-1-64-U13", product_info_test("OS-1-64-U13", "OS1", + false, "U13", 64)), + std::make_pair("OS-2-128", + product_info_test("OS-2-128", "OS2", false, "U", 128)), + std::make_pair("OS-2-32-BH02", product_info_test("OS-2-32-BH02", "OS2", + false, "BH02", 32)), + std::make_pair("OS-2-32-G", + product_info_test("OS-2-32-G", "OS2", false, "G", 32)), + std::make_pair("OS-2-32-U0", product_info_test("OS-2-32-U0", "OS2", + false, "U0", 32)), + std::make_pair("OS-2-32-U2", product_info_test("OS-2-32-U2", "OS2", + false, "U2", 32)), + std::make_pair("OS-2-64-BH", product_info_test("OS-2-64-BH", "OS2", + false, "BH", 64)), + std::make_pair("OS-2-64-G", + product_info_test("OS-2-64-G", "OS2", false, "G", 64)), + std::make_pair("OS-2-64-U02", product_info_test("OS-2-64-U02", "OS2", + false, "U02", 64)), + std::make_pair( + "OS-DOME-128", + product_info_test("OS-DOME-128", "OSDOME", false, "U", 128)), + std::make_pair( + "OS-DOME-32-AH02", + product_info_test("OS-DOME-32-AH02", "OSDOME", false, "AH02", 32)), + std::make_pair( + "OS-DOME-32-AH13", + product_info_test("OS-DOME-32-AH13", "OSDOME", false, "AH13", 32)), + std::make_pair( + "OS-DOME-32-BH02", + product_info_test("OS-DOME-32-BH02", "OSDOME", false, "BH02", 32)), + std::make_pair( + "OS-DOME-32-BH13", + product_info_test("OS-DOME-32-BH13", "OSDOME", false, "BH13", 32)), + std::make_pair( + "OS-DOME-32-G", + product_info_test("OS-DOME-32-G", "OSDOME", false, "G", 32)), + std::make_pair( + "OS-DOME-32-U0", + product_info_test("OS-DOME-32-U0", "OSDOME", false, "U0", 32)), + std::make_pair( + "OS-DOME-32-U1", + product_info_test("OS-DOME-32-U1", "OSDOME", false, "U1", 32)), + std::make_pair( + "OS-DOME-32-U2", + product_info_test("OS-DOME-32-U2", "OSDOME", false, "U2", 32)), + std::make_pair( + "OS-DOME-32-U3", + product_info_test("OS-DOME-32-U3", "OSDOME", false, "U3", 32)), + std::make_pair( + "OS-DOME-64-AH", + product_info_test("OS-DOME-64-AH", "OSDOME", false, "AH", 64)), + std::make_pair( + "OS-DOME-64-BH", + product_info_test("OS-DOME-64-BH", "OSDOME", false, "BH", 64)), + std::make_pair( + "OS-DOME-64-G", + product_info_test("OS-DOME-64-G", "OSDOME", false, "G", 64)), + std::make_pair( + "OS-DOME-64-U02", + product_info_test("OS-DOME-64-U02", "OSDOME", false, "U02", 64)), + std::make_pair( + "OS-DOME-64-U13", + product_info_test("OS-DOME-64-U13", "OSDOME", false, "U13", 64))}; + + for (auto it : test_product_infos) { + auto actual = + ouster::sensor::product_info::create_product_info(it.first); + std::cout << "Comparing:" << std::endl; + std::cout << to_string(actual); + std::cout << "To:" << std::endl; + std::cout << to_string((ouster::sensor::product_info)it.second); + EXPECT_EQ(actual, (ouster::sensor::product_info)it.second); } } diff --git a/tests/packet_writer_test.cpp b/tests/packet_writer_test.cpp index 0d6cdfc0..44923002 100644 --- a/tests/packet_writer_test.cpp +++ b/tests/packet_writer_test.cpp @@ -31,7 +31,7 @@ struct FieldInfo { }; struct ProfileEntry { - const std::pair* fields; + const std::pair* fields; size_t n_fields; size_t chan_data_size; }; @@ -44,7 +44,7 @@ extern Table profiles; uint64_t get_value_mask(const FieldInfo& f); int get_bitness(const FieldInfo& f); -std::map get_fields(UDPProfileLidar profile) { +std::map get_fields(UDPProfileLidar profile) { auto end = profiles.end(); auto it = std::find_if(impl::profiles.begin(), end, @@ -58,7 +58,7 @@ std::map get_fields(UDPProfileLidar profile) { } // namespace sensor } // namespace ouster -using bitness_param = std::tuple>; +using bitness_param = std::tuple>; class FieldInfoSanityTest : public ::testing::TestWithParam {}; // clang-format off @@ -169,10 +169,7 @@ struct cmp_field { LidarScan& ls; template - void operator()(Eigen::Ref> field, ChanField i) { - // FUSA hacks - if (i >= ChanField::RAW_HEADERS) return; - + void operator()(Eigen::Ref> field, const std::string& i) { EXPECT_TRUE((ls.field(i) == field).all()); } }; @@ -230,21 +227,15 @@ TEST_P(PacketWriterTest, packet_writer_randomize_test) { std::fill(ls.status().data(), ls.status().data() + ls.status().size(), 0x1); ls.frame_id = 700; - auto randomise = [&](auto ref_field, ChanField i) { - // need this to skip RAW32_WORD* - if (i >= ChanField::RAW_HEADERS) return; - + auto randomise = [&](auto ref_field, const std::string& i) { // use static seed so that the test does not flake some day and // collectively piss off a bunch of angry developers randomize_field(ref_field, pw.field_value_mask(i), 0xdeadbeef); }; - ouster::impl::foreach_field(ls, randomise); + ouster::impl::foreach_channel_field(ls, pf, randomise); auto fields = get_fields(profile); - auto verify_field = [&](auto ref_field, ChanField i) { - // need this to skip RAW32_WORD* - if (i >= ChanField::RAW_HEADERS) return; - + auto verify_field = [&](auto ref_field, const std::string& i) { // field should not be all zeros EXPECT_FALSE((ref_field == 0).all()); @@ -257,14 +248,16 @@ TEST_P(PacketWriterTest, packet_writer_randomize_test) { uint64_t field_mask = 0; for (int i = 0; i < ref_field.size(); ++i) { T value = *(data + i); + uint64_t value_bits = 0; + memcpy(&value_bits, &value, sizeof(T)); // output must perfectly fit into the value mask - EXPECT_EQ(value, value & value_mask); - field_mask |= value; + EXPECT_EQ(value_bits, value_bits & value_mask); + field_mask |= value_bits; } // verify all possible bits were covered EXPECT_EQ(field_mask, value_mask); }; - ouster::impl::foreach_field(ls, verify_field); + ouster::impl::foreach_channel_field(ls, pf, verify_field); auto g = std::mt19937(0xdeadbeef); auto dinit_id = std::uniform_int_distribution(0, 0xFFFFFF); @@ -299,7 +292,7 @@ TEST_P(PacketWriterTest, packet_writer_randomize_test) { EXPECT_TRUE((ls.timestamp() == ls2.timestamp()).all()); EXPECT_TRUE((ls.measurement_id() == ls2.measurement_id()).all()); - ouster::impl::foreach_field(ls2, cmp_field{ls}); + ouster::impl::foreach_channel_field(ls2, pf, cmp_field{ls}); } TEST_P(PacketWriterTest, scans_to_packets_skips_dropped_packets_test) { @@ -324,15 +317,12 @@ TEST_P(PacketWriterTest, scans_to_packets_skips_dropped_packets_test) { std::fill(ls.status().data(), ls.status().data() + ls.status().size(), 0x1); ls.frame_id = 700; - auto randomise = [&](auto ref_field, ChanField i) { - // need this to skip RAW32_WORD* - if (i >= ChanField::RAW_HEADERS) return; - + auto randomise = [&](auto ref_field, const std::string& i) { // use static seed so that the test does not flake some day and // collectively piss off a bunch of angry developers randomize_field(ref_field, pw.field_value_mask(i), 0xdeadbeef); }; - ouster::impl::foreach_field(ls, randomise); + ouster::impl::foreach_channel_field(ls, pf, randomise); auto packets_orig = std::vector{}; ouster::impl::scan_to_packets(ls, pw, std::back_inserter(packets_orig), 0, @@ -437,7 +427,7 @@ TEST_P(PacketWriterDataTest, packet_writer_data_repr_test) { ASSERT_FALSE(repr_batcher(p, ls_repr)); } - ouster::impl::foreach_field(ls_repr, cmp_field{ls_orig}); + ouster::impl::foreach_channel_field(ls_repr, pw, cmp_field{ls_orig}); } TEST_P(PacketWriterDataTest, packet_writer_raw_headers_match_test) { diff --git a/tests/parsing_benchmark_test.cpp b/tests/parsing_benchmark_test.cpp index 5b9ebae7..cc6fca90 100644 --- a/tests/parsing_benchmark_test.cpp +++ b/tests/parsing_benchmark_test.cpp @@ -13,7 +13,7 @@ #include "ouster/types.h" #include "util.h" -using ouster::sensor::ChanField; +using namespace ouster::sensor; using str_pair = std::pair; class ParsingBenchmarkTestFixture : public ::testing::TestWithParam { @@ -42,7 +42,7 @@ namespace sensor_utils { struct parse_col { template - void operator()(Eigen::Ref> field, ChanField f, + void operator()(Eigen::Ref> field, const std::string& f, const sensor::packet_format& pf, const uint8_t* packet_buf) const { for (int icol = 0; icol < pf.columns_per_packet; icol++) { @@ -56,20 +56,20 @@ struct parse_col { template struct parse_block { template - void operator()(Eigen::Ref> field, ChanField f, + void operator()(Eigen::Ref> field, const std::string& f, const sensor::packet_format& pf, const uint8_t* packet_buf) const { pf.block_field(field, f, packet_buf); } }; -using HashMap = std::map; +using HashMap = std::map; // picked up from // https://wjngkoh.wordpress.com/2015/03/04/c-hash-function-for-eigen-matrix-and-vector/ struct matrix_hash { template - void operator()(Eigen::Ref> matrix, ChanField f, + void operator()(Eigen::Ref> matrix, const std::string& f, HashMap& map) const { size_t seed = 0; for (int i = 0; i < matrix.size(); ++i) { @@ -83,7 +83,7 @@ struct matrix_hash { struct set_zero { template - void operator()(Eigen::Ref> matrix, ChanField) const { + void operator()(Eigen::Ref> matrix, const std::string&) const { matrix.setZero(); } }; @@ -104,15 +104,16 @@ TEST_P(ParsingBenchmarkTestFixture, ParseBlockCorrectnessTest) { auto parse_and_hash = [&](auto parser) -> HashMap { // reset pcap.seek(0); - impl::foreach_field(ls, set_zero{}); + impl::foreach_channel_field(ls, pf, set_zero{}); // parse while (pcap.next_packet()) { if (pcap.current_info().dst_port == 7502) - impl::foreach_field(ls, parser, pf, pcap.current_data()); + impl::foreach_channel_field(ls, pf, parser, pf, + pcap.current_data()); } HashMap map; - impl::foreach_field(ls, matrix_hash{}, map); + impl::foreach_channel_field(ls, pf, matrix_hash{}, map); return map; }; @@ -171,7 +172,7 @@ TEST_P(ParsingBenchmarkTestFixture, ScanBatcherBenchTest) { auto parse = [&](auto parser, std::string name) { t.start(); for (const auto& packet : packets) - impl::foreach_field(ls, parser, pf, packet.data()); + impl::foreach_channel_field(ls, pf, parser, pf, packet.data()); t.stop(); mv[name](t.elapsed_microseconds()); }; diff --git a/tests/pcaps/OS-0-128_v3.0.1_1024x10.2.json b/tests/pcaps/OS-0-128_v3.0.1_1024x10.2.json new file mode 100644 index 00000000..c4523dd4 --- /dev/null +++ b/tests/pcaps/OS-0-128_v3.0.1_1024x10.2.json @@ -0,0 +1,529 @@ +{ + "beam_intrinsics": + { + "beam_altitude_angles": + [ + 44.3, + 43.75, + 43.02, + 42.13, + 41.34, + 40.76, + 40.02, + 39.2, + 38.39, + 37.81, + 37.09, + 36.26, + 35.48, + 34.88, + 34.16, + 33.36, + 32.57, + 31.96, + 31.24, + 30.46, + 29.68, + 29.06, + 28.35, + 27.6, + 26.82, + 26.18, + 25.48, + 24.71, + 23.97, + 23.34, + 22.63, + 21.88, + 21.12, + 20.48, + 19.79, + 19.06, + 18.31, + 17.66, + 16.97, + 16.22, + 15.5, + 14.85, + 14.14, + 13.42, + 12.7, + 12.06, + 11.35, + 10.65, + 9.92, + 9.26, + 8.58, + 7.86, + 7.15, + 6.48, + 5.79, + 5.09, + 4.37, + 3.71, + 3.02, + 2.33, + 1.61, + 0.94, + 0.25, + -0.45, + -1.16, + -1.83, + -2.52, + -3.24, + -3.92, + -4.59, + -5.29, + -6, + -6.69, + -7.38, + -8.07, + -8.76, + -9.46, + -10.18, + -10.88, + -11.54, + -12.24, + -12.96, + -13.67, + -14.35, + -15.04, + -15.78, + -16.48, + -17.12, + -17.84, + -18.6, + -19.3, + -19.96, + -20.66, + -21.4, + -22.11, + -22.78, + -23.48, + -24.27, + -24.97, + -25.65, + -26.34, + -27.12, + -27.84, + -28.5, + -29.19, + -30, + -30.72, + -31.37, + -32.07, + -32.9, + -33.63, + -34.28, + -34.98, + -35.82, + -36.57, + -37.19, + -37.91, + -38.78, + -39.52, + -40.16, + -40.87, + -41.76, + -42.51, + -43.14, + -43.88, + -44.79, + -45.55, + -46.16 + ], + "beam_azimuth_angles": + [ + 10.85, + 3.67, + -3.4, + -10.32, + 10.43, + 3.53, + -3.27, + -9.99, + 10.08, + 3.4, + -3.18, + -9.7, + 9.77, + 3.29, + -3.1, + -9.44, + 9.5, + 3.2, + -3.04, + -9.21, + 9.27, + 3.11, + -2.98, + -9.04, + 9.08, + 3.04, + -2.93, + -8.87, + 8.9, + 2.97, + -2.89, + -8.74, + 8.76, + 2.92, + -2.86, + -8.62, + 8.63, + 2.88, + -2.83, + -8.53, + 8.52, + 2.84, + -2.79, + -8.45, + 8.44, + 2.8, + -2.77, + -8.39, + 8.37, + 2.78, + -2.78, + -8.34, + 8.31, + 2.75, + -2.76, + -8.29, + 8.28, + 2.75, + -2.77, + -8.29, + 8.26, + 2.75, + -2.76, + -8.31, + 8.25, + 2.75, + -2.76, + -8.3, + 8.27, + 2.74, + -2.79, + -8.33, + 8.29, + 2.75, + -2.8, + -8.37, + 8.33, + 2.78, + -2.8, + -8.44, + 8.39, + 2.79, + -2.83, + -8.5, + 8.47, + 2.82, + -2.86, + -8.61, + 8.56, + 2.85, + -2.88, + -8.71, + 8.67, + 2.9, + -2.92, + -8.83, + 8.81, + 2.94, + -2.98, + -8.97, + 8.97, + 3, + -3.02, + -9.15, + 9.15, + 3.07, + -3.11, + -9.34, + 9.37, + 3.14, + -3.18, + -9.6, + 9.62, + 3.23, + -3.25, + -9.87, + 9.89, + 3.32, + -3.35, + -10.18, + 10.24, + 3.46, + -3.46, + -10.55, + 10.64, + 3.6, + -3.61, + -11.02 + ], + "beam_to_lidar_transform": + [ + 1, + 0, + 0, + 27.116, + 0, + 1, + 0, + 0, + 0, + 0, + 1, + 0, + 0, + 0, + 0, + 1 + ], + "lidar_origin_to_beam_origin_mm": 27.116 + }, + "calibration_status": + { + "reflectivity": + { + "timestamp": "2023-06-08T11:05:15", + "valid": true + } + }, + "config_params": + { + "azimuth_window": + [ + 0, + 360000 + ], + "columns_per_packet": 16, + "lidar_mode": "1024x10", + "multipurpose_io_mode": "OFF", + "nmea_baud_rate": "BAUD_9600", + "nmea_ignore_valid_char": 0, + "nmea_in_polarity": "ACTIVE_HIGH", + "nmea_leap_seconds": 0, + "operating_mode": "NORMAL", + "phase_lock_enable": false, + "phase_lock_offset": 0, + "signal_multiplier": 1, + "sync_pulse_in_polarity": "ACTIVE_HIGH", + "sync_pulse_out_angle": 360, + "sync_pulse_out_frequency": 1, + "sync_pulse_out_polarity": "ACTIVE_HIGH", + "sync_pulse_out_pulse_width": 10, + "timestamp_mode": "TIME_FROM_INTERNAL_OSC", + "udp_dest": "169.254.196.55", + "udp_port_imu": 7503, + "udp_port_lidar": 7502, + "udp_profile_imu": "LEGACY", + "udp_profile_lidar": "RNG19_RFL8_SIG16_NIR16_DUAL" + }, + "imu_intrinsics": + { + "imu_to_sensor_transform": + [ + 1, + 0, + 0, + -2.441, + 0, + 1, + 0, + -9.725, + 0, + 0, + 1, + 7.533, + 0, + 0, + 0, + 1 + ] + }, + "lidar_data_format": + { + "column_window": + [ + 0, + 1023 + ], + "columns_per_frame": 1024, + "columns_per_packet": 16, + "pixel_shift_by_row": + [ + 31, + 10, + -10, + -29, + 30, + 10, + -9, + -28, + 29, + 10, + -9, + -28, + 28, + 9, + -9, + -27, + 27, + 9, + -9, + -26, + 26, + 9, + -8, + -26, + 26, + 9, + -8, + -25, + 25, + 8, + -8, + -25, + 25, + 8, + -8, + -25, + 25, + 8, + -8, + -24, + 24, + 8, + -8, + -24, + 24, + 8, + -8, + -24, + 24, + 8, + -8, + -24, + 24, + 8, + -8, + -24, + 24, + 8, + -8, + -24, + 23, + 8, + -8, + -24, + 23, + 8, + -8, + -24, + 24, + 8, + -8, + -24, + 24, + 8, + -8, + -24, + 24, + 8, + -8, + -24, + 24, + 8, + -8, + -24, + 24, + 8, + -8, + -24, + 24, + 8, + -8, + -25, + 25, + 8, + -8, + -25, + 25, + 8, + -8, + -26, + 26, + 9, + -9, + -26, + 26, + 9, + -9, + -27, + 27, + 9, + -9, + -27, + 27, + 9, + -9, + -28, + 28, + 9, + -10, + -29, + 29, + 10, + -10, + -30, + 30, + 10, + -10, + -31 + ], + "pixels_per_column": 128, + "udp_profile_imu": "LEGACY", + "udp_profile_lidar": "RNG19_RFL8_SIG16_NIR16_DUAL" + }, + "lidar_intrinsics": + { + "lidar_to_sensor_transform": + [ + -1, + 0, + 0, + 0, + 0, + -1, + 0, + 0, + 0, + 0, + 1, + 38.195, + 0, + 0, + 0, + 1 + ] + }, + "ouster-sdk": + { + "client_version": "ouster_client 0.11.0c4", + "output_source": "collect_metadata" + }, + "sensor_info": + { + "build_date": "2023-02-09T04:49:03Z", + "build_rev": "v3.0.1", + "image_rev": "ousteros-image-prod-bootes-v3.0.1+20230209044733", + "initialization_id": 9266157, + "prod_line": "OS-0-128", + "prod_pn": "860-105000-07", + "prod_sn": "122322000613", + "status": "RUNNING" + } +} \ No newline at end of file diff --git a/tests/pcaps/OS-0-128_v3.0.1_1024x10.json b/tests/pcaps/OS-0-128_v3.0.1_1024x10.json new file mode 100644 index 00000000..cd188532 --- /dev/null +++ b/tests/pcaps/OS-0-128_v3.0.1_1024x10.json @@ -0,0 +1,529 @@ +{ + "beam_intrinsics": + { + "beam_altitude_angles": + [ + 44.3, + 43.75, + 43.02, + 42.13, + 41.34, + 40.76, + 40.02, + 39.2, + 38.39, + 37.81, + 37.09, + 36.26, + 35.48, + 34.88, + 34.16, + 33.36, + 32.57, + 31.96, + 31.24, + 30.46, + 29.68, + 29.06, + 28.35, + 27.6, + 26.82, + 26.18, + 25.48, + 24.71, + 23.97, + 23.34, + 22.63, + 21.88, + 21.12, + 20.48, + 19.79, + 19.06, + 18.31, + 17.66, + 16.97, + 16.22, + 15.5, + 14.85, + 14.14, + 13.42, + 12.7, + 12.06, + 11.35, + 10.65, + 9.92, + 9.26, + 8.58, + 7.86, + 7.15, + 6.48, + 5.79, + 5.09, + 4.37, + 3.71, + 3.02, + 2.33, + 1.61, + 0.94, + 0.25, + -0.45, + -1.16, + -1.83, + -2.52, + -3.24, + -3.92, + -4.59, + -5.29, + -6, + -6.69, + -7.38, + -8.07, + -8.76, + -9.46, + -10.18, + -10.88, + -11.54, + -12.24, + -12.96, + -13.67, + -14.35, + -15.04, + -15.78, + -16.48, + -17.12, + -17.84, + -18.6, + -19.3, + -19.96, + -20.66, + -21.4, + -22.11, + -22.78, + -23.48, + -24.27, + -24.97, + -25.65, + -26.34, + -27.12, + -27.84, + -28.5, + -29.19, + -30, + -30.72, + -31.37, + -32.07, + -32.9, + -33.63, + -34.28, + -34.98, + -35.82, + -36.57, + -37.19, + -37.91, + -38.78, + -39.52, + -40.16, + -40.87, + -41.76, + -42.51, + -43.14, + -43.88, + -44.79, + -45.55, + -46.16 + ], + "beam_azimuth_angles": + [ + 10.85, + 3.67, + -3.4, + -10.32, + 10.43, + 3.53, + -3.27, + -9.99, + 10.08, + 3.4, + -3.18, + -9.7, + 9.77, + 3.29, + -3.1, + -9.44, + 9.5, + 3.2, + -3.04, + -9.21, + 9.27, + 3.11, + -2.98, + -9.04, + 9.08, + 3.04, + -2.93, + -8.87, + 8.9, + 2.97, + -2.89, + -8.74, + 8.76, + 2.92, + -2.86, + -8.62, + 8.63, + 2.88, + -2.83, + -8.53, + 8.52, + 2.84, + -2.79, + -8.45, + 8.44, + 2.8, + -2.77, + -8.39, + 8.37, + 2.78, + -2.78, + -8.34, + 8.31, + 2.75, + -2.76, + -8.29, + 8.28, + 2.75, + -2.77, + -8.29, + 8.26, + 2.75, + -2.76, + -8.31, + 8.25, + 2.75, + -2.76, + -8.3, + 8.27, + 2.74, + -2.79, + -8.33, + 8.29, + 2.75, + -2.8, + -8.37, + 8.33, + 2.78, + -2.8, + -8.44, + 8.39, + 2.79, + -2.83, + -8.5, + 8.47, + 2.82, + -2.86, + -8.61, + 8.56, + 2.85, + -2.88, + -8.71, + 8.67, + 2.9, + -2.92, + -8.83, + 8.81, + 2.94, + -2.98, + -8.97, + 8.97, + 3, + -3.02, + -9.15, + 9.15, + 3.07, + -3.11, + -9.34, + 9.37, + 3.14, + -3.18, + -9.6, + 9.62, + 3.23, + -3.25, + -9.87, + 9.89, + 3.32, + -3.35, + -10.18, + 10.24, + 3.46, + -3.46, + -10.55, + 10.64, + 3.6, + -3.61, + -11.02 + ], + "beam_to_lidar_transform": + [ + 1, + 0, + 0, + 27.116, + 0, + 1, + 0, + 0, + 0, + 0, + 1, + 0, + 0, + 0, + 0, + 1 + ], + "lidar_origin_to_beam_origin_mm": 27.116 + }, + "calibration_status": + { + "reflectivity": + { + "timestamp": "2023-06-08T11:05:15", + "valid": true + } + }, + "config_params": + { + "azimuth_window": + [ + 0, + 360000 + ], + "columns_per_packet": 16, + "lidar_mode": "1024x10", + "multipurpose_io_mode": "OFF", + "nmea_baud_rate": "BAUD_9600", + "nmea_ignore_valid_char": 0, + "nmea_in_polarity": "ACTIVE_HIGH", + "nmea_leap_seconds": 0, + "operating_mode": "NORMAL", + "phase_lock_enable": false, + "phase_lock_offset": 0, + "signal_multiplier": 1, + "sync_pulse_in_polarity": "ACTIVE_HIGH", + "sync_pulse_out_angle": 360, + "sync_pulse_out_frequency": 1, + "sync_pulse_out_polarity": "ACTIVE_HIGH", + "sync_pulse_out_pulse_width": 10, + "timestamp_mode": "TIME_FROM_INTERNAL_OSC", + "udp_dest": "169.254.196.55", + "udp_port_imu": 7503, + "udp_port_lidar": 7502, + "udp_profile_imu": "LEGACY", + "udp_profile_lidar": "RNG19_RFL8_SIG16_NIR16_DUAL" + }, + "imu_intrinsics": + { + "imu_to_sensor_transform": + [ + 1, + 0, + 0, + -2.441, + 0, + 1, + 0, + -9.725, + 0, + 0, + 1, + 7.533, + 0, + 0, + 0, + 1 + ] + }, + "lidar_data_format": + { + "column_window": + [ + 0, + 1023 + ], + "columns_per_frame": 1024, + "columns_per_packet": 16, + "pixel_shift_by_row": + [ + 31, + 10, + -10, + -29, + 30, + 10, + -9, + -28, + 29, + 10, + -9, + -28, + 28, + 9, + -9, + -27, + 27, + 9, + -9, + -26, + 26, + 9, + -8, + -26, + 26, + 9, + -8, + -25, + 25, + 8, + -8, + -25, + 25, + 8, + -8, + -25, + 25, + 8, + -8, + -24, + 24, + 8, + -8, + -24, + 24, + 8, + -8, + -24, + 24, + 8, + -8, + -24, + 24, + 8, + -8, + -24, + 24, + 8, + -8, + -24, + 23, + 8, + -8, + -24, + 23, + 8, + -8, + -24, + 24, + 8, + -8, + -24, + 24, + 8, + -8, + -24, + 24, + 8, + -8, + -24, + 24, + 8, + -8, + -24, + 24, + 8, + -8, + -24, + 24, + 8, + -8, + -25, + 25, + 8, + -8, + -25, + 25, + 8, + -8, + -26, + 26, + 9, + -9, + -26, + 26, + 9, + -9, + -27, + 27, + 9, + -9, + -27, + 27, + 9, + -9, + -28, + 28, + 9, + -10, + -29, + 29, + 10, + -10, + -30, + 30, + 10, + -10, + -31 + ], + "pixels_per_column": 128, + "udp_profile_imu": "LEGACY", + "udp_profile_lidar": "RNG19_RFL8_SIG16_NIR16_DUAL" + }, + "lidar_intrinsics": + { + "lidar_to_sensor_transform": + [ + -1, + 0, + 0, + 0, + 0, + -1, + 0, + 0, + 0, + 0, + 1, + 38.195, + 0, + 0, + 0, + 1 + ] + }, + "ouster-sdk": + { + "client_version": "ouster_client 0.11.0c4", + "output_source": "collect_metadata" + }, + "sensor_info": + { + "build_date": "2023-02-09T04:49:03Z", + "build_rev": "v3.0.1", + "image_rev": "ousteros-image-prod-bootes-v3.0.1+20230209044733", + "initialization_id": 9266157, + "prod_line": "OS-0-128", + "prod_pn": "860-105000-07", + "prod_sn": "122322000614", + "status": "RUNNING" + } +} \ No newline at end of file diff --git a/tests/pcaps/OS-0-128_v3.0.1_1024x10.pcap b/tests/pcaps/OS-0-128_v3.0.1_1024x10.pcap new file mode 100644 index 00000000..e69de29b diff --git a/tests/pcaps/OS-0-128_v3.0.1_1024x10_20240321_125947.2.json b/tests/pcaps/OS-0-128_v3.0.1_1024x10_20240321_125947.2.json new file mode 100644 index 00000000..cd188532 --- /dev/null +++ b/tests/pcaps/OS-0-128_v3.0.1_1024x10_20240321_125947.2.json @@ -0,0 +1,529 @@ +{ + "beam_intrinsics": + { + "beam_altitude_angles": + [ + 44.3, + 43.75, + 43.02, + 42.13, + 41.34, + 40.76, + 40.02, + 39.2, + 38.39, + 37.81, + 37.09, + 36.26, + 35.48, + 34.88, + 34.16, + 33.36, + 32.57, + 31.96, + 31.24, + 30.46, + 29.68, + 29.06, + 28.35, + 27.6, + 26.82, + 26.18, + 25.48, + 24.71, + 23.97, + 23.34, + 22.63, + 21.88, + 21.12, + 20.48, + 19.79, + 19.06, + 18.31, + 17.66, + 16.97, + 16.22, + 15.5, + 14.85, + 14.14, + 13.42, + 12.7, + 12.06, + 11.35, + 10.65, + 9.92, + 9.26, + 8.58, + 7.86, + 7.15, + 6.48, + 5.79, + 5.09, + 4.37, + 3.71, + 3.02, + 2.33, + 1.61, + 0.94, + 0.25, + -0.45, + -1.16, + -1.83, + -2.52, + -3.24, + -3.92, + -4.59, + -5.29, + -6, + -6.69, + -7.38, + -8.07, + -8.76, + -9.46, + -10.18, + -10.88, + -11.54, + -12.24, + -12.96, + -13.67, + -14.35, + -15.04, + -15.78, + -16.48, + -17.12, + -17.84, + -18.6, + -19.3, + -19.96, + -20.66, + -21.4, + -22.11, + -22.78, + -23.48, + -24.27, + -24.97, + -25.65, + -26.34, + -27.12, + -27.84, + -28.5, + -29.19, + -30, + -30.72, + -31.37, + -32.07, + -32.9, + -33.63, + -34.28, + -34.98, + -35.82, + -36.57, + -37.19, + -37.91, + -38.78, + -39.52, + -40.16, + -40.87, + -41.76, + -42.51, + -43.14, + -43.88, + -44.79, + -45.55, + -46.16 + ], + "beam_azimuth_angles": + [ + 10.85, + 3.67, + -3.4, + -10.32, + 10.43, + 3.53, + -3.27, + -9.99, + 10.08, + 3.4, + -3.18, + -9.7, + 9.77, + 3.29, + -3.1, + -9.44, + 9.5, + 3.2, + -3.04, + -9.21, + 9.27, + 3.11, + -2.98, + -9.04, + 9.08, + 3.04, + -2.93, + -8.87, + 8.9, + 2.97, + -2.89, + -8.74, + 8.76, + 2.92, + -2.86, + -8.62, + 8.63, + 2.88, + -2.83, + -8.53, + 8.52, + 2.84, + -2.79, + -8.45, + 8.44, + 2.8, + -2.77, + -8.39, + 8.37, + 2.78, + -2.78, + -8.34, + 8.31, + 2.75, + -2.76, + -8.29, + 8.28, + 2.75, + -2.77, + -8.29, + 8.26, + 2.75, + -2.76, + -8.31, + 8.25, + 2.75, + -2.76, + -8.3, + 8.27, + 2.74, + -2.79, + -8.33, + 8.29, + 2.75, + -2.8, + -8.37, + 8.33, + 2.78, + -2.8, + -8.44, + 8.39, + 2.79, + -2.83, + -8.5, + 8.47, + 2.82, + -2.86, + -8.61, + 8.56, + 2.85, + -2.88, + -8.71, + 8.67, + 2.9, + -2.92, + -8.83, + 8.81, + 2.94, + -2.98, + -8.97, + 8.97, + 3, + -3.02, + -9.15, + 9.15, + 3.07, + -3.11, + -9.34, + 9.37, + 3.14, + -3.18, + -9.6, + 9.62, + 3.23, + -3.25, + -9.87, + 9.89, + 3.32, + -3.35, + -10.18, + 10.24, + 3.46, + -3.46, + -10.55, + 10.64, + 3.6, + -3.61, + -11.02 + ], + "beam_to_lidar_transform": + [ + 1, + 0, + 0, + 27.116, + 0, + 1, + 0, + 0, + 0, + 0, + 1, + 0, + 0, + 0, + 0, + 1 + ], + "lidar_origin_to_beam_origin_mm": 27.116 + }, + "calibration_status": + { + "reflectivity": + { + "timestamp": "2023-06-08T11:05:15", + "valid": true + } + }, + "config_params": + { + "azimuth_window": + [ + 0, + 360000 + ], + "columns_per_packet": 16, + "lidar_mode": "1024x10", + "multipurpose_io_mode": "OFF", + "nmea_baud_rate": "BAUD_9600", + "nmea_ignore_valid_char": 0, + "nmea_in_polarity": "ACTIVE_HIGH", + "nmea_leap_seconds": 0, + "operating_mode": "NORMAL", + "phase_lock_enable": false, + "phase_lock_offset": 0, + "signal_multiplier": 1, + "sync_pulse_in_polarity": "ACTIVE_HIGH", + "sync_pulse_out_angle": 360, + "sync_pulse_out_frequency": 1, + "sync_pulse_out_polarity": "ACTIVE_HIGH", + "sync_pulse_out_pulse_width": 10, + "timestamp_mode": "TIME_FROM_INTERNAL_OSC", + "udp_dest": "169.254.196.55", + "udp_port_imu": 7503, + "udp_port_lidar": 7502, + "udp_profile_imu": "LEGACY", + "udp_profile_lidar": "RNG19_RFL8_SIG16_NIR16_DUAL" + }, + "imu_intrinsics": + { + "imu_to_sensor_transform": + [ + 1, + 0, + 0, + -2.441, + 0, + 1, + 0, + -9.725, + 0, + 0, + 1, + 7.533, + 0, + 0, + 0, + 1 + ] + }, + "lidar_data_format": + { + "column_window": + [ + 0, + 1023 + ], + "columns_per_frame": 1024, + "columns_per_packet": 16, + "pixel_shift_by_row": + [ + 31, + 10, + -10, + -29, + 30, + 10, + -9, + -28, + 29, + 10, + -9, + -28, + 28, + 9, + -9, + -27, + 27, + 9, + -9, + -26, + 26, + 9, + -8, + -26, + 26, + 9, + -8, + -25, + 25, + 8, + -8, + -25, + 25, + 8, + -8, + -25, + 25, + 8, + -8, + -24, + 24, + 8, + -8, + -24, + 24, + 8, + -8, + -24, + 24, + 8, + -8, + -24, + 24, + 8, + -8, + -24, + 24, + 8, + -8, + -24, + 23, + 8, + -8, + -24, + 23, + 8, + -8, + -24, + 24, + 8, + -8, + -24, + 24, + 8, + -8, + -24, + 24, + 8, + -8, + -24, + 24, + 8, + -8, + -24, + 24, + 8, + -8, + -24, + 24, + 8, + -8, + -25, + 25, + 8, + -8, + -25, + 25, + 8, + -8, + -26, + 26, + 9, + -9, + -26, + 26, + 9, + -9, + -27, + 27, + 9, + -9, + -27, + 27, + 9, + -9, + -28, + 28, + 9, + -10, + -29, + 29, + 10, + -10, + -30, + 30, + 10, + -10, + -31 + ], + "pixels_per_column": 128, + "udp_profile_imu": "LEGACY", + "udp_profile_lidar": "RNG19_RFL8_SIG16_NIR16_DUAL" + }, + "lidar_intrinsics": + { + "lidar_to_sensor_transform": + [ + -1, + 0, + 0, + 0, + 0, + -1, + 0, + 0, + 0, + 0, + 1, + 38.195, + 0, + 0, + 0, + 1 + ] + }, + "ouster-sdk": + { + "client_version": "ouster_client 0.11.0c4", + "output_source": "collect_metadata" + }, + "sensor_info": + { + "build_date": "2023-02-09T04:49:03Z", + "build_rev": "v3.0.1", + "image_rev": "ousteros-image-prod-bootes-v3.0.1+20230209044733", + "initialization_id": 9266157, + "prod_line": "OS-0-128", + "prod_pn": "860-105000-07", + "prod_sn": "122322000614", + "status": "RUNNING" + } +} \ No newline at end of file diff --git a/tests/pcaps/OS-0-128_v3.0.1_1024x10_20240321_125947.json b/tests/pcaps/OS-0-128_v3.0.1_1024x10_20240321_125947.json new file mode 100644 index 00000000..3f6abdb8 --- /dev/null +++ b/tests/pcaps/OS-0-128_v3.0.1_1024x10_20240321_125947.json @@ -0,0 +1,530 @@ +{ + "beam_intrinsics": + { + "beam_altitude_angles": + [ + 44.3, + 43.75, + 43.02, + 42.13, + 41.34, + 40.76, + 40.02, + 39.2, + 38.39, + 37.81, + 37.09, + 36.26, + 35.48, + 34.88, + 34.16, + 33.36, + 32.57, + 31.96, + 31.24, + 30.46, + 29.68, + 29.06, + 28.35, + 27.6, + 26.82, + 26.18, + 25.48, + 24.71, + 23.97, + 23.34, + 22.63, + 21.88, + 21.12, + 20.48, + 19.79, + 19.06, + 18.31, + 17.66, + 16.97, + 16.22, + 15.5, + 14.85, + 14.14, + 13.42, + 12.7, + 12.06, + 11.35, + 10.65, + 9.92, + 9.26, + 8.58, + 7.86, + 7.15, + 6.48, + 5.79, + 5.09, + 4.37, + 3.71, + 3.02, + 2.33, + 1.61, + 0.94, + 0.25, + -0.45, + -1.16, + -1.83, + -2.52, + -3.24, + -3.92, + -4.59, + -5.29, + -6, + -6.69, + -7.38, + -8.07, + -8.76, + -9.46, + -10.18, + -10.88, + -11.54, + -12.24, + -12.96, + -13.67, + -14.35, + -15.04, + -15.78, + -16.48, + -17.12, + -17.84, + -18.6, + -19.3, + -19.96, + -20.66, + -21.4, + -22.11, + -22.78, + -23.48, + -24.27, + -24.97, + -25.65, + -26.34, + -27.12, + -27.84, + -28.5, + -29.19, + -30, + -30.72, + -31.37, + -32.07, + -32.9, + -33.63, + -34.28, + -34.98, + -35.82, + -36.57, + -37.19, + -37.91, + -38.78, + -39.52, + -40.16, + -40.87, + -41.76, + -42.51, + -43.14, + -43.88, + -44.79, + -45.55, + -46.16 + ], + "beam_azimuth_angles": + [ + 10.85, + 3.67, + -3.4, + -10.32, + 10.43, + 3.53, + -3.27, + -9.99, + 10.08, + 3.4, + -3.18, + -9.7, + 9.77, + 3.29, + -3.1, + -9.44, + 9.5, + 3.2, + -3.04, + -9.21, + 9.27, + 3.11, + -2.98, + -9.04, + 9.08, + 3.04, + -2.93, + -8.87, + 8.9, + 2.97, + -2.89, + -8.74, + 8.76, + 2.92, + -2.86, + -8.62, + 8.63, + 2.88, + -2.83, + -8.53, + 8.52, + 2.84, + -2.79, + -8.45, + 8.44, + 2.8, + -2.77, + -8.39, + 8.37, + 2.78, + -2.78, + -8.34, + 8.31, + 2.75, + -2.76, + -8.29, + 8.28, + 2.75, + -2.77, + -8.29, + 8.26, + 2.75, + -2.76, + -8.31, + 8.25, + 2.75, + -2.76, + -8.3, + 8.27, + 2.74, + -2.79, + -8.33, + 8.29, + 2.75, + -2.8, + -8.37, + 8.33, + 2.78, + -2.8, + -8.44, + 8.39, + 2.79, + -2.83, + -8.5, + 8.47, + 2.82, + -2.86, + -8.61, + 8.56, + 2.85, + -2.88, + -8.71, + 8.67, + 2.9, + -2.92, + -8.83, + 8.81, + 2.94, + -2.98, + -8.97, + 8.97, + 3, + -3.02, + -9.15, + 9.15, + 3.07, + -3.11, + -9.34, + 9.37, + 3.14, + -3.18, + -9.6, + 9.62, + 3.23, + -3.25, + -9.87, + 9.89, + 3.32, + -3.35, + -10.18, + 10.24, + 3.46, + -3.46, + -10.55, + 10.64, + 3.6, + -3.61, + -11.02 + ], + "beam_to_lidar_transform": + [ + 1, + 0, + 0, + 27.116, + 0, + 1, + 0, + 0, + 0, + 0, + 1, + 0, + 0, + 0, + 0, + 1 + ], + "lidar_origin_to_beam_origin_mm": 27.116 + }, + "calibration_status": + { + "reflectivity": + { + "timestamp": "2023-06-08T11:05:15", + "valid": true + } + }, + "config_params": + { + "azimuth_window": + [ + 0, + 360000 + ], + "columns_per_packet": 16, + "lidar_mode": "1024x10", + "multipurpose_io_mode": "OFF", + "nmea_baud_rate": "BAUD_9600", + "nmea_ignore_valid_char": 0, + "nmea_in_polarity": "ACTIVE_HIGH", + "nmea_leap_seconds": 0, + "operating_mode": "NORMAL", + "phase_lock_enable": false, + "phase_lock_offset": 0, + "signal_multiplier": 1, + "sync_pulse_in_polarity": "ACTIVE_HIGH", + "sync_pulse_out_angle": 360, + "sync_pulse_out_frequency": 1, + "sync_pulse_out_polarity": "ACTIVE_HIGH", + "sync_pulse_out_pulse_width": 10, + "timestamp_mode": "TIME_FROM_INTERNAL_OSC", + "udp_dest": "169.254.196.55", + "udp_port_imu": 7503, + "udp_port_lidar": 7502, + "udp_profile_imu": "LEGACY", + "udp_profile_lidar": "RNG19_RFL8_SIG16_NIR16_DUAL" + }, + "imu_intrinsics": + { + "imu_to_sensor_transform": + [ + 1, + 0, + 0, + -2.441, + 0, + 1, + 0, + -9.725, + 0, + 0, + 1, + 7.533, + 0, + 0, + 0, + 1 + ] + }, + "lidar_data_format": + { + "column_window": + [ + 0, + 1023 + ], + "columns_per_frame": 1024, + "columns_per_packet": 16, + "pixel_shift_by_row": + [ + 31, + 10, + -10, + -29, + 30, + 10, + -9, + -28, + 29, + 10, + -9, + -28, + 28, + 9, + -9, + -27, + 27, + 9, + -9, + -26, + 26, + 9, + -8, + -26, + 26, + 9, + -8, + -25, + 25, + 8, + -8, + -25, + 25, + 8, + -8, + -25, + 25, + 8, + -8, + -24, + 24, + 8, + -8, + -24, + 24, + 8, + -8, + -24, + 24, + 8, + -8, + -24, + 24, + 8, + -8, + -24, + 24, + 8, + -8, + -24, + 23, + 8, + -8, + -24, + 23, + 8, + -8, + -24, + 24, + 8, + -8, + -24, + 24, + 8, + -8, + -24, + 24, + 8, + -8, + -24, + 24, + 8, + -8, + -24, + 24, + 8, + -8, + -24, + 24, + 8, + -8, + -25, + 25, + 8, + -8, + -25, + 25, + 8, + -8, + -26, + 26, + 9, + -9, + -26, + 26, + 9, + -9, + -27, + 27, + 9, + -9, + -27, + 27, + 9, + -9, + -28, + 28, + 9, + -10, + -29, + 29, + 10, + -10, + -30, + 30, + 10, + -10, + -31 + ], + "pixels_per_column": 128, + "udp_profile_imu": "LEGACY", + "udp_profile_lidar": "RNG19_RFL8_SIG16_NIR16_DUAL" + }, + "lidar_intrinsics": + { + "lidar_to_sensor_transform": + [ + -1, + 0, + 0, + 0, + 0, + -1, + 0, + 0, + 0, + 0, + 1, + 38.195, + 0, + 0, + 0, + 1 + ] + }, + "ouster-sdk": + { + "client_version": "ouster_client 0.11.0c4", + "output_source": "collect_metadata" + }, + "sensor_info": + { + "build_date": "2023-02-09T04:49:03Z", + "build_rev": "v3.0.1", + "image_rev": "ousteros-image-prod-bootes-v3.0.1+20230209044733", + "initialization_id": 9266157, + "prod_line": "OS-0-128", + "prod_pn": "860-105000-07", + "prod_sn": "122322000614", + "status": "RUNNING" + } +}New data to be appended +New data to be appended diff --git a/tests/pcaps/OS-0-128_v3.0.1_1024x10_20240321_125947.pcap b/tests/pcaps/OS-0-128_v3.0.1_1024x10_20240321_125947.pcap new file mode 100644 index 00000000..e69de29b diff --git a/tests/pcaps/duplicate_id.json b/tests/pcaps/duplicate_id.json new file mode 100644 index 00000000..101d8dcb --- /dev/null +++ b/tests/pcaps/duplicate_id.json @@ -0,0 +1,506 @@ +{ + "beam_intrinsics": + { + "beam_altitude_angles": + [ + 10.96, + 10.79, + 10.63, + 10.47, + 10.3, + 10.15, + 9.96, + 9.82, + 9.63, + 9.46, + 9.3, + 9.15, + 8.94, + 8.78, + 8.62, + 8.46, + 8.27, + 8.1, + 7.94, + 7.78, + 7.58, + 7.43, + 7.26, + 7.09, + 6.91, + 6.73, + 6.57, + 6.41, + 6.22, + 6.04, + 5.87, + 5.72, + 5.51, + 5.34, + 5.19, + 5.02, + 4.83, + 4.66, + 4.5, + 4.34, + 4.14, + 3.98, + 3.79, + 3.63, + 3.44, + 3.26, + 3.1, + 2.93, + 2.74, + 2.58, + 2.41, + 2.25, + 2.05, + 1.88, + 1.71, + 1.54, + 1.35, + 1.18, + 1.02, + 0.84, + 0.65, + 0.48, + 0.31, + 0.14, + -0.04, + -0.21, + -0.39, + -0.56, + -0.74, + -0.92, + -1.09, + -1.25, + -1.45, + -1.62, + -1.78, + -1.96, + -2.14, + -2.32, + -2.47, + -2.64, + -2.85, + -3, + -3.17, + -3.34, + -3.53, + -3.71, + -3.88, + -4.04, + -4.23, + -4.39, + -4.57, + -4.75, + -4.91, + -5.08, + -5.25, + -5.42, + -5.6, + -5.78, + -5.97, + -6.12, + -6.3, + -6.47, + -6.63, + -6.8, + -6.98, + -7.15, + -7.33, + -7.5, + -7.68, + -7.86, + -8, + -8.17, + -8.36, + -8.54, + -8.69, + -8.87, + -9.04, + -9.2, + -9.37, + -9.54, + -9.72, + -9.88, + -10.05, + -10.22, + -10.4, + -10.55, + -10.71, + -10.88 + ], + "beam_azimuth_angles": + [ + 2.11, + 0.73, + -0.65, + -2.04, + 2.12, + 0.74, + -0.66, + -2.04, + 2.11, + 0.72, + -0.66, + -2.04, + 2.1, + 0.71, + -0.66, + -2.04, + 2.1, + 0.73, + -0.66, + -2.04, + 2.12, + 0.73, + -0.67, + -2.04, + 2.11, + 0.71, + -0.68, + -2.03, + 2.11, + 0.72, + -0.65, + -2.05, + 2.09, + 0.72, + -0.66, + -2.05, + 2.1, + 0.72, + -0.67, + -2.06, + 2.09, + 0.73, + -0.67, + -2.07, + 2.09, + 0.71, + -0.69, + -2.07, + 2.09, + 0.71, + -0.67, + -2.05, + 2.09, + 0.7, + -0.69, + -2.07, + 2.08, + 0.7, + -0.69, + -2.07, + 2.09, + 0.7, + -0.69, + -2.07, + 2.09, + 0.72, + -0.68, + -2.07, + 2.08, + 0.7, + -0.69, + -2.07, + 2.08, + 0.69, + -0.69, + -2.07, + 2.07, + 0.68, + -0.69, + -2.06, + 2.07, + 0.69, + -0.7, + -2.07, + 2.07, + 0.69, + -0.69, + -2.09, + 2.07, + 0.69, + -0.7, + -2.08, + 2.06, + 0.67, + -0.7, + -2.08, + 2.07, + 0.67, + -0.71, + -2.08, + 2.05, + 0.67, + -0.7, + -2.07, + 2.06, + 0.67, + -0.72, + -2.08, + 2.06, + 0.67, + -0.7, + -2.11, + 2.04, + 0.67, + -0.71, + -2.1, + 2.05, + 0.66, + -0.71, + -2.1, + 2.04, + 0.67, + -0.71, + -2.11, + 2.03, + 0.66, + -0.72, + -2.11 + ], + "lidar_origin_to_beam_origin_mm": 13.762 + }, + "calibration_status": + { + "reflectivity": + { + "timestamp": "", + "valid": false + } + }, + "client_version": "ouster_client 0.9.0", + "config_params": + { + "azimuth_window": + [ + 0, + 360000 + ], + "columns_per_packet": 16, + "lidar_mode": "1024x10", + "multipurpose_io_mode": "OFF", + "nmea_baud_rate": "BAUD_9600", + "nmea_ignore_valid_char": 0, + "nmea_in_polarity": "ACTIVE_HIGH", + "nmea_leap_seconds": 0, + "operating_mode": "NORMAL", + "phase_lock_enable": true, + "phase_lock_offset": 0, + "signal_multiplier": 1, + "sync_pulse_in_polarity": "ACTIVE_HIGH", + "sync_pulse_out_angle": 360, + "sync_pulse_out_frequency": 1, + "sync_pulse_out_polarity": "ACTIVE_HIGH", + "sync_pulse_out_pulse_width": 0, + "timestamp_mode": "TIME_FROM_PTP_1588", + "udp_dest": "10.5.6.1", + "udp_port_imu": 48887, + "udp_port_lidar": 34636, + "udp_profile_imu": "LEGACY", + "udp_profile_lidar": "RNG15_RFL8_NIR8" + }, + "imu_intrinsics": + { + "imu_to_sensor_transform": + [ + 1, + 0, + 0, + 6.253, + 0, + 1, + 0, + -11.775, + 0, + 0, + 1, + 11.645, + 0, + 0, + 0, + 1 + ] + }, + "lidar_data_format": + { + "column_window": + [ + 0, + 1023 + ], + "columns_per_frame": 1024, + "columns_per_packet": 16, + "pixel_shift_by_row": + [ + 6, + 2, + -2, + -6, + 6, + 2, + -2, + -6, + 6, + 2, + -2, + -6, + 6, + 2, + -2, + -6, + 6, + 2, + -2, + -6, + 6, + 2, + -2, + -6, + 6, + 2, + -2, + -6, + 6, + 2, + -2, + -6, + 6, + 2, + -2, + -6, + 6, + 2, + -2, + -6, + 6, + 2, + -2, + -6, + 6, + 2, + -2, + -6, + 6, + 2, + -2, + -6, + 6, + 2, + -2, + -6, + 6, + 2, + -2, + -6, + 6, + 2, + -2, + -6, + 6, + 2, + -2, + -6, + 6, + 2, + -2, + -6, + 6, + 2, + -2, + -6, + 6, + 2, + -2, + -6, + 6, + 2, + -2, + -6, + 6, + 2, + -2, + -6, + 6, + 2, + -2, + -6, + 6, + 2, + -2, + -6, + 6, + 2, + -2, + -6, + 6, + 2, + -2, + -6, + 6, + 2, + -2, + -6, + 6, + 2, + -2, + -6, + 6, + 2, + -2, + -6, + 6, + 2, + -2, + -6, + 6, + 2, + -2, + -6, + 6, + 2, + -2, + -6 + ], + "pixels_per_column": 128, + "udp_profile_imu": "LEGACY", + "udp_profile_lidar": "RNG15_RFL8_NIR8" + }, + "lidar_intrinsics": + { + "lidar_to_sensor_transform": + [ + -1, + 0, + 0, + 0, + 0, + -1, + 0, + 0, + 0, + 0, + 1, + 78.296, + 0, + 0, + 0, + 1 + ] + }, + "sensor_info": + { + "build_date": "2022-09-21T17:47:45Z", + "build_rev": "v2.4.0", + "image_rev": "ousteros-image-prod-aries-v2.4.0+20220921174636", + "initialization_id": 8247312, + "prod_line": "OS-2-128", + "prod_pn": "840-103576-06", + "prod_sn": "992219000042", + "status": "RUNNING" + } +} diff --git a/tests/pcaps/duplicate_id.pcap b/tests/pcaps/duplicate_id.pcap new file mode 100644 index 0000000000000000000000000000000000000000..f734cf3a4c8bd9702b0ad06bd2fdc1fae47892b1 GIT binary patch literal 10310 zcmb7~33OCd*6#}e>)d(1b*t*uoT`K*WUkEfKt?hrgd~K_6Pf3E20{Rt5fKGMw4E9i z5l}=yP!v&U^=n63R77p1#s2iu7Tb4D`nkTh-g@ixd$rbH=iH&HlAPc9@BiL6@4Wo< z!8?onO8I+m zTMqo*(q=#J_-c1kULfw70FMif+Jp0ME>R=Gabtd@BO?7Fdu{F`zEO>lI6n89nxA{p zo>O|$$1Cn0pOVrOYF_!EX=zT+5HtNnp53w$}8HWXW?3D5@*zkQaGlT=!v8` znJU_&n$lla9MzvlWY#*F&xO~5{UkhSP@zi$@jJ6K^u}gMqN*yT+I1fI;^l3!PUprbP|zk2{4r+tWX*<}R{CEw>YqO~k|3nw$+ z*y)|$j#!AA4bI4~ENIP#^EFK&1)Vd&O#>$blRGcDni`j0ei?~ihVWWr~3~7Cb#MQJ=Z%gKByKm0R_T_~qJXY2RJEZ($_PU%KcE6P` z`POdSty+@HOx1PQ?5SuUBy_1+EAmaHrKw8-{m;4*k{i^Vrf^dxTK|DVd^LdB$F-7nmpC3gk>VV>0{iBq+;jlin`xlXD36!wVRuYLw45Wc7}?$h z&6BnHt<~4^I;%DWFGu@sbx}cm>JK5cjYor9(7K~&`yiyD=xlInTUJ15|B`^zUU)A5 ziYs;JjQ{ejzu0_tMc6GH5}c`fHMY11AGf7+p0X{U8F#pQW9(t~-(`;~?Y2j>zGOG{ z6xzI7kJ-|jlI-bgo^yruJZKB7{>WC8tJ?jmpYrt0x@L>dIg9JveYg;_43`Vh_x$9W zRA^pWl#+(aVemL>p+ijrj#JP8K8Z){sbx`~ z(Z%pM&19-;c;22-nB$oomZ7dLAxu>j@O%L8nHd_O7SsY$DdHdzMuJVvEdN4H%{S|@ zIETzE`_<^T8lJD-Az|ro42BW(7m@Khp}5jdK;_pof&Xvizc+V*=KDMUugd?;$z4NG z`JY@czuS?bN8iLi%=;J>1n0|PAXP8^&DS6Og-}L7t6qBCHxFfqoGy1@861ZW`}*X= zu?ebpEdPjSz-r4_^V(!S$=^JsKsp?Bv zY}+NvWj>O<<(61`_@t;b?+{IERtc7YrGnDJ2#q~|;gyz)oNm*8j^FV|j+yP_)J?Oj zyr+Scw->VfW=~e$+{jS(rZW7@GKRdHU_zfPr^(?mTAE9u$z5hz>f225V_g(ARYl1Y z36$J%fgC(}fW)Wvk;3#kk{=UEq7x&rshdRKo@)d%&`m_FSVK^$`Gh$wj9`Ml#Hon) za6a@EoDVpS)1j5P5Ltxtv5B}8vjnHzH!&*uON@(n8;%z+&U+UorgveT84Va8T!#rB zM$8y%!uSvZV`J{b__!-TX*&nh(HDSF|0IwbI)OCQ4dmfA!1vYyp*spRPPl>4>;c$5 zGZ0&DYRk8NtJQT~({k-ngG_N!jE;HIaMaMntH6u31z;D(it4FPB?^TPgMKJkHzpP5t z%T#&Tq}tkV=#{!ry|pb`?;QI=S<(`&v#$A3Hm~}VZ0gx0>q{3%u8t2x^O~ncL(3M? z-k2vCx^#lQ?-yREyvlpbzRFp4-pBFxb#v@Y8OLnwXHAEzSbj?aORe=_r5&w|xHpCo z?+Rh??II&hm(tWw8qG|l(8NxOUh()wiiIrjQ>7F$n?*707fEh#tH5(ZksW#42rtwG1>?y%K|g8~sVUEfC}=hnDWO7;h( zr9a>sRQOCpQuTUw$gLKpdg<&uR!-fm9VSgP1@%gY*V* zHx8pB$!TtW)Wf4I$!2XoVl`H?R%hn{i?b768@*Tcu8vYY`d&7g`XAHtb$xnMyHRiN zJFb|zoH}FUyRx{syDqrR651r%8 zBl|er_A$=9btT8oO|ZJX$t*kP!z$Ajmf2gzuzTGMzcGTrHQfYQWJWXxp zXkF(x#Z1>w<r{PLW2u=rm zjWN;hV`A7V7#;H##x2=|@kyhYl30N;K{XibW5L8^1=EEu#)LSeCVv7{;UzG1>L^gE zo&;=77cfqA0CliNu(ql#~gN4SJXcml#~X?w${|mNd1fV2ZbR2h~||r za!Y~&(i;8aP?m^>5f3AS^7F2QW5I(V?!}G7 zjp%#3m)Tqchppyn-l`kuw&({wRy_t>c>g+piG>q-jo9klrY7?*b8aSa?2PK_% z#(*g3_C`-eDj zTN8(^Z()_g(Ja5o$f~m{OW&8wWV{y4a2r{M*m#R3cV^Q}e+12M(bL3wf);ukDQ2vU z>N*}r;gf+B-}?kf&ut^AfvqIHzLVr87L#9I>Aj{Ao!tbf+@%&#Q0c3m$j6T zga3?EAs3;NJcly@kHIWbj?-Zm_Div z6T{0f+QSoLQ&`Lpz6g_|egItjMZh(`3JilU0IKLIKo$1@-BbsddA<(l*4G2JD*)(P z6yQH)146$7#D*`mV%JAnsr#x{?t#&+`S8Vg7M_7D_~VO{6dVg#go{z%U|e96!;aQB z9tK*w=2@EoDM9rrcQ`uVFzqlQFIk-8PDR)mQj}$o2hZ~Jh}aCr)mEo@RkMd{#NQUszRqfF#;j^zsl{{n zZPhVcr0SZk8<+GxXta(^>8*9D-n8Zi#W1)>QTj!l+ICqsR1C<@z7?`i^NDDvJ1RQY ztQS2x3Ph#eTWC9eoA>B`i`P{>$qOqV=X4{JoZ}H#r>zU&xJ{+3^`1DE+N7|$jV#OU zO=NWY{26A}#87iT((J|pnjZG0#jRGFnY&HV>zgTVs)RC(MN(Y9FU55|Pm;5H$lfC( zBsJbhvLhN&9J@|bAN-V{M$Zy$&(#uCaS_p$o=wnsK?LXbK28Qdhtm-+;at!|I2(}% zRRyw;oPdi7OK~~)8Ya5`gmDSyF)`*8MlXddq<3I^>ME=)E*BHL9T-mCC-=rDweULa030qbNvkVlZi2moT6C+K`gg)9hwYyC!RXn0o}v-wl4 z)c2m&(5^{3Gta^?$U+DX;%qn*rp4K~pD-@e?(jq&dReDCad~!7VPTHL6RlYt43sgd zh8)Ihbe!ofE42nKEts+WhTJ3C<0xY+(@adFngqQkWp_+1;wT|L(>JLN5`yD z9c$NHYbm{>^^#&5=vJIPw`Ftl5m~M1kS#p{vZLl5QE7Tgw6u(g#_lN5++-BY?LY7y zoiFmn`h&c#{1MJJG{&j>8#!jokE5pxSi?q@O@0_>#ce+^>^3)}JLu2g6Br}zxIvTa zk@Da1ZAJ%~G;`d)PULxn7AitCcY`-Bz$=~iyMhX&k#^DNqBT^Q0jEF)$2dN1ns`R!~ zMp>r=2Ffgj#SWh$)+2K6h=;lKLx*iJ*cx3;SzS#JSsb0R#n`<=^&Y-cwbZ?6w00yL zt;5xNTP>#N+8$IKqaBLJ$a}KAb(d@@FOrQNPT8aRby456QEWe0Bs$x|M16xSsO^8{ z^=(h^jl1{54$lG3)zitD_N737s&c~GWvpe^n~lB~V}jC#||_C++k@f%pDG*HZV5hZL0rd;dv6g&Jl$!^+B^8M{3J6=W7JO55FyK(w*CIkIA3AmoWYjurRp@O`l)%RZ3>N+&;bMq`5 zhb*MvVzeL5xnzvkQ&!((SO$Z7^>XEg_;fuUwRw85rj&Vb&Cr~X~ zoDdY7cE8P4`$B{-y07SuL0)mSu2_^=kg_DfK&s&f1Mz}BWBI#|z)(1L!9Z%VVwodq zd5;@AF?Zgl zIC@GIXYUo++%_iLD>7ws|F2R&_bJiXyk5*2DG==)vZ$`%1#9PfywLt2Z|u8|m-@GJ z#{Nc5o(SN?S)P+OC$W|pD{Hz(!_u>VW`u1vhMoH-ZQFgDX6}BErl*(C%A}sAHvyVm zf1Tnc^QeXgmQ(aPAFBOnGez~^4QrJtQWzN_>E1$;-FSxW{tMu1PXoUB2$0r34}3cxp08Kw(Rv_lsQ_!A zTLFYVFQBit0;N>~>M#k|p0BhVbQV(A+gkI`8(O(T^X~b17EVAG(s3y|3}?e_u!g=9 z=TR4@tk7XZdm3ejs`EL6S;G!1+S7?JR$a?^EoaWAU-@csEn0u18OULv4h)*df8i00 z*8lgeP5NoSsv~_{j1TrG)GjH;#jo}&P%R1>>~q~v|S%rTn$St z*4~q5TTiCyHTDB!VbEyq_cSW~ZoS%YQ&Bs&E7qX^#i#$2EVcZdHcv#-ng}%XXdtX@?aS!k1ydm9s?Y^ zIfOObMX~04eq{J{uQTjU8>4KxO&2`yHH~k*NV9_hv^ceh<~Coa=(*1*s9Rzxx9vi6)D0`Y?d2Dznefib^LDmoE?H9ly*(zQ!mgqW)3U|kF|4zMA6#p(*9Taf zU5}W3+Cx?A$g4)n>JFo`Z;{d3@6y{U&nb@PHHzBjr8tN8%C_bl*`wGjd-hzEbd5Wq zA8CQzFMrX|`LmF<^%q`0_dIVNnc&SsD|lwh1m`=N$f;{RIP>-&V2u*MS~e|a^%Gw) z(w4IfyQ06x(Z{sMZan z-+?ZY9&ICAw`P&#%uRw%evP2wP7@<>j}Uxb0n92v1d|*<&{-@&gkQkfs26a?{~+A4 zJ8(LBIqc0xLoU2=4k`&9{sHWML4O)`662E}!FcE}l-PPqNGyi6OFEY64}EDThH>rx z1oHZuplAO}KxjM)#PP?0spo!R?P>!;TNM!2RRZHwHc$sVfLvn)YL5sk{WM_vKY#!S@z5jt;7T-yi^vV2Iu&)W(0ZcI)bezhfl=WR#BunF* zXlyAK&CLq*r&nRU`WN0Z^9*m98s;s1$-J;_g!7z=;*66DXV`RuW!8JJ>Rq=PdEzss z_0h8owM&F7d__xJ-hy??5m=u<7Fusp4F|rU=({dZWXm$jv?Z8gClrbu}HOHoH~HfR@425rHq zs6?EP3C8Ip7fvNhI2&^V(??v!#K;pEm;4YW`1WG@s$28Oi5cb3268C3LvBPOoK%Q5p0FGnS5_sbFNHMukPf~}CAj?L!W zN8>G?wYSXn{#9mOpHX#2_Wji|3Uc-JIumC}*F-ILpw-EI(&u z%^QAXjAL&z_FYFAayQPHH(#TT>))WMEl20+F|PTAGTiqrMcs9d;@d+gc`T69&CsxJ z`3Ko@XqL2W>4N0ckoKtJ*~pmaPBmEvKbpF9NggS)}HaE()C3w*KsEtD&$r5wrddVb7NqzLUDwI|U9zd!AzRBokZc|MB~#6S zXle`)tt~$Z-o2Lu`9joR|!%m*rSPm=Q5Y9Y%2d4~PVdEY!v6ijhGmha) zjB(e447XFEdL0gXre9O)zIP~U?HTAtBPr96lNvf~fHliPs&;CU zG;A6mxygJ|m<=Iy(_awAxHFK2rwBG;CsCUdMR27)1iwrmxXgdzY{+p~6CT8w@NGC9 zuoEX^l5jaD5El|muy$2o7vvL67k3WhqmE-j(jiO?t;38d<(L#*0=pn7nBp&ChK>ao zU;hKFL;ngaT`vHo_5kd59R{YsZNN4R9mbkfK$<83%4jwS?DPV(4DDIkwwVEE_< zT7BK0U{-lWYa4z|>l)O^m*!b`0kY7G>tXF=35~%GQJ%Oi7_Qgkb)S&*W``AdRJ86W z1LP$*B9MY0PEs`!u8ezZzagcV?_i$w2wAR~XiRa3XCekts$AaCMfl~Wg=ZrzK&lim zWIKx9k6cE=30E-6T!e~M%*xODD z`Zdq<&Y3yhGF`%3Mvc6BX8#QP=;vXN;67Xo-G`F_gHQ)Dab2_z?1Wly zDNcZFyo2dt&tgL4(~yY+ux_ctOc~Xf6kh;&2*wOS&|&mwFn#qm&|%zwovx>WuKGcs z8$1lu?ybOMtO<5U%YZmo4C|I8U>&dkLoo?-%@lmr@|)J#_?}j(cuQ;UdPVCPIHw(Z zQgiXmc@|DX7DjPnVi|6Zh{tsizbk*@B3PKf_}~AxXHiJuhYRnE=YGBNyYlTAz(~Yh0m;W4UeUSX z@rbURGk%_l7aV3(9U)~&k8%YUzwZc2oAL@keN^aY!^`_^X$>3f9xE}E)T}YPN7~G$ z{x?kKo;+Dyoqw^?h*a=ws?rV%~>KG#r z?O>FBKhuWYmuSPp5n9|eMPs8H+R*U{rOuwG^qY=T)R-To4(ce=?uD?AdV^%T`bon? zH92u4lN4tIVNd-6?3nC;052a4HDaDPenXA#N>v0=Eu6 zkxRt&aXz>{S;mD}4EmERm@fVeOc!}S#wNlDT7${3cdCm{fxYu+ObQ?{Sd&5Z_!3CN z*8xBN46xQe4ztPwz}CABcnmcHsip*yvl8&#Nx(U522zm##9A7N1B-yY>wT@E>`krG zeoku~d{OJut#MqQr{_gT&n#|>Zoo}3X}CT-4EA^7+O)t}e$bha_n||{2?~uw^-0mY zIl~dH4yBY0{UhpLW#3~H68{hpf>=OL)rtpP!DxVEbBgI)$S)*W!pm5W5-p-vhE0@j88K9 zeLvCq?XS`Li9@uwZw#{V3uW&8h*GDYr_{~ID7tS6W$ouE;T(1u zV4WzY8gVZEPMnLmjLFe2Vp7aL*!4JwvF> fields{ - {1, {UINT32, 0, 0x0007ffff, 0}}, - {2, {UINT8, 2, 0b11111000, 3}}, - {3, {UINT8, 3, 0, 0}}, - {4, {UINT32, 4, 0x0007ffff, 0}}, - {5, {UINT8, 6, 0b11111000, 3}}, - {6, {UINT8, 7, 0, 0}}, - {7, {UINT16, 8, 0, 0}}, - {8, {UINT16, 10, 0, 0}}, - {9, {UINT16, 12, 0, 0}}, - {10, {UINT32, 0, 0, 0}}, - {11, {UINT32, 4, 0, 0}}, - {12, {UINT32, 8, 0, 0}}, - {13, {UINT32, 12, 0, 0}}}; + std::vector> fields{ + {ChanField::RANGE, {UINT32, 0, 0x0007ffff, 0}}, + {ChanField::FLAGS, {UINT8, 2, 0b11111000, 3}}, + {ChanField::REFLECTIVITY, {UINT8, 3, 0, 0}}, + {ChanField::RANGE2, {UINT32, 4, 0x0007ffff, 0}}, + {ChanField::FLAGS2, {UINT8, 6, 0b11111000, 3}}, + {ChanField::REFLECTIVITY2, {UINT8, 7, 0, 0}}, + {ChanField::SIGNAL, {UINT16, 8, 0, 0}}, + {ChanField::SIGNAL2, {UINT16, 10, 0, 0}}, + {ChanField::NEAR_IR, {UINT16, 12, 0, 0}}, + {ChanField::RAW32_WORD1, {UINT32, 0, 0, 0}}, + {ChanField::RAW32_WORD2, {UINT32, 4, 0, 0}}, + {ChanField::RAW32_WORD3, {UINT32, 8, 0, 0}}, + {ChanField::RAW32_WORD4, {UINT32, 12, 0, 0}}}; size_t chan_data_size = 16; EXPECT_NO_THROW(add_custom_profile(profile_nr, name, fields, chan_data_size)); @@ -42,10 +42,12 @@ TEST(ProfileExtension, ProfileExtensionTest) { auto scan = ouster::LidarScan(40, 60, prof); - int i = 0; - for (auto it = scan.begin(); it != scan.end(); ++it, ++i) - EXPECT_EQ(static_cast(it->first), fields[i].first); - EXPECT_EQ(i, fields.size()); + for (const auto& field: fields) + { + auto res = scan.fields().find(field.first); + EXPECT_TRUE(res != scan.fields().end()); + } + EXPECT_EQ(scan.fields().size(), fields.size()); // TODO: would be good to check parsing here too -- Tim T. diff --git a/tests/scan_batcher_test.cpp b/tests/scan_batcher_test.cpp index 9a2f4e8d..21c4f37a 100644 --- a/tests/scan_batcher_test.cpp +++ b/tests/scan_batcher_test.cpp @@ -58,12 +58,10 @@ std::vector random_frame(UDPProfileLidar profile, ls.timestamp().data() + ls.timestamp().size(), 1000); std::fill(ls.status().data(), ls.status().data() + ls.status().size(), 0x1); - auto randomise = [&](auto ref_field, ChanField i) { - if (i >= ChanField::RAW_HEADERS) return; - + auto randomise = [&](auto ref_field, const std::string& i) { randomize_field(ref_field, pw.field_value_mask(i)); }; - ouster::impl::foreach_field(ls, randomise); + ouster::impl::foreach_channel_field(ls, pw, randomise); auto g = std::mt19937(0xdeadbeef); auto dinit_id = std::uniform_int_distribution(0, 0xFFFFFF); @@ -172,8 +170,8 @@ TEST_P(ScanBatcherTest, scan_batcher_skips_test) { columns_per_packet); { // pre-fill lidar scan so we know which fields/headers are changed - auto fill = [](auto ref_field, ChanField) { ref_field = 1; }; - ouster::impl::foreach_field(ls, fill); + auto fill = [](auto ref_field, const std::string&) { ref_field = 1; }; + ouster::impl::foreach_channel_field(ls, pf, fill); ls.packet_timestamp() = 2000; ls.timestamp() = 100; ls.status() = 0x0f; @@ -192,10 +190,7 @@ TEST_P(ScanBatcherTest, scan_batcher_skips_test) { EXPECT_EQ(ls.frame_id, frame_id); - auto test_skipped_fields = [&](auto ref_field, ChanField chan) { - // custom fields are never touched - if (chan >= ChanField::CUSTOM0 && chan <= ChanField::CUSTOM9) return; - + auto test_skipped_fields = [&](auto ref_field, const std::string& chan) { // dropped frames should all be zero, RAW_HEADERS or not for (auto& m_id : dropped_m_ids) { EXPECT_TRUE((ref_field.col(m_id) == 0).all()); @@ -245,29 +240,23 @@ TEST_P(ScanBatcherTest, scan_batcher_skips_test) { dropped_packets.size()); }; - ouster::impl::foreach_field(ls, test_skipped_fields); + ouster::impl::foreach_channel_field(ls, pf, test_skipped_fields); test_headers(ls); // now repeat for RAW_HEADERS and CUSTOM fields - LidarScanFieldTypes rh_types(ls.begin(), ls.end()); + LidarScanFieldTypes rh_types(ls.field_types()); rh_types.emplace_back(ChanField::RAW_HEADERS, ChanFieldType::UINT32); - rh_types.emplace_back(ChanField::CUSTOM0, ChanFieldType::UINT32); - rh_types.emplace_back(ChanField::CUSTOM1, ChanFieldType::UINT32); - rh_types.emplace_back(ChanField::CUSTOM2, ChanFieldType::UINT32); - rh_types.emplace_back(ChanField::CUSTOM3, ChanFieldType::UINT32); - rh_types.emplace_back(ChanField::CUSTOM4, ChanFieldType::UINT32); - rh_types.emplace_back(ChanField::CUSTOM5, ChanFieldType::UINT32); - rh_types.emplace_back(ChanField::CUSTOM6, ChanFieldType::UINT32); - rh_types.emplace_back(ChanField::CUSTOM7, ChanFieldType::UINT32); - rh_types.emplace_back(ChanField::CUSTOM8, ChanFieldType::UINT32); - rh_types.emplace_back(ChanField::CUSTOM9, ChanFieldType::UINT32); + rh_types.emplace_back("CUSTOM0", ChanFieldType::UINT32); + rh_types.emplace_back("CUSTOM9", ChanFieldType::UINT32); auto rh_ls = LidarScan(columns_per_frame, pixels_per_column, rh_types.begin(), rh_types.end(), columns_per_packet); { // pre-fill lidar scan so we know which fields/headers are changed - auto fill = [](auto ref_field, ChanField) { ref_field = 1; }; - ouster::impl::foreach_field(rh_ls, fill); + auto fill = [](auto ref_field, const std::string&) { ref_field = 1; }; + ouster::impl::foreach_channel_field(rh_ls, pf, fill); + ouster::impl::visit_field(rh_ls, "CUSTOM0", fill, ""); + ouster::impl::visit_field(rh_ls, "CUSTOM9", fill, ""); rh_ls.packet_timestamp() = 2000; rh_ls.timestamp() = 100; rh_ls.status() = 0x0f; @@ -283,14 +272,16 @@ TEST_P(ScanBatcherTest, scan_batcher_skips_test) { EXPECT_EQ(rh_ls.frame_id, frame_id); - auto test_custom_fields = [](auto ref_field, ChanField chan) { - if (chan >= ChanField::CUSTOM0 && chan <= ChanField::CUSTOM9) { - EXPECT_TRUE((ref_field == 1).all()); - } + auto test_custom_fields = [](auto ref_field) { + EXPECT_TRUE((ref_field == 1).all()); }; - ouster::impl::foreach_field(rh_ls, test_custom_fields); - ouster::impl::foreach_field(rh_ls, test_skipped_fields); + ouster::impl::visit_field(rh_ls, "CUSTOM0", test_custom_fields); + ouster::impl::visit_field(rh_ls, "CUSTOM9", test_custom_fields); + + ouster::impl::foreach_channel_field(rh_ls, pf, test_skipped_fields); + ouster::impl::visit_field(rh_ls, ChanField::RAW_HEADERS, + test_skipped_fields, ChanField::RAW_HEADERS); test_headers(rh_ls); } @@ -348,8 +339,8 @@ TEST_P(ScanBatcherTest, scan_batcher_block_parse_dropped_packets_test) { columns_per_packet); { // pre-fill lidar scan so we know which fields/headers are changed - auto fill = [](auto ref_field, ChanField) { ref_field = 1; }; - ouster::impl::foreach_field(ls, fill); + auto fill = [](auto ref_field, const std::string&) { ref_field = 1; }; + ouster::impl::foreach_channel_field(ls, pf, fill); ls.packet_timestamp() = 2000; ls.timestamp() = 100; ls.status() = 0x0f; @@ -362,10 +353,7 @@ TEST_P(ScanBatcherTest, scan_batcher_block_parse_dropped_packets_test) { } EXPECT_TRUE(batcher(*next_frame_packet, ls)); - auto test_skipped_fields = [&](auto ref_field, ChanField chan) { - // custom fields are never touched - if (chan >= ChanField::CUSTOM0 && chan <= ChanField::CUSTOM9) return; - + auto test_skipped_fields = [&](auto ref_field, const std::string& chan) { // dropped frames should all be zero, RAW_HEADERS or not for (auto& m_id : dropped_m_ids) { EXPECT_TRUE((ref_field.col(m_id) == 0).all()); @@ -397,28 +385,22 @@ TEST_P(ScanBatcherTest, scan_batcher_block_parse_dropped_packets_test) { dropped_packets.size()); }; - ouster::impl::foreach_field(ls, test_skipped_fields); + ouster::impl::foreach_channel_field(ls, pf, test_skipped_fields); test_headers(ls); - // now repeat for RAW_HEADERS and CUSTOM fields - LidarScanFieldTypes custom_types(ls.begin(), ls.end()); - custom_types.emplace_back(ChanField::CUSTOM0, ChanFieldType::UINT32); - custom_types.emplace_back(ChanField::CUSTOM1, ChanFieldType::UINT32); - custom_types.emplace_back(ChanField::CUSTOM2, ChanFieldType::UINT32); - custom_types.emplace_back(ChanField::CUSTOM3, ChanFieldType::UINT32); - custom_types.emplace_back(ChanField::CUSTOM4, ChanFieldType::UINT32); - custom_types.emplace_back(ChanField::CUSTOM5, ChanFieldType::UINT32); - custom_types.emplace_back(ChanField::CUSTOM6, ChanFieldType::UINT32); - custom_types.emplace_back(ChanField::CUSTOM7, ChanFieldType::UINT32); - custom_types.emplace_back(ChanField::CUSTOM8, ChanFieldType::UINT32); - custom_types.emplace_back(ChanField::CUSTOM9, ChanFieldType::UINT32); + // now repeat for CUSTOM fields + LidarScanFieldTypes custom_types(ls.field_types()); + custom_types.emplace_back("CUSTOM0", ChanFieldType::UINT32); + custom_types.emplace_back("CUSTOM9", ChanFieldType::UINT32); auto custom_ls = LidarScan(columns_per_frame, pixels_per_column, custom_types.begin(), custom_types.end(), columns_per_packet); { // pre-fill lidar scan so we know which fields/headers are changed - auto fill = [](auto ref_field, ChanField) { ref_field = 1; }; - ouster::impl::foreach_field(custom_ls, fill); + auto fill = [](auto ref_field, const std::string&) { ref_field = 1; }; + ouster::impl::foreach_channel_field(custom_ls, pf, fill); + ouster::impl::visit_field(custom_ls, "CUSTOM0", fill, ""); + ouster::impl::visit_field(custom_ls, "CUSTOM9", fill, ""); custom_ls.packet_timestamp() = 2000; custom_ls.timestamp() = 100; custom_ls.status() = 0x0f; @@ -433,14 +415,14 @@ TEST_P(ScanBatcherTest, scan_batcher_block_parse_dropped_packets_test) { EXPECT_EQ(custom_ls.frame_id, frame_id); - auto test_custom_fields = [](auto ref_field, ChanField chan) { - if (chan >= ChanField::CUSTOM0 && chan <= ChanField::CUSTOM9) { - EXPECT_TRUE((ref_field == 1).all()); - } + auto test_custom_fields = [](auto ref_field) { + EXPECT_TRUE((ref_field == 1).all()); }; - ouster::impl::foreach_field(custom_ls, test_custom_fields); - ouster::impl::foreach_field(custom_ls, test_skipped_fields); + ouster::impl::visit_field(custom_ls, "CUSTOM0", test_custom_fields); + ouster::impl::visit_field(custom_ls, "CUSTOM9", test_custom_fields); + + ouster::impl::foreach_channel_field(custom_ls, pf, test_skipped_fields); test_headers(custom_ls); } @@ -454,18 +436,18 @@ TEST_P(ScanBatcherTest, scan_batcher_wraparound_test) { auto ls = LidarScan(columns_per_frame, pixels_per_column, profile, columns_per_packet); + packet_format pf(profile, pixels_per_column, columns_per_packet); + packet_writer pw(profile, pixels_per_column, columns_per_packet); + // pre-fill lidar scan so we know which fields/headers are changed - auto fill = [](auto ref_field, ChanField) { ref_field = 1; }; - ouster::impl::foreach_field(ls, fill); + auto fill = [](auto ref_field, const std::string&) { ref_field = 1; }; + ouster::impl::foreach_channel_field(ls, pf, fill); ls.packet_timestamp() = 2000; ls.timestamp() = 100; ls.status() = 0x0f; ls.measurement_id() = 10000; ls.frame_id = 0; - packet_format pf(profile, pixels_per_column, columns_per_packet); - packet_writer pw(profile, pixels_per_column, columns_per_packet); - auto packet = std::make_unique(); std::memset(packet->buf.data(), 0, packet->buf.size()); packet->host_timestamp = 100; @@ -487,13 +469,13 @@ TEST_P(ScanBatcherTest, scan_batcher_wraparound_test) { EXPECT_TRUE((ls.timestamp() == 100).all()); EXPECT_TRUE((ls.status() == 0x0f).all()); EXPECT_TRUE((ls.measurement_id() == 10000).all()); - auto test_fields = [](auto ref_field, ChanField) { + auto test_fields = [](auto ref_field, const std::string&) { EXPECT_TRUE((ref_field == 1).all()); }; - ouster::impl::foreach_field(ls, test_fields); + ouster::impl::foreach_channel_field(ls, pf, test_fields); } -using HashMap = std::map; +using HashMap = std::map; using snapshot_param = std::tuple; class ScanBatcherSnapshotTest : public ::testing::TestWithParam {}; @@ -547,7 +529,7 @@ INSTANTIATE_TEST_CASE_P( // https://wjngkoh.wordpress.com/2015/03/04/c-hash-function-for-eigen-matrix-and-vector/ struct matrix_hash { template - void operator()(Eigen::Ref> matrix, ChanField f, + void operator()(Eigen::Ref> matrix, const std::string& f, HashMap& map) const { size_t seed = 0; for (int i = 0; i < matrix.size(); ++i) { @@ -587,13 +569,13 @@ TEST_P(ScanBatcherSnapshotTest, snapshot_test) { } HashMap hashes; - ouster::impl::foreach_field(ls, matrix_hash{}, hashes); + ouster::impl::foreach_channel_field(ls, pf, matrix_hash{}, hashes); EXPECT_EQ(hashes, snapshot_hashes); } namespace alternatives { -using Fields = std::vector>; +using Fields = std::vector>; static const Fields lb_field_info{ {ChanField::RANGE, {UINT32, 0, 0x7fff, -3}}, // uint16 => uint32 @@ -660,7 +642,7 @@ TEST_P(ScanBatcherSnapshotTest, extended_profile_comp_test) { EXPECT_FALSE(batcher(pcap.current_data(), packet_ts, ls)); } pcap.seek(0); - ouster::impl::foreach_field(ls, matrix_hash{}, hashes); + ouster::impl::foreach_channel_field(ls, pf, matrix_hash{}, hashes); return hashes; }; diff --git a/tests/udp_queue_test.cpp b/tests/udp_queue_test.cpp index e9170ad0..d4f92d21 100644 --- a/tests/udp_queue_test.cpp +++ b/tests/udp_queue_test.cpp @@ -307,12 +307,12 @@ TEST_P(UdpQueuePcapTest, single_client_test) { { // collect packets PcapReader pcap(data_dir + "/" + std::get<0>(test_params)); while (pcap.next_packet()) { - if (pcap.current_info().dst_port == info.udp_port_lidar) { + if (pcap.current_info().dst_port == info.config.udp_port_lidar) { LidarPacket p(pcap.current_length()); std::memcpy(p.buf.data(), pcap.current_data(), p.buf.size()); lidar_packets.push_back(std::move(p)); } - if (pcap.current_info().dst_port == info.udp_port_imu) { + if (pcap.current_info().dst_port == info.config.udp_port_imu) { ImuPacket p(pcap.current_length()); std::memcpy(p.buf.data(), pcap.current_data(), p.buf.size()); imu_packets.push_back(std::move(p)); @@ -333,8 +333,9 @@ TEST_P(UdpQueuePcapTest, single_client_test) { bool stop = false; std::vector> replays; replays.push_back(std::make_shared( - data_dir + "/" + std::get<0>(test_params), info.udp_port_lidar, - info.udp_port_imu, lidar_port, imu_port)); + data_dir + "/" + std::get<0>(test_params), + info.config.udp_port_lidar.value(), info.config.udp_port_imu.value(), + lidar_port, imu_port)); std::thread producer(&BufferedUDPSource::produce, &queue); std::thread sensor_emu(&replay, &stop, replays, 1); @@ -402,8 +403,10 @@ TEST(UdpQueueTest, multi_client_test) { int n_lidar = 0, n_imu = 0; while (pcap.next_packet()) { - if (pcap.current_info().dst_port == info.udp_port_lidar) ++n_lidar; - if (pcap.current_info().dst_port == info.udp_port_imu) ++n_imu; + if (pcap.current_info().dst_port == info.config.udp_port_lidar) + ++n_lidar; + if (pcap.current_info().dst_port == info.config.udp_port_imu) + ++n_imu; } Event e_lidar{i, client_state::LIDAR_DATA}; Event e_imu{i, client_state::IMU_DATA}; @@ -413,13 +416,13 @@ TEST(UdpQueueTest, multi_client_test) { pcap.reset(); while (pcap.next_packet()) { - if (pcap.current_info().dst_port == info.udp_port_lidar) { + if (pcap.current_info().dst_port == info.config.udp_port_lidar) { std::memcpy(orig_packets.back(e_lidar).buf.data(), pcap.current_data(), orig_packets.back(e_lidar).buf.size()); orig_packets.push(e_lidar); } - if (pcap.current_info().dst_port == info.udp_port_imu) { + if (pcap.current_info().dst_port == info.config.udp_port_imu) { std::memcpy(orig_packets.back(e_imu).buf.data(), pcap.current_data(), orig_packets.back(e_imu).buf.size()); @@ -439,8 +442,8 @@ TEST(UdpQueueTest, multi_client_test) { int imu_port = port++; auto info = metadata_from_json(data_dir + "/" + p.second); replays.push_back(std::make_shared( - data_dir + "/" + p.first, info.udp_port_lidar, info.udp_port_imu, - lidar_port, imu_port)); + data_dir + "/" + p.first, info.config.udp_port_lidar.value(), + info.config.udp_port_imu.value(), lidar_port, imu_port)); auto id = producer.add_client( init_client("localhost", lidar_port, imu_port), info, 1.f);