diff --git a/.gitignore b/.gitignore index 85bea34b..3b00ad39 100644 --- a/.gitignore +++ b/.gitignore @@ -9,4 +9,18 @@ python/**/*.pyd # visual studio .vs/ -out/ \ No newline at end of file +out/ + +# vim +*.swp + +# CMake, Ninja +.ninja_deps +.ninja_log +CMakeFiles +CTestTestfile.cmake + +# Built artifacts +generated +rules.ninja +*.a diff --git a/CHANGELOG.rst b/CHANGELOG.rst index c37ae89b..a72ccb91 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,6 +2,64 @@ Changelog ========= +20230710 +======== + +* Update vcpkg ref of build to 2023-02-24 +* + +ouster_osf +---------- + +* Add Ouster OSF C++/Python library to save stream of LidarScans with metadata + +ouster_client +------------- + +* Add ``LidarScan.pose`` with poses per column +* Add ``_client.IndexedPcapReader`` and ``_client.PcapIndex`` to enable random + pcap file access by frame number +* [BREAKING] remove ``ouster::Imu`` object +* Add get_field_types function for LidarScan, from sensor_info +* bugfix: return metadata regardless of sensor_info status field +* Make timeout on curl more configurable + +ouster_viz +---------- + +* [BREAKING] Changed Python binding for ``Cloud.set_column_poses()`` to accept ``[Wx4x4]`` array + of poses, column-storage order +* bugfix: fix label re-use +* Add ``LidarScan.pose`` handling to ``viz.LidarScanViz``, and new ``T`` keyboard + binding to toggle column poses usage + +ouster_pcap +----------- +* bugfix: Use unordered map to store stream_keys to avoid comparison operators on map + +Python SDK +---------- +* Add Python 3.11 wheels +* Retire simple-viz for ouster-cli utility +* Add default ? key binding to LidarScanViz and consolidate bindings into stored definition +* Remove pcap-to-csv for ouster-cli utility +* Add validator class for LidarPacket + +ouster-cli +---------- +This release also marks the introduction of the ouster-cli utility which includes, among many features: +* Visualization from a connected sensor with automatic configuration +* Recording from a connected sensor +* Simultaneous record and viz from a connected sensor +* Obtaining metadata from a connected sensor +* Visualization from a specified PCAP +* Slice, info, and conversion for a specificed PCAP +* Utilities for benchmarking system, printing system-info +* Discovery which indicates all connected sensors on network +* Automatic logging to .ouster-cli +* Mapping utilities + + [20230403] ========== diff --git a/CMakeLists.txt b/CMakeLists.txt index 76a051a8..b5828d3f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -7,16 +7,17 @@ include(DefaultBuildType) include(VcpkgEnv) # ==== Project Name ==== -project(ouster_example VERSION 20230403) +project(ouster_example VERSION 20230710) # generate version header -set(OusterSDK_VERSION_STRING 0.8.1) +set(OusterSDK_VERSION_STRING 0.9.0) include(VersionGen) # ==== Options ==== option(CMAKE_POSITION_INDEPENDENT_CODE "Build position independent code." ON) option(BUILD_SHARED_LIBS "Build shared libraries." OFF) option(BUILD_PCAP "Build pcap utils." ON) +option(BUILD_OSF "Build Ouster OSF library." OFF) option(BUILD_VIZ "Build Ouster visualizer." ON) option(BUILD_TESTING "Build tests" OFF) option(BUILD_EXAMPLES "Build C++ examples" OFF) @@ -56,6 +57,10 @@ if(BUILD_PCAP) add_subdirectory(ouster_pcap) endif() +if(BUILD_OSF) + add_subdirectory(ouster_osf) +endif() + if(BUILD_VIZ) add_subdirectory(ouster_viz) endif() @@ -88,7 +93,7 @@ set(CPACK_SOURCE_IGNORE_FILES /.git /dist) set(CPACK_DEBIAN_PACKAGE_NAME ouster-sdk) set(CPACK_DEBIAN_FILE_NAME DEB-DEFAULT) set(CPACK_DEBIAN_PACKAGE_DEPENDS - "libjsoncpp-dev, libeigen3-dev, libtins-dev, libglfw3-dev, libglew-dev, libspdlog-dev") + "libjsoncpp-dev, libeigen3-dev, libtins-dev, libglfw3-dev, libglew-dev, libspdlog-dev, libpng-dev, libflatbuffers-dev") include(CPack) # ==== Install ==== diff --git a/README.rst b/README.rst index 71b964d0..e3da782b 100644 --- a/README.rst +++ b/README.rst @@ -18,6 +18,7 @@ reading and visualizing data. * `ouster_client `_ contains an example C++ client for ouster sensors * `ouster_pcap `_ contains C++ pcap functions for ouster sensors +* `ouster_osf `_ contains C++ OSF library to store ouster sensors data * `ouster_viz `_ contains a customizable point cloud visualizer * `python `_ contains the code for the ouster sensor python SDK (``ouster-sdk`` Python package) diff --git a/cmake/FindPcap.cmake b/cmake/FindPcap.cmake index ec325f38..b7c91f66 100644 --- a/cmake/FindPcap.cmake +++ b/cmake/FindPcap.cmake @@ -14,6 +14,14 @@ find_library(PCAP_LIBRARY NAMES pcap pcap_static wpcap HINTS ${PC_PCAP_LIBRARY_DIRS}) +if(NOT TARGET libpcap::libpcap) + add_library(libpcap::libpcap UNKNOWN IMPORTED) + set_target_properties(libpcap::libpcap PROPERTIES + INTERFACE_INCLUDE_DIRECTORIES "${PCAP_INCLUDE_DIR}" + IMPORTED_LINK_INTERFACE_LANGUAGES "C" + IMPORTED_LOCATION "${PCAP_LIBRARY}") +endif() + include(FindPackageHandleStandardArgs) find_package_handle_standard_args(Pcap REQUIRED_VARS PCAP_LIBRARY PCAP_INCLUDE_DIR diff --git a/cmake/FindPybind11Internal.cmake b/cmake/FindPybind11Internal.cmake new file mode 100644 index 00000000..5a2dd59e --- /dev/null +++ b/cmake/FindPybind11Internal.cmake @@ -0,0 +1,54 @@ +include(FindPackageHandleStandardArgs) + +if(DEFINED PYTHON_EXECUTABLE) + execute_process( + COMMAND + "${PYTHON_EXECUTABLE}" "-c" + "import sys; print(f'{str(sys.version_info.major)}.{str(sys.version_info.minor)}.{str(sys.version_info.micro)}')" + OUTPUT_VARIABLE _version_full + OUTPUT_STRIP_TRAILING_WHITESPACE) + + execute_process( + COMMAND + "${PYTHON_EXECUTABLE}" "-c" + "import pybind11;print(pybind11.get_cmake_dir())" + OUTPUT_VARIABLE _pybind_dir + RESULT_VARIABLE _pybind_result + ERROR_VARIABLE _pybind_error + OUTPUT_STRIP_TRAILING_WHITESPACE) + if(${_pybind_result} EQUAL 0) + LIST(APPEND CMAKE_PREFIX_PATH "${_pybind_dir}") + if("${_version_full}" VERSION_GREATER_EQUAL "3.11.0") + find_package(pybind11 2.10 REQUIRED) + else() + find_package(pybind11 2.0 REQUIRED) + endif() + + set(_PYBIND11_INTERNAL_PYTHON_VERSION "") + if(NOT PYTHON_VERSION STREQUAL "") + message("Found Python Version VIA: PYTHON_VERSION") + set(_PYBIND11_INTERNAL_PYTHON_VERSION "${PYTHON_VERSION}") + elseif(NOT PYTHONLIBS_VERSION_STRING STREQUAL "") + message("Found Python Version VIA: PYTHONLIBS_VERSION_STRING") + set(_PYBIND11_INTERNAL_PYTHON_VERSION "${PYTHONLIBS_VERSION_STRING}") + elseif(NOT PYTHON_VERSION_STRING STREQUAL "") + message("Found Python Version VIA: PYTHON_VERSION_STRING") + set(_PYBIND11_INTERNAL_PYTHON_VERSION "${PYTHON_VERSION_STRING}") + endif() + if(VCPKG_TOOLCHAIN AND NOT "${_version_full}" VERSION_EQUAL "${_PYBIND11_INTERNAL_PYTHON_VERSION}") + message(FATAL_ERROR "Python Versions Do Not Match +\tRequested Version: +\t\t${_version_full} +\tVersions Found: +\t\tPYTHON_VERSION: \"${PYTHON_VERSION}\" +\t\tPYTHONLIBS_VERSION_STRING: \"${PYTHONLIBS_VERSION_STRING}\" +\t\tPYTHON_VERSION_STRING: \"${PYTHON_VERSION_STRING}\" +\tInternal Cache: \"${_PYBIND11_INTERNAL_PYTHON_VERSION}\"") + endif() + else() + message(FATAL_ERROR "ERROR In Setting Pybind11 CMAKE Prefix Path: ${_pybind_error}") + endif() +else() + message(FATAL_ERROR "PYTHON_EXECUTABLE NOT SET") +endif() + diff --git a/cmake/Findlibtins.cmake b/cmake/Findlibtins.cmake index ae181213..55c87d38 100644 --- a/cmake/Findlibtins.cmake +++ b/cmake/Findlibtins.cmake @@ -54,5 +54,6 @@ if(NOT TARGET libtins) add_library(libtins INTERFACE) set_target_properties(libtins PROPERTIES INTERFACE_LINK_LIBRARIES ${LIBTINS_LIBRARIES}) + add_library(libtins::libtins ALIAS libtins) install(TARGETS libtins EXPORT ouster-sdk-targets) endif() diff --git a/cmake/OusterSDKConfig.cmake.in b/cmake/OusterSDKConfig.cmake.in index 3d7692cd..0ca13a80 100644 --- a/cmake/OusterSDKConfig.cmake.in +++ b/cmake/OusterSDKConfig.cmake.in @@ -8,6 +8,13 @@ find_dependency(jsoncpp) find_dependency(CURL) find_dependency(spdlog) +# ouster_osf dependencies +if(@BUILD_OSF@) + find_package(ZLIB REQUIRED) + find_package(PNG REQUIRED) + find_package(Flatbuffers NAMES Flatbuffers FlatBuffers) +endif() + # viz dependencies if(@BUILD_VIZ@) set(OpenGL_GL_PREFERENCE GLVND) diff --git a/conan/test_package/conanfile.py b/conan/test_package/conanfile.py index a52cf82b..bc77cb2c 100644 --- a/conan/test_package/conanfile.py +++ b/conan/test_package/conanfile.py @@ -11,6 +11,7 @@ def build(self): cmake = CMake(self) # Current dir is "test_package/build/" and CMakeLists.txt is # in "test_package" + cmake.definitions["BUILD_OSF"] = True cmake.definitions[ "CMAKE_PROJECT_PackageTest_INCLUDE"] = os.path.join( self.build_folder, "conan_paths.cmake") @@ -27,4 +28,7 @@ def test(self): os.chdir("examples") # on Windows VS puts execulables under `./Release` or `./Debug` folders exec_path = f"{os.sep}{self.settings.build_type}" if self.settings.os == "Windows" else "" - self.run(".%s%sclient_example" % (exec_path, os.sep)) + self.run(f".{exec_path}{os.sep}client_example") + + # Smoke test OSF lib + self.run(f".{exec_path}{os.sep}osf_reader_example") diff --git a/conanfile.py b/conanfile.py index 26fd9018..96f42d50 100644 --- a/conanfile.py +++ b/conanfile.py @@ -17,6 +17,7 @@ class OusterSDKConan(ConanFile): options = { "build_viz": [True, False], "build_pcap": [True, False], + "build_osf": [True, False], "shared": [True, False], "fPIC": [True, False], "ensure_cpp17": [True, False], @@ -25,6 +26,7 @@ class OusterSDKConan(ConanFile): default_options = { "build_viz": False, "build_pcap": False, + "build_osf": False, "shared": False, "fPIC": True, "ensure_cpp17": False, @@ -37,6 +39,7 @@ class OusterSDKConan(ConanFile): "conan/*", "ouster_client/*", "ouster_pcap/*", + "ouster_osf/*", "ouster_viz/*", "tests/*", "CMakeLists.txt", @@ -57,20 +60,19 @@ def config_options(self): del self.options.fPIC def requirements(self): - # not required directly here but because libtins and libcurl pulling - # slightly different versions of zlib and openssl we need to set it - # here explicitly - self.requires("zlib/1.2.13") - self.requires("openssl/1.1.1s") - self.requires("eigen/3.4.0") self.requires("jsoncpp/1.9.5") - self.requires("spdlog/1.10.0") + self.requires("spdlog/1.11.0") + self.requires("fmt/9.1.0") self.requires("libcurl/7.84.0") if self.options.build_pcap: self.requires("libtins/4.3") + if self.options.build_osf: + self.requires("flatbuffers/23.5.26") + self.requires("libpng/1.6.39") + if self.options.build_viz: self.requires("glad/0.1.34") if self.settings.os != "Windows": @@ -83,6 +85,7 @@ 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") diff --git a/docs/cli/changelog.rst b/docs/cli/changelog.rst new file mode 100644 index 00000000..3869ccaf --- /dev/null +++ b/docs/cli/changelog.rst @@ -0,0 +1,6 @@ +========= +Changelog +========= + +[0.9.0] +* Initial Public Release diff --git a/docs/cli/common-use-cases.rst b/docs/cli/common-use-cases.rst new file mode 100644 index 00000000..96f24c30 --- /dev/null +++ b/docs/cli/common-use-cases.rst @@ -0,0 +1,128 @@ +.. _common commands: + + +Common Use Cases +---------------- + +One of the goals of the Ouster Sensor Tools package is to easily allow the most common sensor and +recorded data interactions. We cover some common use cases here, listed alphabetically. Please note +that wherever is used, you are expected to substitute in your sensor's hostname, +e.g., ``os1-991913000010.local``. + + +Benchmarking +++++++++++++ + +To help users obtain performance metrics for the Ouster SDK, we +provide a benchmarking utility which will download `8 seconds of OS2 data`_, gather system info, +time various data operations, and generate a report which you can share with others. Give it a try! + +.. code:: bash + + $ ouster-cli util benchmark + +This will generate a report in a directory named ouster-bench, which will be located in the current working directory. + +.. _8 seconds of OS2 data: https://data.ouster.dev/drive/7377 + + +Discovering sensors on local network +++++++++++++++++++++++++++++++++++++ + +Sensors announce their presence on the network using Multicast Domain Name Service (mDNS). Use +helper utility command ``discover`` to list names and IPs af all available sensors on the local +network: + +.. code:: bash + + $ ouster-cli discover + +Collecting Metadata ++++++++++++++++++++ + +Sensor metadata, necessary for interpreting and parsing the pcap data, can be collected from sensors +using: + +.. code:: bash + + $ ouster-cli source info > .json + +This will generate a ``.json`` file named ``.json`` with the metadata inside. To +output it to a differently named file, simply change ``.json`` to +``.json``. You can also print the metadata to screen by removing ``>`` and +everything after it in the command. + + +Configuring Your Sensor ++++++++++++++++++++++++ + +``ouster-cli`` provides utilities for configuring your sensor with configuration parameters such as +``lidar_mode`` and ``azimuth_window``. + +To quickly auto-configure a sensor with with standard ports, azimuth window, operating mode, and +auto udp dest: + +.. code:: bash + + $ ouster-cli source config + +But what if you want to specify the ports and lidar_mode? You can use the ``sensor config`` command thusly: + +.. code:: bash + + $ ouster-cli source config lidar_mode 1024x10 udp_port_lidar 29847 + +.. note:: + + Multiple `` `` pairs can be passed this way! + +You may have a configuration that you want to use repeatedly. Typing these in at the command line +every time would be annoying. You can instead save your config to a json, named CONFIG_JSON, here, +and run: + +.. code:: bash + + $ ouster-cli source config -c + +And finally, you may wish to save a configuration after setting your sensor up perfectly. To do so: + +.. code:: bash + + $ ouster-cli source config -d + +That will print your json to stdout. Use ``>`` to redirect it to a file! + + +Recording Pcaps ++++++++++++++++ + +To record data from a udp port (7502 by default) to a pcap file in the current directory and write +the metadata to a json file with the same name, simply use: + +.. code:: bash + + $ ouster-cli source record + +This will record until you keyboard interrupt, i.e., use ``CTRL+C``. You can also set it to record +a specific length or number of packets, or to use different ports for lidar and IMU data. As always +with ``ouster-cli``, use ``--help`` to discover how those options work. + + +Visualizing Lidar Data +++++++++++++++++++++++ + +The following visualizes lidar data arriving on a udp port. Note that you may have to use +``ouster-cli source config`` first to configure your sensor properly. + +.. code:: bash + + $ ouster-cli source viz + + +The following replays lidar data saved in a pcap file and visualizes the output. It will looks for a +metadata json file with the same name as PCAP FILE by default, but you can specify a file using ``-m +``. + +.. code:: bash + + $ ouster-cli source viz diff --git a/docs/cli/getting-started.rst b/docs/cli/getting-started.rst new file mode 100644 index 00000000..3a833258 --- /dev/null +++ b/docs/cli/getting-started.rst @@ -0,0 +1,94 @@ +Getting Started +=============== + +Installation +------------ +The ouster sensor command line utility comes with the ouster-sdk python package. + +.. code:: + + pip3 install ouster-sdk + +Using the cli +------------- + +After installation you should have access to the ``ouster-cli`` utility, a command line interface +which is the main entry point to our internal tools. It is organized as a tree of +commands. To see what this means, open a terminal and type: + +.. code:: + + ouster-cli + +.. note:: + + On Ubuntu, if you're working outside a `virtual environment`_, you may have to add ``ouster-cli`` + to your path: + + .. code:: + + export PATH=$PATH:/.local/bin + + We recommend using python virtual environments for all users! + +That should return a menu of some possible commands, among them, ``discover``, ``mapping``, ``source`` and ``util``. +Each of these is itself the key to a submenu of further commands. Let's try looking +at ``source``: + +1. First download the sample data here :ref:`sample-data-download`. +2. Next run the following to see a list of commands +.. code:: + + ouster-cli source + +You should see a few commands listed there, including ``viz`` and ``info``. To figure out how to use +them, try using ``--help``. For example, for help with the ``ouster-cli source viz`` command: + +.. code:: + + ouster-cli source viz --help + +Remember that you can use ``--help`` with any ``ouster-cli`` command, regardless how far down the +menu tree you are. + +If you have a live sensor, you can replace the with the hostname of the sensor + +.. code:: + + ouster-cli source viz --help + +.. admonition:: Ubuntu UFW Firewall may cause: ``No packets received within 1.0s`` + + On some Ubuntu setups we've observed the situations when everything is configured properly so + that: + + - sensor is seen on the network and its Web page can be reached + - sensor destination IP is set to the IP of the computer where data is expeted + - sensor lidar port is known (i.e. default ``7502``, or some others) + - sensor is in ``RUNNING`` state + - sensor lidar packets traffic is seen on the expected machine and can be recorded with + ``tcpdump -w`` command to a pcap file (or ``Wireshark`` tools) + - CLI comamnd ``ouster-cli source {info,config}`` are working properly + - Viz ``ouster-cli source [pcap file] viz`` from the ``tcpdump`` recorded pcap can be played and visualized + + But ``ouster-cli source viz``, or ``ouster-cli source record`` still can't receive any packets + and get the following error:: + + ouster.client.core.ClientTimeout: No packets received within 1.0s + + Please check your `UFW Firewall`_ settings and try to allow the UDP traffic for ``7502`` + (or whatever the **UDP Port Lidar** is set on the sensor):: + + sudo ufw allow 7502/udp + +.. _UFW Firewall: https://help.ubuntu.com/community/UFW + + +.. _virtual environment: https://docs.python.org/3/library/venv.html + +What next? +---------- + +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` to +see what an ``ouster-cli`` workflow might look like, or you can read through :ref:`common commands`. diff --git a/docs/cli/overview.rst b/docs/cli/overview.rst new file mode 100644 index 00000000..d1001ffc --- /dev/null +++ b/docs/cli/overview.rst @@ -0,0 +1,12 @@ +============================ +Ouster Sensor Tools Overview +============================ + +The Ouster Sensor Tools CLI provides users features and tools for +interacting with sensor hardware and sensor data, including: + +- benchmarking performance from the command line +- collecting metadata from the command line +- configuring your sensor from the command line +- recording pcaps from the command line +- visualizing lidar data from pcaps or a sensor from the command line diff --git a/docs/cli/sample-sessions.rst b/docs/cli/sample-sessions.rst new file mode 100644 index 00000000..d4893bab --- /dev/null +++ b/docs/cli/sample-sessions.rst @@ -0,0 +1,104 @@ +.. _sample sessions: + +Sample Sessions +=============== + +These sample sessions will provide sample workflows for working with pcaps, sensors, and OSFs. +Either can be done independently, but users with access to a sensor may wish to work with the sensor +first, so that they will have recorded a pcap play back in the second sample. + + +Sample sensor session +--------------------- + +Connect a sensor via Ethernet to the system with Ouster SDK installed. +.. note:: + Bear in mind that following the steps below will modify the sensor's configuration. + +First we configure the sensor with standard ports, azimuth window, operating mode, and auto udp +dest: + +.. code:: bash + + $ ouster-cli source config + +You should get a return that looks like + +.. code:: bash + + No config specified; using defaults and auto UDP dest: + { + "azimuth_window": + [ + 0, + 360000 + ], + "operating_mode": "NORMAL", + "udp_port_imu": 7503, + "udp_port_lidar": 7502 + } + +Let's see what the sensor is seeing in a pretty visualizer: + +.. code:: bash + + $ ouster-cli source viz + +That looked nice! Let's record ten seconds of data to a pcap so we can view it on repeat! + +.. code:: bash + + $ ouster-cli source record -s 10 + +That should produce screen output that looks something like: + +.. code:: bash + + Connecting to + Recording for up to 10.0 seconds... + Wrote X GiB to ./OS--_.pcap + +Go ahead and look in the current directory for the named pcap file and associated metadata file. + +Continue to the next section, `Sample pcap session`_ to see what you can do with your new pcap file. + + +Sample pcap session +------------------- + +If you don't have a pcap lying around or that you just recorded from a sensor, you can download one +the `OS2 bridge sample data`_ and unzip the contents. + +Let's take a look at your pcap: + +.. code:: bash + + $ ouster-cli source info + +This should output something that looks like: + +.. code:: bash + Reading pcap: [####################################] 100% + File size: 2247.16M + Packets read: 85085 + Encapsulation: ETHERNET + Capture start: 2023-02-16 22:28:58.159505 + Capture end: 2023-02-16 22:30:49.369547 + Duration: 0:01:51.210042 + UDP Streams: + Src IP Dst IP Src Port Dst Port AF Frag Size Count + 127.0.0.1 127.0.0.1 7502 7502 4 No 33024 71182 + 127.0.0.1 127.0.0.1 7503 7503 4 No 48 13903 + +That tells us the number of packets belonging to each port captured in the pcap, and the associated +size. + +To visualize the pcap at 2x speed while looping back: + +.. code:: bash + + $ ouster-cli source viz -r 2.0 -e loop + +You can check check out all the available options by typing ``--help`` after ``ouster-cli source viz``. + + .. _OS2 bridge sample data: https://data.ouster.io/sdk-samples/OS2/OS2_128_bridge_sample.zip diff --git a/docs/conf.py b/docs/conf.py index da693219..b414ea34 100644 --- a/docs/conf.py +++ b/docs/conf.py @@ -65,9 +65,13 @@ def parse_version(): 'sphinx_rtd_theme', 'sphinx_copybutton', 'sphinx_tabs.tabs', - 'breathe' + 'breathe', + 'sphinx_rtd_size', ] +# Page width +sphinx_rtd_size_width = "70%" + # Full path generated Doxygen XML dir resolved in do_doxygen_generate_xml() # handler below breathe_projects = {'cpp_api': "xml"} diff --git a/docs/cpp/building.rst b/docs/cpp/building.rst index c548b970..b4570988 100644 --- a/docs/cpp/building.rst +++ b/docs/cpp/building.rst @@ -14,7 +14,7 @@ The C++ example code is available `on the Ouster Github Building on Linux / macOS ========================= -To install build dependencies on Ubuntu, run: +To install build dependencies on Ubuntu:20.04+, run: .. code:: console @@ -47,10 +47,25 @@ defaults: -DBUILD_VIZ=OFF # Do not build the sample visualizer -DBUILD_PCAP=OFF # Do not build pcap tools + -DBUILD_OSF=OFF # Do not build OSF lib -DBUILD_EXAMPLES=ON # Build C++ examples -DBUILD_TESTING=ON # Build tests -DBUILD_SHARED_LIBS=ON # Build shared instead of static libraries +.. admonition:: Additional dependencies required to build Ouster OSF lib + + To build Ouster OSF library as part of the SDK you need to pass ``BUILD_OSF=ON`` and ensure that + ``libpng`` and ``flatbuffers`` packages are available on the system. + + On Ubuntu:20.04+ systems:: + + $ sudo apt install libpng-dev libflatbuffers-dev + + On macOS:: + + $ brew install libpng flatbuffers + + Building on Windows =================== @@ -62,10 +77,10 @@ for dependencies. Follow the official documentation to set up your build environ `_ * `Visual Studio CPP Support `_ -* `Vcpkg, at tag "2022.02.23" installed and integrated with Visual Studio +* `Vcpkg, at tag "2023.02.24" installed and integrated with Visual Studio `_ -**Note** You'll need to run ``git checkout 2022.02.23`` in the vcpkg directory before bootstrapping +**Note** You'll need to run ``git checkout 2023.02.24`` in the vcpkg directory before bootstrapping to use the correct versions of the dependencies. Building may fail unexpectedly if you skip this step. @@ -79,7 +94,7 @@ You should be able to install dependencies with .. code:: powershell - PS > .\vcpkg.exe install --triplet x64-windows jsoncpp eigen3 curl libtins glfw3 glew spdlog + PS > .\vcpkg.exe install --triplet x64-windows jsoncpp eigen3 curl libtins glfw3 glew spdlog libpng flatbuffers After these steps are complete, you should be able to open, build and run the ``ouster_example`` project using Visual Studio: diff --git a/docs/index.rst b/docs/index.rst index c7606d5b..f70cbd0d 100644 --- a/docs/index.rst +++ b/docs/index.rst @@ -38,6 +38,16 @@ C++ API Reference Changelog +.. toctree:: + :caption: Command Line Utility Guide + :hidden: + + Overview + Getting Started + Sample Sessions + Common Use Cases + Changelog + .. toctree:: :caption: Migration Guides :hidden: diff --git a/docs/installation.rst b/docs/installation.rst index f4745ad4..077ed21a 100644 --- a/docs/installation.rst +++ b/docs/installation.rst @@ -35,11 +35,22 @@ Ubuntu 18.04, the default Python 3 version is is 3.6, so you'll have to install .. note:: - Using a virtual environment with the Ouster Python SDK is recommended. Users newer to Python should - read the official `venv instructions`_ and ensure that they upgrade pip *after* activating their - venv. If you're using venv on Windows, you'll want to use ``python`` and ``pip`` instead of ``py + Using a virtual environment with the Ouster Python SDK is recommended. + Users newer to Python should read the official `venv instructions`_ and + ensure that they upgrade pip *after* activating their venv. If you're using + venv on Windows, you'll want to use ``python`` and ``pip`` instead of ``py -3`` and ``py -3 -m pip`` in the following Powershell snippets. +.. note:: + + Python 3 when installed with macOS Developer Tools uses LibreSSL 2.8.3 (or + an older version.) OusterSDK, like many Python 3-compatible packages, + requires urllib3 which is not compatible with LibreSSL and requires OpenSSL + 1.1.1 or newer. To account for this, macOS users should install an official + distribution of Python 3 or one provided by Homebrew, as these use OpenSSL + 1.1.1 or newer. In either case, installing OusterSDK into a Python 3 + virtual enviroment is still recommended. + If you're using an unsupported platform like a non-glibc-based linux distribution, or wish to modify the Ouster Python SDK, you will need to build from source. See the `build instructions`_ for requirements needed to build from a source distribution or from a clone of the repository. diff --git a/docs/overview.rst b/docs/overview.rst index a4c9a03e..70fc74df 100644 --- a/docs/overview.rst +++ b/docs/overview.rst @@ -12,11 +12,11 @@ Quick links * :doc:`python/quickstart` * :doc:`cpp/building` * `Ouster ROS 1 driver`_ - +* :doc:`python/viz/index` + :doc:`cli/getting-started` .. figure:: /images/simple-viz.png :align: center -* :doc:`python/viz/index` Compatibility with FW --------------------- diff --git a/docs/python/devel.rst b/docs/python/devel.rst index beebec34..b6284b46 100644 --- a/docs/python/devel.rst +++ b/docs/python/devel.rst @@ -16,6 +16,8 @@ Building the Python SDK from source requires several dependencies: - `jsoncpp `_ >= 1.7 - `libtins `_ >= 3.4 - `libpcap `_ +- `libpng `_ >= 1.6 +- `flatbuffers `_ >= 1.1 - `libglfw3 `_ >= 3.2 - `libglew `_ >= 2.1 or `glad `_ - `spdlog `_ >= 1.9 @@ -33,14 +35,15 @@ On supported Debian-based linux systems, you can install all build dependencies $ sudo apt install build-essential cmake \ libeigen3-dev libjsoncpp-dev libtins-dev libpcap-dev \ - python3-dev python3-pip pybind11-dev libcurl4-openssl-dev \ - libglfw3-dev libglew-dev libspdlog-dev + python3-dev python3-pip libcurl4-openssl-dev \ + libglfw3-dev libglew-dev libspdlog-dev \ + libpng-dev libflatbuffers-dev On macos >= 10.13, using homebrew, you should be able to run: .. code:: console - $ brew install cmake eigen curl jsoncpp libtins python3 pybind11 glfw glew spdlog + $ brew install cmake eigen curl jsoncpp libtins python3 glfw glew spdlog libpng flatbuffers After you have the system dependencies, you can build the SDK with: @@ -52,6 +55,9 @@ After you have the system dependencies, you can build the SDK with: # make sure you have an up-to-date version of pip installed $ python3 -m pip install --user --upgrade pip + # install pybind11 + $ python3 -m pip install pybind11 + # then, build an installable "wheel" package $ python3 -m pip wheel --no-deps $OUSTER_SDK_PATH/python @@ -74,9 +80,9 @@ package manager and run: .. code:: powershell - PS > vcpkg install --triplet=x64-windows eigen3 jsoncpp libtins pybind11 glfw3 glad[gl-api-33] spdlog + PS > vcpkg install --triplet=x64-windows eigen3 jsoncpp libtins glfw3 glad[gl-api-33] spdlog libpng flatbuffers -The currently tested vcpkg tag is ``2022.02.23``. After that, using a developer powershell prompt: +The currently tested vcpkg tag is ``2023.02.24``. After that, using a developer powershell prompt: .. code:: powershell @@ -92,6 +98,9 @@ The currently tested vcpkg tag is ``2022.02.23``. After that, using a developer # set build options related to the compiler PS > $env:CMAKE_GENERATOR_PLATFORM="x64" PS > $env:CMAKE_GENERATOR="Visual Studio 15 2017" + + # install pybind11 + PS > py -m pip install pybind11 # then, build an installable "wheel" package PS > py -m pip wheel --no-deps "$env:OUSTER_SDK_PATH\python" @@ -116,7 +125,9 @@ change. For a local debug build, you can also add the ``-g`` flag. The Ouster SDK package includes configuration for ``flake8`` and ``mypy``. To run: .. code:: console - + # install pybind11 + $ python3 -m pip install pybind11 + # install and run flake8 linter $ python3 -m pip install flake8 $ cd ${OUSTER_SDK_PATH}/python @@ -169,8 +180,8 @@ image, run: --build-arg BASE=ubuntu:20.04 \ -t ouster-sdk-tox -the ``BASE`` argument will default to ``ubuntu:18.04``, but can also be set to other docker tags, -e.g. ``ubuntu:20.04``, ``ubuntu:22.04`` or ``debian:11``. Then, run the container to invoke 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 diff --git a/docs/python/examples/conversion.rst b/docs/python/examples/conversion.rst index cb1d3c6f..735d00cd 100644 --- a/docs/python/examples/conversion.rst +++ b/docs/python/examples/conversion.rst @@ -31,10 +31,9 @@ To convert the first ``5`` scans of our sample data from a pcap file, you can tr The source code of an example below: -.. literalinclude:: /../python/src/ouster/sdk/examples/pcap.py +.. literalinclude:: /../python/src/ouster/cli/core/pcap.py :start-after: [doc-stag-pcap-to-csv] :end-before: [doc-etag-pcap-to-csv] - :emphasize-lines: 37-41 :linenos: :dedent: @@ -112,4 +111,4 @@ To convert to the first ``5`` scans of our sample data from a pcap file to ``PLY Checkout the :func:`.examples.pcap.pcap_to_ply` documentation for the example source code. .. _Open3d File IO: http://www.open3d.org/docs/release/tutorial/geometry/file_io.html#Point-cloud -.. _plyfile: https://pypi.org/project/plyfile/ \ No newline at end of file +.. _plyfile: https://pypi.org/project/plyfile/ diff --git a/docs/python/viz/index.rst b/docs/python/viz/index.rst index 9851f0f0..5528acf0 100644 --- a/docs/python/viz/index.rst +++ b/docs/python/viz/index.rst @@ -1,6 +1,6 @@ -======================= +====================== Point Cloud Visualizer -======================= +====================== The Ouster visualization toolkit is written in C++ with Python bindings for Python functionality. It consists of the following: diff --git a/docs/python/viz/viz-run.rst b/docs/python/viz/viz-run.rst index 03f240be..852a7932 100644 --- a/docs/python/viz/viz-run.rst +++ b/docs/python/viz/viz-run.rst @@ -51,13 +51,16 @@ Keyboard controls: ``2`` Toggle second return point cloud visibility ``0`` Toggle orthographic camera ``=/-`` Dolly in/out - ``(space)`` Toggle pause + ``?`` Prints key bindings + ``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 - ``shift-z`` Save a screenshot of the current view - ``shift-x`` Toggle a continuous saving of screenshots + ``shift+z`` Save a screenshot of the current view + ``shift+x`` Toggle a continuous saving of screenshots + ``?`` Print keys to standard out ============== =============================================== .. diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index b22f5285..84fad334 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -17,6 +17,13 @@ else() message(STATUS "No ouster_pcap library available; skipping examples") endif() +if(TARGET OusterSDK::ouster_osf) + add_executable(osf_reader_example osf_reader_example.cpp) + target_link_libraries(osf_reader_example PRIVATE OusterSDK::ouster_osf) +else() + message(STATUS "No ouster_osf library available; skipping examples") +endif() + if(TARGET OusterSDK::ouster_viz) add_executable(viz_example viz_example.cpp) target_link_libraries(viz_example PRIVATE OusterSDK::ouster_client OusterSDK::ouster_viz) diff --git a/examples/client_example.cpp b/examples/client_example.cpp index d28b419e..abeabf68 100644 --- a/examples/client_example.cpp +++ b/examples/client_example.cpp @@ -37,9 +37,8 @@ int main(int argc, char* argv[]) { return argc == 1 ? EXIT_SUCCESS : EXIT_FAILURE; } - // Limit ouster_client log statements to "info" and direct the output to log - // file rather than the console (default). - sensor::init_logger("info", "ouster.log"); + // Limit ouster_client log statements to "info" + sensor::init_logger("info"); std::cerr << "Ouster client example " << ouster::SDK_VERSION << std::endl; /* diff --git a/examples/osf_reader_example.cpp b/examples/osf_reader_example.cpp new file mode 100644 index 00000000..b575e220 --- /dev/null +++ b/examples/osf_reader_example.cpp @@ -0,0 +1,49 @@ +/** + * Copyright (c) 2023, Ouster, Inc. + * All rights reserved. + * + * This file contains example code for working with the LidarScan class of the + * C++ Ouster SDK. Please see the sdk docs at static.ouster.dev for clearer + * explanations. + */ + +#include + +#include "ouster/impl/build.h" +#include "ouster/osf/reader.h" +#include "ouster/osf/stream_lidar_scan.h" + +using namespace ouster; + +int main(int argc, char* argv[]) { + if (argc != 2) { + std::cerr << "Version: " << ouster::SDK_VERSION_FULL << " (" + << ouster::BUILD_SYSTEM << ")" + << "\n\nUsage: osf_reader_example " << std::endl; + + return (argc == 1) ? EXIT_SUCCESS : EXIT_FAILURE; + } + + const std::string osf_file = argv[1]; + + // open OSF file + osf::Reader reader(osf_file); + + // read all messages from OSF in timestamp order + for (const auto& m : reader.messages()) { + std::cout << "m.ts: " << m.ts().count() << ", m.id: " << m.id() + << std::endl; + + // In OSF file there maybe different type of messages stored, so here we + // only interested in LidarScan messages + if (m.is()) { + // Decoding LidarScan messages + auto ls = m.decode_msg(); + + // if decoded successfully just print on the screen LidarScan + if (ls) { + std::cout << "ls = " << to_string(*ls) << std::endl; + } + } + } +} \ No newline at end of file diff --git a/ouster_client/CMakeLists.txt b/ouster_client/CMakeLists.txt index 049b4f48..e6bf44d9 100644 --- a/ouster_client/CMakeLists.txt +++ b/ouster_client/CMakeLists.txt @@ -7,7 +7,8 @@ find_package(spdlog REQUIRED) # ==== Libraries ==== add_library(ouster_client src/client.cpp src/types.cpp src/netcompat.cpp src/lidar_scan.cpp src/image_processing.cpp src/buffered_udp_source.cpp src/parsing.cpp - src/sensor_http.cpp src/sensor_http_imp.cpp src/sensor_tcp_imp.cpp src/logging.cpp) + src/sensor_http.cpp src/sensor_http_imp.cpp src/sensor_tcp_imp.cpp src/logging.cpp + src/profile_extension.cpp) target_link_libraries(ouster_client PUBLIC Eigen3::Eigen diff --git a/ouster_client/include/ouster/client.h b/ouster_client/include/ouster/client.h index 81320e90..1ddacca4 100644 --- a/ouster_client/include/ouster/client.h +++ b/ouster_client/include/ouster/client.h @@ -14,6 +14,7 @@ #include "ouster/types.h" #include "ouster/version.h" +#include "ouster/defaults.h" namespace ouster { namespace sensor { @@ -96,7 +97,7 @@ std::shared_ptr init_client(const std::string& hostname, lidar_mode ld_mode = MODE_UNSPEC, timestamp_mode ts_mode = TIME_FROM_UNSPEC, int lidar_port = 0, int imu_port = 0, - int timeout_sec = 60); + int timeout_sec = DEFAULT_HTTP_REQUEST_TIMEOUT_SECONDS); /** * [BETA] Connect to and configure the sensor and start listening for data via @@ -119,7 +120,7 @@ std::shared_ptr init_client(const std::string& hostname, std::shared_ptr mtp_init_client(const std::string& hostname, const sensor_config& config, const std::string& mtp_dest_host, - bool main, int timeout_sec = 60); + bool main, int timeout_sec = DEFAULT_HTTP_REQUEST_TIMEOUT_SECONDS); /** @}*/ @@ -170,7 +171,9 @@ bool read_imu_packet(const client& cli, uint8_t* buf, const packet_format& pf); * * @throw runtime_error if the sensor is in ERROR state, the firmware version * used to initialize the HTTP or TCP client is invalid, the metadata could - * not be retrieved from the sensor, or the response could not be parsed. + * not be retrieved from the sensor within the timeout period, + * a timeout occured while waiting for the sensor to finish initializing, + * or the response could not be parsed. * * @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. @@ -178,7 +181,7 @@ bool read_imu_packet(const client& cli, uint8_t* buf, const packet_format& pf); * * @return a text blob of metadata parseable into a sensor_info struct. */ -std::string get_metadata(client& cli, int timeout_sec = 60, +std::string get_metadata(client& cli, int timeout_sec = DEFAULT_HTTP_REQUEST_TIMEOUT_SECONDS, bool legacy_format = false); /** @@ -193,7 +196,7 @@ std::string get_metadata(client& cli, int timeout_sec = 60, * @return true if sensor config successfully populated. */ bool get_config(const std::string& hostname, sensor_config& config, - bool active = true); + bool active = true, int timeout_sec = DEFAULT_HTTP_REQUEST_TIMEOUT_SECONDS); // clang-format off /** @@ -221,7 +224,7 @@ enum config_flags : uint8_t { * @return true if config params successfuly set on sensor. */ bool set_config(const std::string& hostname, const sensor_config& config, - uint8_t config_flags = 0); + uint8_t config_flags = 0, int timeout_sec = DEFAULT_HTTP_REQUEST_TIMEOUT_SECONDS); /** * Return the port used to listen for lidar UDP data. diff --git a/ouster_client/include/ouster/defaults.h b/ouster_client/include/ouster/defaults.h new file mode 100644 index 00000000..f0c500a9 --- /dev/null +++ b/ouster_client/include/ouster/defaults.h @@ -0,0 +1,3 @@ +#pragma once + +constexpr int DEFAULT_HTTP_REQUEST_TIMEOUT_SECONDS = 40; diff --git a/ouster_client/include/ouster/impl/profile_extension.h b/ouster_client/include/ouster/impl/profile_extension.h new file mode 100644 index 00000000..804e88c0 --- /dev/null +++ b/ouster_client/include/ouster/impl/profile_extension.h @@ -0,0 +1,33 @@ +/** + * Copyright (c) 2023, Ouster, Inc. + * All rights reserved. + */ + +#pragma once + +#include +#include +#include + +#include "ouster/types.h" + +namespace ouster { +namespace sensor { +namespace impl { + +struct FieldInfo { + ChanFieldType ty_tag; + size_t offset; + uint64_t mask; + int shift; +}; + +} // namespace impl + +void add_custom_profile( + int profile_nr, const std::string& name, + const std::vector>& fields, + size_t chan_data_size); + +} // namespace sensor +} // namespace ouster diff --git a/ouster_client/include/ouster/lidar_scan.h b/ouster_client/include/ouster/lidar_scan.h index 3abc9150..6b141394 100644 --- a/ouster_client/include/ouster/lidar_scan.h +++ b/ouster_client/include/ouster/lidar_scan.h @@ -52,6 +52,7 @@ class LidarScan { Header timestamp_; Header measurement_id_; Header status_; + std::vector pose_; std::map fields_; LidarScanFieldTypes field_types_; @@ -236,6 +237,15 @@ class LidarScan { /** @copydoc status() */ Eigen::Ref> status() const; + /** + * Access the vector of poses (per each timestamp). + * + * @return a reference to vector with poses (4x4) homogeneous. + */ + std::vector& pose(); + /** @copydoc pose() */ + const std::vector& pose() const; + /** * Assess completeness of scan. * @param[in] window The column window to use for validity assessment @@ -482,47 +492,6 @@ class ScanBatcher { bool operator()(const uint8_t* packet_buf, LidarScan& ls); }; -/** - * Imu Data - */ -struct Imu { - union { - std::array angular_vel; - struct { - double wx, wy, wz; - }; - }; - union { - std::array linear_accel; - struct { - double ax, ay, az; - }; - }; - union { - std::array ts; - struct { - uint64_t sys_ts, accel_ts, gyro_ts; - }; - }; -}; - -/** Equality for Imu */ -inline bool operator==(const Imu& a, const Imu& b) { - return a.angular_vel == b.angular_vel && a.linear_accel == b.linear_accel && - a.ts == b.ts; -}; - -/** Not Equality for Imu */ -inline bool operator!=(const Imu& a, const Imu& b) { - return !(a == b); -}; - -std::string to_string(const Imu& imu); - -/// Reconstructs buf with UDP imu_packet to osf::Imu object -void packet_to_imu(const uint8_t* buf, const ouster::sensor::packet_format& pf, - Imu& imu); - } // namespace ouster #include "ouster/impl/cartesian.h" diff --git a/ouster_client/include/ouster/sensor_http.h b/ouster_client/include/ouster/sensor_http.h index 2b764a06..8e01fa40 100644 --- a/ouster_client/include/ouster/sensor_http.h +++ b/ouster_client/include/ouster/sensor_http.h @@ -11,6 +11,7 @@ #include #include +#include #include @@ -122,23 +123,23 @@ class SensorHttp { * * @param[in] hostname hostname of the sensor to communicate with. */ - static std::string firmware_version_string(const std::string& hostname); + static std::string firmware_version_string(const std::string& hostname, int timeout_sec = DEFAULT_HTTP_REQUEST_TIMEOUT_SECONDS); /** * Retrieves sensor firmware version information. * * @param[in] hostname hostname of the sensor to communicate with. */ - static ouster::util::version firmware_version(const std::string& hostname); + static ouster::util::version firmware_version(const std::string& hostname, int timeout_sec = DEFAULT_HTTP_REQUEST_TIMEOUT_SECONDS); /** * Creates an instance of the SensorHttp interface. * * @param[in] hostname hostname of the sensor to communicate with. */ - static std::unique_ptr create(const std::string& hostname); + static std::unique_ptr create(const std::string& hostname, int timeout_sec = DEFAULT_HTTP_REQUEST_TIMEOUT_SECONDS); }; } // namespace util } // namespace sensor -} // namespace ouster \ No newline at end of file +} // namespace ouster diff --git a/ouster_client/include/ouster/types.h b/ouster_client/include/ouster/types.h index 3cae90e5..fd433083 100644 --- a/ouster_client/include/ouster/types.h +++ b/ouster_client/include/ouster/types.h @@ -367,7 +367,7 @@ struct data_format { ColumnWindow column_window; ///< window of columns over which sensor fires UDPProfileLidar udp_profile_lidar; ///< profile of lidar packet UDPProfileIMU udp_profile_imu; ///< profile of imu packet - uint16_t fps; ///< frames per second + uint16_t fps; ///< frames per second }; /** Stores necessary information from sensor to parse and project sensor data. @@ -675,10 +675,13 @@ void check_signal_multiplier(const double signal_multiplier); * @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); +sensor_info parse_metadata(const std::string& metadata, + bool skip_beam_validation = false); /** * Parse metadata given path to a json file. @@ -686,10 +689,13 @@ sensor_info parse_metadata(const std::string& metadata); * @throw runtime_error if json file does not exist or is malformed. * * @param[in] json_file path to a json file containing sensor metadata. + * @param[in] skip_beam_validation whether to skip validation on metadata - not + * for use on recorded data or metadata from sensors * * @return a sensor_info struct populated with a subset of the metadata. */ -sensor_info metadata_from_json(const std::string& json_file); +sensor_info metadata_from_json(const std::string& json_file, + bool skip_beam_validation = false); /** * Get a string representation of the sensor_info. All fields included. Not @@ -1165,5 +1171,12 @@ class packet_format final { */ const packet_format& get_format(const sensor_info& info); +namespace impl { + +/** Maximum number of allowed lidar profiles */ +constexpr int MAX_NUM_PROFILES = 32; + +} // namespace impl + } // namespace sensor } // namespace ouster diff --git a/ouster_client/src/client.cpp b/ouster_client/src/client.cpp index c92820b0..e5ab05f4 100644 --- a/ouster_client/src/client.cpp +++ b/ouster_client/src/client.cpp @@ -25,8 +25,8 @@ #include "logging.h" #include "netcompat.h" -#include "ouster/types.h" #include "ouster/sensor_http.h" +#include "ouster/types.h" using namespace std::chrono_literals; namespace chrono = std::chrono; @@ -245,45 +245,51 @@ SOCKET mtp_data_socket(int port, const std::string& udp_dest_host = "", } Json::Value collect_metadata(const std::string& hostname, int timeout_sec) { - auto sensor_http = SensorHttp::create(hostname); + // Note, this function throws std::runtime_error if + // 1. the metadata couldn't be retrieved + // 2. the sensor is in the INITIALIZING state when timeout is reached + auto sensor_http = SensorHttp::create(hostname, timeout_sec); auto timeout_time = chrono::steady_clock::now() + chrono::seconds{timeout_sec}; std::string status; // TODO: can remove this loop when we drop support for FW 2.4 do { - if (chrono::steady_clock::now() >= timeout_time) return false; + if (chrono::steady_clock::now() >= timeout_time) { + throw std::runtime_error( + "A timeout occurred while waiting for the sensor to initialize." + ); + } std::this_thread::sleep_for(1s); status = sensor_http->sensor_info()["status"].asString(); } while (status == "INITIALIZING"); - // not all metadata available when sensor isn't RUNNING - if (status != "RUNNING") { + try { + auto metadata = sensor_http->metadata(); + // merge extra info into metadata + metadata["client_version"] = client_version(); + return metadata; + } catch (const std::runtime_error& e) { throw std::runtime_error( "Cannot obtain full metadata with sensor status: " + status + ". Please ensure that sensor is not in a STANDBY, UNCONFIGURED, " "WARMUP, or ERROR state"); } - - auto metadata = sensor_http->metadata(); - // merge extra info into metadata - metadata["client_version"] = client_version(); - return metadata; } } // namespace bool get_config(const std::string& hostname, sensor_config& config, - bool active) { - auto sensor_http = SensorHttp::create(hostname); + bool active, int timeout_sec) { + auto sensor_http = SensorHttp::create(hostname, timeout_sec); auto res = sensor_http->get_config_params(active); config = parse_config(res); return true; } bool set_config(const std::string& hostname, const sensor_config& config, - uint8_t config_flags) { - auto sensor_http = SensorHttp::create(hostname); + uint8_t config_flags, int timeout_sec) { + auto sensor_http = SensorHttp::create(hostname, timeout_sec); // reset staged config to avoid spurious errors auto config_params = sensor_http->active_config_params(); @@ -358,10 +364,13 @@ bool set_config(const std::string& hostname, const sensor_config& config, } std::string get_metadata(client& cli, int timeout_sec, bool legacy_format) { + // Note, this function calls functions that throw std::runtime_error + // on timeout. try { cli.meta = collect_metadata(cli.hostname, timeout_sec); } catch (const std::exception& e) { - logger().warn(std::string("Unable to retrieve sensor metadata: ") + e.what()); + logger().warn(std::string("Unable to retrieve sensor metadata: ") + + e.what()); throw; } @@ -386,7 +395,7 @@ std::string get_metadata(client& cli, int timeout_sec, bool legacy_format) { // TODO: remove after release of FW 3.2/3.3 (sufficient warning) sensor_config config; get_config(cli.hostname, config); - auto fw_version = SensorHttp::firmware_version(cli.hostname); + auto fw_version = SensorHttp::firmware_version(cli.hostname, timeout_sec); // only warn for people on the latest FW, as people on older FWs may not // care if (fw_version.major >= 3 && @@ -550,8 +559,8 @@ client_state poll_client(const client& c, const int timeout_sec) { } static bool recv_fixed(SOCKET fd, void* buf, int64_t len) { - // Have to read longer than len because you need to know if the packet is - // too large + // Have to read longer than len because you need to know if the packet + // is too large int64_t bytes_read = recv(fd, (char*)buf, len + 1, 0); if (bytes_read == len) { @@ -577,12 +586,15 @@ int get_lidar_port(client& cli) { return get_sock_port(cli.lidar_fd); } int get_imu_port(client& cli) { return get_sock_port(cli.imu_fd); } -bool in_multicast(const std::string& addr) { return IN_MULTICAST(ntohl(inet_addr(addr.c_str()))); } +bool in_multicast(const std::string& addr) { + return IN_MULTICAST(ntohl(inet_addr(addr.c_str()))); +} /** * Return the socket file descriptor used to listen for lidar UDP data. * - * @param[in] cli client returned by init_client associated with the connection. + * @param[in] cli client returned by init_client associated with the + * connection. * * @return the socket file descriptor. */ @@ -591,7 +603,8 @@ extern SOCKET get_lidar_socket_fd(client& cli) { return cli.lidar_fd; } /** * Return the socket file descriptor used to listen for imu UDP data. * - * @param[in] cli client returned by init_client associated with the connection. + * @param[in] cli client returned by init_client associated with the + * connection. * * @return the socket file descriptor. */ diff --git a/ouster_client/src/curl_client.h b/ouster_client/src/curl_client.h index 2b93afc8..7e0295f9 100644 --- a/ouster_client/src/curl_client.h +++ b/ouster_client/src/curl_client.h @@ -6,12 +6,13 @@ class CurlClient : public ouster::util::HttpClient { public: - CurlClient(const std::string& base_url_) : HttpClient(base_url_) { + CurlClient(const std::string& base_url_, int timeout_seconds) : 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 { diff --git a/ouster_client/src/lidar_scan.cpp b/ouster_client/src/lidar_scan.cpp index 40481996..55e552ba 100644 --- a/ouster_client/src/lidar_scan.cpp +++ b/ouster_client/src/lidar_scan.cpp @@ -93,7 +93,9 @@ struct DefaultFieldsEntry { size_t n_fields; }; -Table default_scan_fields{ +using ouster::sensor::impl::MAX_NUM_PROFILES; +// clang-format off +Table default_scan_fields{ {{UDPProfileLidar::PROFILE_LIDAR_LEGACY, {legacy_field_slots.data(), legacy_field_slots.size()}}, {UDPProfileLidar::PROFILE_RNG19_RFL8_SIG16_NIR16_DUAL, @@ -126,10 +128,10 @@ LidarScan::LidarScan(size_t w, size_t h, LidarScanFieldTypes field_types) : timestamp_{Header::Zero(w)}, 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)} { - // TODO: error on duplicate fields for (const auto& ft : field_types_) { if (fields_.count(ft.first) > 0) throw std::invalid_argument("Duplicated fields found"); @@ -206,6 +208,9 @@ Eigen::Ref> LidarScan::status() const { return status_; } +std::vector& LidarScan::pose() { return pose_; } +const std::vector& LidarScan::pose() const { return pose_; } + bool LidarScan::complete(sensor::ColumnWindow window) const { const auto& status = this->status(); auto start = window.first; @@ -231,7 +236,7 @@ bool operator==(const LidarScan& a, const LidarScan& b) { a.field_types_ == b.field_types_ && (a.timestamp() == b.timestamp()).all() && (a.measurement_id() == b.measurement_id()).all() && - (a.status() == b.status()).all(); + (a.status() == b.status()).all() && a.pose() == b.pose(); } LidarScanFieldTypes get_field_types(const LidarScan& ls) { @@ -289,7 +294,7 @@ std::string to_string(const LidarScan& ls) { auto st = ls.status().cast(); ss << " status = (" << st.minCoeff() << "; " << st.mean() << "; " << st.maxCoeff() << ")" << std::endl; - + ss << " poses = (size: " << ls.pose().size() << ")" << std::endl; ss << "}"; return ss.str(); } @@ -633,46 +638,4 @@ bool ScanBatcher::operator()(const uint8_t* packet_buf, LidarScan& ls) { return false; } -std::string to_string(const Imu& imu) { - std::stringstream ss; - ss << "Imu: "; - ss << "linear_accel: ["; - for (size_t i = 0; i < imu.linear_accel.size(); ++i) { - if (i > 0) ss << ", "; - ss << imu.linear_accel[i]; - } - ss << "]"; - ss << ", angular_vel = ["; - for (size_t i = 0; i < imu.angular_vel.size(); ++i) { - if (i > 0) ss << ", "; - ss << imu.angular_vel[i]; - } - ss << "]"; - ss << ", ts: ["; - std::array labels{"sys_ts", "accel_ts", "gyro_ts"}; - for (size_t i = 0; i < imu.ts.size(); ++i) { - if (i > 0) ss << ", "; - ss << labels[i] << " = "; - ss << imu.ts[i]; - } - ss << "]"; - return ss.str(); -} - -void packet_to_imu(const uint8_t* buf, const ouster::sensor::packet_format& pf, - Imu& imu) { - // Storing all available timestamps - imu.sys_ts = pf.imu_sys_ts(buf); - imu.accel_ts = pf.imu_accel_ts(buf); - imu.gyro_ts = pf.imu_gyro_ts(buf); - - imu.linear_accel[0] = pf.imu_la_x(buf); - imu.linear_accel[1] = pf.imu_la_y(buf); - imu.linear_accel[2] = pf.imu_la_z(buf); - - imu.angular_vel[0] = pf.imu_av_x(buf); - imu.angular_vel[1] = pf.imu_av_y(buf); - imu.angular_vel[2] = pf.imu_av_z(buf); -} - } // namespace ouster diff --git a/ouster_client/src/parsing.cpp b/ouster_client/src/parsing.cpp index 72ee3d8a..f918e2d9 100644 --- a/ouster_client/src/parsing.cpp +++ b/ouster_client/src/parsing.cpp @@ -103,7 +103,7 @@ static const Table five_word_pixel_info{{ {ChanField::RAW32_WORD5, {UINT32, 16, 0, 0}}, }}; -Table profiles{{ +Table profiles{{ {UDPProfileLidar::PROFILE_LIDAR_LEGACY, {legacy_field_info.data(), legacy_field_info.size(), 12}}, {UDPProfileLidar::PROFILE_RNG19_RFL8_SIG16_NIR16_DUAL, diff --git a/ouster_client/src/profile_extension.cpp b/ouster_client/src/profile_extension.cpp new file mode 100644 index 00000000..eed240b8 --- /dev/null +++ b/ouster_client/src/profile_extension.cpp @@ -0,0 +1,159 @@ +/** + * Copyright (c) 2023, Ouster, Inc. + * All rights reserved. + */ + +#include "ouster/impl/profile_extension.h" + +#include +#include +#include +#include + +#include "ouster/types.h" + +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; + +namespace ouster { + +namespace impl { + +// definition copied from lidar_scan.cpp +struct DefaultFieldsEntry { + const std::pair* fields; + size_t n_fields; +}; + +// lidar_scan.cpp +extern Table + default_scan_fields; + +static void extend_default_scan_fields( + UDPProfileLidar profile, + 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; }); + + if (it == end) + throw std::runtime_error("Limit of scan fields has been reached"); + + *it = {profile, {scan_fields.data(), scan_fields.size()}}; +} + +} // namespace impl + +namespace sensor { +namespace impl { + +struct ExtendedProfile { + UDPProfileLidar profile; + std::string name; + std::vector> slots; + std::vector> fields; + size_t chan_data_size; +}; + +/** + * Storage container for dynamically added profiles + * + * used with std::list because we need pointers to elements inside to stay valid + * when new elements are added + */ +std::list extended_profiles_data{}; + +// definition copied from parsing.cpp +struct ProfileEntry { + const std::pair* fields; + size_t n_fields; + size_t chan_data_size; +}; + +// types.cpp +extern Table + udp_profile_lidar_strings; +// parsing.cpp +extern Table profiles; + +static void extend_profile_entries( + UDPProfileLidar profile, + const std::vector>& fields, + size_t chan_data_size) { + auto end = impl::profiles.end(); + auto it = std::find_if(impl::profiles.begin(), end, + [](const auto& kv) { return kv.first == 0; }); + + if (it == end) + throw std::runtime_error("Limit of parsing profiles has been reached"); + + *it = {profile, {fields.data(), fields.size(), chan_data_size}}; +} + +} // namespace impl + +void extend_udp_profile_lidar_strings(UDPProfileLidar profile, + const char* name) { + auto begin = impl::udp_profile_lidar_strings.begin(); + auto end = impl::udp_profile_lidar_strings.end(); + + if (end != std::find_if(begin, end, [profile](const auto& p) { + return p.first == profile; + })) + throw std::invalid_argument( + "Lidar profile of given number already exists"); + if (end != std::find_if(begin, end, [name](const auto& p) { + return p.second && std::strcmp(p.second, name) == 0; + })) + throw std::invalid_argument( + "Lidar profile of given name already exists"); + + auto it = + std::find_if(begin, end, [](const auto& kv) { return kv.first == 0; }); + + if (it == end) + throw std::runtime_error("Limit of lidar profiles has been reached"); + + *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) { + if (profile_nr == 0) + throw std::invalid_argument("profile_nr of 0 are not allowed"); + + auto udp_profile = static_cast(profile_nr); + + { + // fill in profile + 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); + } + + impl::extended_profiles_data.push_back(std::move(profile)); + } + + // retake reference to stored data, profile.name.c_str() can get invalidated + // after move + auto&& profile = impl::extended_profiles_data.back(); + + extend_udp_profile_lidar_strings(profile.profile, profile.name.c_str()); + impl::extend_profile_entries(profile.profile, profile.fields, + profile.chan_data_size); + ouster::impl::extend_default_scan_fields(profile.profile, profile.slots); +} + +} // namespace sensor +} // namespace ouster diff --git a/ouster_client/src/sensor_http.cpp b/ouster_client/src/sensor_http.cpp index 964c4133..da5fedf7 100644 --- a/ouster_client/src/sensor_http.cpp +++ b/ouster_client/src/sensor_http.cpp @@ -14,13 +14,13 @@ using namespace ouster::sensor; using namespace ouster::sensor::util; using namespace ouster::sensor::impl; -string SensorHttp::firmware_version_string(const string& hostname) { - auto http_client = std::make_unique("http://" + hostname); +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"); } -version SensorHttp::firmware_version(const string& hostname) { - auto result = firmware_version_string(hostname); +version SensorHttp::firmware_version(const string& hostname, int timeout_sec) { + auto result = firmware_version_string(hostname, timeout_sec); auto rgx = std::regex(R"(v(\d+).(\d+)\.(\d+))"); std::smatch matches; std::regex_search(result, matches, rgx); @@ -36,8 +36,8 @@ version SensorHttp::firmware_version(const string& hostname) { } } -std::unique_ptr SensorHttp::create(const string& hostname) { - auto fw = firmware_version(hostname); +std::unique_ptr SensorHttp::create(const string& hostname, int timeout_sec) { + auto fw = firmware_version(hostname, timeout_sec); if (fw == invalid_version || fw.major < 2) { throw std::runtime_error( @@ -52,11 +52,11 @@ 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); + return std::make_unique(hostname, timeout_sec); case 2: - return std::make_unique(hostname); + return std::make_unique(hostname, timeout_sec); } } - return std::make_unique(hostname); + return std::make_unique(hostname, timeout_sec); } diff --git a/ouster_client/src/sensor_http_imp.cpp b/ouster_client/src/sensor_http_imp.cpp index 8f03405b..2225057a 100644 --- a/ouster_client/src/sensor_http_imp.cpp +++ b/ouster_client/src/sensor_http_imp.cpp @@ -5,8 +5,8 @@ using std::string; using namespace ouster::sensor::impl; -SensorHttpImp::SensorHttpImp(const string& hostname) - : http_client(std::make_unique("http://" + hostname)) {} +SensorHttpImp::SensorHttpImp(const string& hostname, int timeout_sec) + : http_client(std::make_unique("http://" + hostname, timeout_sec)) {} SensorHttpImp::~SensorHttpImp() = default; @@ -96,16 +96,16 @@ void SensorHttpImp::execute(const string& url, const string& validation) const { validation + "]"); } -SensorHttpImp_2_2::SensorHttpImp_2_2(const string& hostname) - : SensorHttpImp(hostname) {} +SensorHttpImp_2_2::SensorHttpImp_2_2(const string& hostname, int timeout_sec) + : SensorHttpImp(hostname, timeout_sec) {} void SensorHttpImp_2_2::set_udp_dest_auto() const { return execute("api/v1/sensor/cmd/set_udp_dest_auto", "\"set_config_param\""); } -SensorHttpImp_2_1::SensorHttpImp_2_1(const string& hostname) - : SensorHttpImp_2_2(hostname) {} +SensorHttpImp_2_1::SensorHttpImp_2_1(const string& hostname, int timeout_sec) + : SensorHttpImp_2_2(hostname, timeout_sec) {} Json::Value SensorHttpImp_2_1::metadata() const { Json::Value root; diff --git a/ouster_client/src/sensor_http_imp.h b/ouster_client/src/sensor_http_imp.h index f8827e23..dc5ae06b 100644 --- a/ouster_client/src/sensor_http_imp.h +++ b/ouster_client/src/sensor_http_imp.h @@ -26,7 +26,7 @@ class SensorHttpImp : public util::SensorHttp { * * @param[in] hostname hostname of the sensor to communicate with. */ - SensorHttpImp(const std::string& hostname); + SensorHttpImp(const std::string& hostname, int timeout_sec); /** * Deconstruct the sensor http interface. @@ -130,7 +130,7 @@ class SensorHttpImp : public util::SensorHttp { // TODO: remove when firmware 2.2 has been fully phased out class SensorHttpImp_2_2 : public SensorHttpImp { public: - SensorHttpImp_2_2(const std::string& hostname); + SensorHttpImp_2_2(const std::string& hostname, int timeout_sec); void set_udp_dest_auto() const override; }; @@ -145,7 +145,7 @@ class SensorHttpImp_2_1 : public SensorHttpImp_2_2 { * * @param[in] hostname hostname of the sensor to communicate with. */ - SensorHttpImp_2_1(const std::string& hostname); + SensorHttpImp_2_1(const std::string& hostname, int timeout_sec); /** * Queries the sensor metadata. diff --git a/ouster_client/src/types.cpp b/ouster_client/src/types.cpp index a26cfe92..9f2ffaa9 100644 --- a/ouster_client/src/types.cpp +++ b/ouster_client/src/types.cpp @@ -101,7 +101,8 @@ Table chanfield_strings{{ {ChanField::RAW32_WORD9, "RAW32_WORD9"}, }}; -Table udp_profile_lidar_strings{{ +// clang-format off +Table udp_profile_lidar_strings{{ {PROFILE_LIDAR_LEGACY, "LEGACY"}, {PROFILE_RNG19_RFL8_SIG16_NIR16_DUAL, "RNG19_RFL8_SIG16_NIR16_DUAL"}, {PROFILE_RNG19_RFL8_SIG16_NIR16, "RNG19_RFL8_SIG16_NIR16"}, @@ -368,7 +369,7 @@ static optional rlookup(const impl::Table table, auto end = table.end(); auto res = std::find_if(table.begin(), end, [&](const std::pair& p) { - return std::strcmp(p.second, v) == 0; + return p.second && std::strcmp(p.second, v) == 0; }); return res == end ? nullopt : make_optional(res->first); @@ -760,7 +761,8 @@ static data_format parse_data_format(const Json::Value& root) { if (profile) { format.udp_profile_lidar = profile.value(); } else { - throw std::runtime_error{"Unexpected udp lidar profile"}; + throw std::runtime_error{"Unexpected udp lidar profile: " + + root["udp_profile_lidar"].asString()}; } } else { logger().warn("No lidar profile found. Using LEGACY lidar profile"); @@ -791,7 +793,7 @@ static data_format parse_data_format(const Json::Value& root) { return format; } // namespace sensor -static sensor_info parse_legacy(const std::string& meta) { +static sensor_info parse_legacy(const std::string& meta, bool skip_beam_validation) { Json::Value root{}; Json::CharReaderBuilder builder{}; std::string errors{}; @@ -1030,8 +1032,12 @@ static sensor_info parse_legacy(const std::string& meta) { } }; - zero_check(info.beam_altitude_angles, "beam_altitude_angles"); - zero_check(info.beam_azimuth_angles, "beam_azimuth_angles"); + if (!skip_beam_validation) { + zero_check(info.beam_altitude_angles, "beam_altitude_angles"); + zero_check(info.beam_azimuth_angles, "beam_azimuth_angles"); + } else { + logger().warn("Skipping all 0 beam angle check"); + } info.extrinsic = mat4d::Identity(); @@ -1101,7 +1107,7 @@ std::string convert_to_legacy(const std::string& metadata) { return Json::writeString(write_builder, result); } -sensor_info parse_metadata(const std::string& metadata) { +sensor_info parse_metadata(const std::string& metadata, bool skip_beam_validation) { Json::Value root{}; Json::CharReaderBuilder builder{}; std::string errors{}; @@ -1116,15 +1122,15 @@ sensor_info parse_metadata(const std::string& metadata) { sensor_info info{}; if (is_new_format(metadata)) { logger().debug("parsing non-legacy metadata format"); - info = parse_legacy(convert_to_legacy(metadata)); + info = parse_legacy(convert_to_legacy(metadata), skip_beam_validation); } else { logger().debug("parsing legacy metadata format"); - info = parse_legacy(metadata); + info = parse_legacy(metadata, skip_beam_validation); } return info; } -sensor_info metadata_from_json(const std::string& json_file) { +sensor_info metadata_from_json(const std::string& json_file, bool skip_beam_validation) { std::stringstream buf{}; std::ifstream ifs{}; ifs.open(json_file); @@ -1137,7 +1143,7 @@ sensor_info metadata_from_json(const std::string& json_file) { throw std::runtime_error{ss.str()}; } - return parse_metadata(buf.str()); + return parse_metadata(buf.str(), skip_beam_validation); } std::string to_string(const sensor_info& info) { diff --git a/ouster_osf/CHANGELOG.rst b/ouster_osf/CHANGELOG.rst new file mode 100644 index 00000000..ef9fdd82 --- /dev/null +++ b/ouster_osf/CHANGELOG.rst @@ -0,0 +1,6 @@ +2023-07-01 +=========== + +Initial public Ouster OSF soft release + +* C++ OSF lib with Python binding \ No newline at end of file diff --git a/ouster_osf/CMakeLists.txt b/ouster_osf/CMakeLists.txt new file mode 100644 index 00000000..768c082b --- /dev/null +++ b/ouster_osf/CMakeLists.txt @@ -0,0 +1,158 @@ +# ==== Debug params ============ +# == NOTE(pb): Left intentionally +# set(CMAKE_VERBOSE_MAKEFILE OFF) +# set(CMAKE_FIND_DEBUG_MODE OFF) + +option(OUSTER_OSF_NO_MMAP "Don't use mmap(), useful for WASM targets" OFF) +option(OUSTER_OSF_NO_THREADING "Don't use threads, useful for WASM targets" OFF) + +# ==== Requirements ==== +find_package(ZLIB REQUIRED) +find_package(PNG REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(jsoncpp REQUIRED) +find_package(spdlog REQUIRED) + +# TODO: Extract to a separate FindFlatbuffers cmake file +# Flatbuffers flatc resolution and different search name 'flatbuffers` with Conan +# NOTE2[pb]: 200221007: We changed Conan cmake package to look to `flatbuffers` +# because it started failing out of blue :idk:scream: will see. +if(NOT CONAN_EXPORTED) + find_package(Flatbuffers REQUIRED) + if(NOT DEFINED FLATBUFFERS_FLATC_EXECUTABLE) + set(FLATBUFFERS_FLATC_EXECUTABLE flatbuffers::flatc) + endif() + message(STATUS "Flatbuffers found: ${Flatbuffers_DIR}" ) +else() + find_package(flatbuffers REQUIRED) + if(WIN32) + set(FLATBUFFERS_FLATC_EXECUTABLE flatc.exe) + else() + set(FLATBUFFERS_FLATC_EXECUTABLE flatc) + endif() + message(STATUS "flatbuffers found: ${Flatbuffers_DIR}" ) +endif() + +# TODO[pb]: Move to flatbuffers 2.0 and check do we still need this??? +# Using this link lib search method so to get shared .so library and not +# static in Debian systems. But it correctly find static lib in vcpkg/manylinux +# builds. +# STORY: We need to make it static (but with -fPIC) for Python bindings. +# However in Debian packages we can only use shared libs because static +# are not compiled with PIC. Though in vcpkg it uses static lib, +# which we've confirmed to be the case and what we need for manylinux. +# find_library(flatbuffers_lib NAMES flatbuffers REQUIRED) +# set(flatbuffers_lib flatbuffers::flatbuffers) + +# === Flatbuffer builder functions ==== +include(cmake/osf_fb_utils.cmake) + +set(OSF_FB_DIR ${CMAKE_CURRENT_SOURCE_DIR}/fb) + +set(FB_SOURCE_GEN_DIR ${CMAKE_CURRENT_BINARY_DIR}/fb_source_generated) +set(FB_BINARY_SCHEMA_DIR ${CMAKE_CURRENT_BINARY_DIR}/fb_binary_schemas) + +set(FB_MODULES_TO_BUILD os_sensor streaming ml) + +# ======= Typescript Flatbuffer Generators ===== +set(FB_TS_GENERATED_DIR ${FB_SOURCE_GEN_DIR}/ts) +build_ts_fb_modules( + TARGET ts_gen + FB_DIR "${OSF_FB_DIR}" + FB_MODULES "${FB_MODULES_TO_BUILD}" + SOURCE_GEN_DIR "${FB_TS_GENERATED_DIR}" + ) + +# ======= Python Flatbuffer Generators ===== +set(FB_PYTHON_GENERATED_DIR ${FB_SOURCE_GEN_DIR}/python) +build_py_fb_modules( + TARGET py_gen + FB_DIR "${OSF_FB_DIR}" + SOURCE_GEN_DIR "${FB_PYTHON_GENERATED_DIR}" + ) + +# ======= C++ Flatbuffer Generators ===== +set(FB_CPP_GENERATED_DIR ${FB_SOURCE_GEN_DIR}/cpp) +build_cpp_fb_modules( + TARGET cpp_gen + FB_DIR "${OSF_FB_DIR}" + FB_MODULES "${FB_MODULES_TO_BUILD}" + SOURCE_GEN_DIR "${FB_CPP_GENERATED_DIR}" + BINARY_SCHEMA_DIR "${FB_BINARY_SCHEMA_DIR}" + ) + +# === Always generate C++ stubs ============== +# and skip Typescript and Python code from FB specs generation +# since they not needed during a regular OSF lib builds +add_custom_target(all_fb_gen ALL DEPENDS cpp_gen) # ts_gen py_gen + +add_library(ouster_osf STATIC src/compat_ops.cpp + src/png_tools.cpp + src/basics.cpp + src/crc32.cpp + src/metadata.cpp + src/writer.cpp + src/meta_lidar_sensor.cpp + src/meta_extrinsics.cpp + src/meta_streaming_info.cpp + src/stream_lidar_scan.cpp + src/layout_standard.cpp + src/layout_streaming.cpp + src/file.cpp + src/reader.cpp + src/operations.cpp + src/json_utils.cpp + src/fb_utils.cpp + src/pcap_source.cpp +) + +if (OUSTER_OSF_NO_MMAP) + target_compile_definitions(ouster_osf PRIVATE OUSTER_OSF_NO_MMAP) +endif() + +if (OUSTER_OSF_NO_THREADING) + target_compile_definitions(ouster_osf PRIVATE OUSTER_OSF_NO_THREADING) +endif() + +# Include Flatbuffers generated C++ headers +target_include_directories(ouster_osf PUBLIC + $ + $ +) +target_link_libraries(ouster_osf + PUBLIC + OusterSDK::ouster_client OusterSDK::ouster_pcap PNG::PNG + flatbuffers::flatbuffers ZLIB::ZLIB jsoncpp_lib +) +target_include_directories(ouster_osf PUBLIC + $ + $ + ${EIGEN3_INCLUDE_DIR} +) +add_dependencies(ouster_osf cpp_gen) +add_library(OusterSDK::ouster_osf ALIAS ouster_osf) + + +# Check if ouster_client compiled with -mavx2 option and add those to ouster_osf +# If we are not matching -mavx2 compile flag Eigen lib functions might crash with +# SegFault and double free/memory corruption errors... +get_target_property(CLIENT_OPTIONS OusterSDK::ouster_client COMPILE_OPTIONS) +if(CLIENT_OPTIONS MATCHES ".*-mavx2.*") + target_compile_options(ouster_osf PUBLIC -mavx2) +endif() + +if(CMAKE_PROJECT_NAME STREQUAL PROJECT_NAME AND BUILD_TESTING) + enable_testing() + add_subdirectory(tests) +endif() + +# ==== Install ========================================================= +install(TARGETS ouster_osf + EXPORT ouster-sdk-targets + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include) + +install(DIRECTORY include/ouster DESTINATION include) +install(DIRECTORY ${FB_CPP_GENERATED_DIR}/ DESTINATION include/fb_generated) diff --git a/ouster_osf/README.rst b/ouster_osf/README.rst new file mode 100644 index 00000000..a9b54725 --- /dev/null +++ b/ouster_osf/README.rst @@ -0,0 +1,31 @@ +=================== +Ouster OSF library +=================== + +:Maintainers: Pavlo Bashmakov +:Description: OSF File format and tools to store/process lidar sensor data +:Project-type: lib + +Summary +------- + +Ouster OSF is an extendable file format to efficiently store the lidar information +with sensor intrinsics and streaming capabilities. + +This is the beginning of the public OSF to be able to read and process OSF files +generated by Ouster tools (i.e. mapping or Data App) + + +Build & Test +------------- + +See the main Ouser SDK Building docs for details. + + +TODOs +----- + +- [ ] OSF API Documentaion +- [ ] OSF Usage Documentaion +- [ ] OSF examples +- [ ] more OSF related tools ... \ No newline at end of file diff --git a/ouster_osf/cmake/osf_fb_utils.cmake b/ouster_osf/cmake/osf_fb_utils.cmake new file mode 100644 index 00000000..4f909d4b --- /dev/null +++ b/ouster_osf/cmake/osf_fb_utils.cmake @@ -0,0 +1,395 @@ +# ======================================================================= +# ======== Flatbuffers Generators for C++/Python/Javasctipt ============= + +# This file provides three generator functions that makes rules for convertion +# of Flatbuffer's specs (*.fbs) to the corresponding language stubs: +# - 'build_cpp_fb_modules' - for C++ include headers +# - 'build_ts_fb_modules' - for Typescript modules +# - 'build_py_fb_modules' - for Python modules + +# All functions have the common structure and use the common parameters +# (with binary schemas generators available only in C++): + +# TARGET - (REQUIRED) Name of the target that will combine all custom +# generator commands. It can later be used as dependency for other +# targets (including ALL - to always generate code if needed) + +# FB_DIR - (OPTIONAL) Root of Flatbuffers definitions, where specs organized +# by modules. (see FB_MODULES param) +# Example layout of FB_DIR ('project-luna/fb') for imaginary +# 'project-luna': +# - project-luna +# - fb +# - background_rad.fbs +# - rover_base +# - rover_state.fbs +# - sars_stream.fbs +# - laser_eye +# - photons_stream.fbs +# - neutrino_extrinsics.fbs +# default: if ommited defaults to 'current-project/fb' +# (i.e. "${CMAKE_CURRENT_SOURCE_DIR}/fb") + +# FB_MODULES - (OPTIONAL) List of modules (i.e. subdirectories of FB_DIR) +# to use for code generation. The resulting subdirectories +# structure is preserved (Typescript/C++ only). +# For example (C++) file 'FB_DIR/rover_base/sars_stream.fbs' will +# be converted to 'SOURCE_GEN_DIR/reover_base/sars_stream_generated.h' +# For the 'project-luna' above we have two modules: +# 'rover_base' and 'laser_eye'. +# default: empty, i.e. no modules used for generation and only +# root *.fbs files (FB_DIR/*.fbs) used. (except Python, +# where FB_MODULES is not used and all files recursively +# is generated) + +# SOURCE_GEN_DIR - (OPTIONAL) Location of the generated source files. +# It's also available as TARGET property by SOURCE_GEN_DIR +# name which is usefull if using default SOURCE_GEN_DIR +# location. +# default: "${CMAKE_CURRENT_BINARY_DIR}/fb_source_generated" + +# BINARY_SCHEMA_DIR - (OPTIONAL) Location of the generated binary schemas +# files (*.bfbs). +# It's also available as property BINARY_SCHEMA_DIR +# for generated TARGET. +# default: "${CMAKE_CURRENT_BINARY_DIR}/fb_binary_schemas" + + +# ======= Common Initializer ============================================= + +macro(initialize_osf_fb_utils_defaults) + # Use flatc reolve from flatbuffers/CMake/BuildFlatBuffers.cmake source + # Test if including from FindFlatBuffers + if(FLATBUFFERS_FLATC_EXECUTABLE) + set(FLATC ${FLATBUFFERS_FLATC_EXECUTABLE}) + else() + set(FLATC flatc) + endif() + set(FB_DEFAULT_ROOT_DIR "${CMAKE_CURRENT_SOURCE_DIR}/fb") + set(FB_DEFAULT_SOURCE_GEN_DIR "${CMAKE_CURRENT_BINARY_DIR}/fb_source_generated") + set(FB_DEFAULT_BINARY_SCHEMA_DIR "${CMAKE_CURRENT_BINARY_DIR}/fb_binary_schemas") +endmacro() + +# --- Log/debug helper function +# logs messages if FB_VERBOSE is true (in cmake sense, e.g. 1, Y, TRUE, YES, ON) +macro(fb_log MSG) + if(FB_VERBOSE) + message(STATUS ${MSG}) + endif() +endmacro() + + +# ======= C++ Generation ============================================= + +# --- Helper macro: +# Generates Flatbuffers binary schema generation target for FB_FILE in FB_MODULE +# Result placed in RESULT_DIR +# Side effect: Variable ALL_GEN_FILE appended with generated file. +macro(make_binary_schemas_command) + set(PARSE_ONE_VALUE_ARGS FB_FILE FB_MODULE RESULT_DIR) + cmake_parse_arguments(MOD_ARG "" "${PARSE_ONE_VALUE_ARGS}" "" ${ARGN}) + fb_log("BIN MOD: FB_FILE = ${MOD_ARG_FB_FILE}") + fb_log("BIN MOD: FB_MODULE = ${MOD_ARG_FB_MODULE}") + fb_log("BIN MOD: RESULT_DIR = ${MOD_ARG_RESULT_DIR}") + if (NOT ${MOD_ARG_FB_MODULE} STREQUAL "") + set(BIN_DIR ${MOD_ARG_RESULT_DIR}/${MOD_ARG_FB_MODULE}) + else() + set(BIN_DIR ${MOD_ARG_RESULT_DIR}) + endif() + get_filename_component(FB_NAME ${MOD_ARG_FB_FILE} NAME_WE) + set(BIN_FILE ${BIN_DIR}/${FB_NAME}.bfbs) + add_custom_command( + OUTPUT ${BIN_FILE} + DEPENDS ${MOD_ARG_FB_FILE} + COMMAND + ${FLATC} -b --schema + -o ${BIN_DIR} + ${MOD_ARG_FB_FILE} + ) + list(APPEND ALL_GEN_FILES ${BIN_FILE}) +endmacro() + +# --- Helper macro: +# Generates Flatbuffers C++ schema generation target for FB_FILE in FB_MODULE +# Result placed in RESULT_DIR +# Side effect: Variable ALL_GEN_FILE appended with generated file. +macro(make_cpp_gen_command) + set(PARSE_ONE_VALUE_ARGS FB_FILE FB_MODULE RESULT_DIR) + cmake_parse_arguments(MOD_ARG "" "${PARSE_ONE_VALUE_ARGS}" "" ${ARGN}) + fb_log("CPP MOD: FB_FILE = ${MOD_ARG_FB_FILE}") + fb_log("CPP MOD: FB_MODULE = ${MOD_ARG_FB_MODULE}") + fb_log("CPP MOD: RESULT_DIR = ${MOD_ARG_RESULT_DIR}") + if (NOT ${MOD_ARG_FB_MODULE} STREQUAL "") + set(GEN_DIR ${MOD_ARG_RESULT_DIR}/${MOD_ARG_FB_MODULE}) + else() + set(GEN_DIR ${MOD_ARG_RESULT_DIR}) + endif() + get_filename_component(FB_NAME ${MOD_ARG_FB_FILE} NAME_WE) + set(GEN_FILE ${GEN_DIR}/${FB_NAME}_generated.h) + add_custom_command( + OUTPUT ${GEN_FILE} + DEPENDS ${MOD_ARG_FB_FILE} + COMMAND + ${FLATC} --cpp --no-includes --scoped-enums + -o ${GEN_DIR} + ${MOD_ARG_FB_FILE} + ) + list(APPEND ALL_GEN_FILES ${GEN_FILE}) +endmacro() + +# C++ Flatbuffer generators +# Params: TARGET (REQUIRED), FB_DIR, FB_MODULES, SOURCE_GEN_DIR, BINARY_SCHEMA_DIR +# Properties available on generated TARGET: SOURCE_GEN_DIR, BINARY_SCHEMA_DIR +function(build_cpp_fb_modules) + + initialize_osf_fb_utils_defaults() + + set(PARSE_OPTIONS "") + set(PARSE_ONE_VALUE_ARGS + TARGET + FB_DIR + SOURCE_GEN_DIR + BINARY_SCHEMA_DIR + ) + set(PARSE_MULTI_VALUE_ARGS FB_MODULES) + cmake_parse_arguments(ARGS "${PARSE_OPTIONS}" "${PARSE_ONE_VALUE_ARGS}" + "${PARSE_MULTI_VALUE_ARGS}" ${ARGN} ) + + fb_log("... BUILDING CPP FB MODULES ............ ") + fb_log("ARGS_TARGET = ${ARGS_TARGET}") + fb_log("ARGS_FB_DIR = ${ARGS_FB_DIR}") + fb_log("ARGS_FB_MODULES = ${ARGS_FB_MODULES}") + fb_log("ARG_SOURCE_GEN_DIR = ${ARGS_SOURCE_GEN_DIR}") + fb_log("ARG_BINARY_GEN_DIR = ${ARGS_BINARY_SCHEMA_DIR}") + + if (NOT DEFINED ARGS_FB_DIR) + set(ARGS_FB_DIR "${FB_DEFAULT_ROOT_DIR}") + fb_log("using default FB_DIR = ${ARGS_FB_DIR}") + endif() + + if (NOT DEFINED ARGS_SOURCE_GEN_DIR) + set(ARGS_SOURCE_GEN_DIR "${FB_DEFAULT_SOURCE_GEN_DIR}/cpp") + fb_log("using default SOURCE_GEN_DIR = ${ARGS_SOURCE_GEN_DIR}") + endif() + + if (NOT DEFINED ARGS_BINARY_SCHEMA_DIR) + set(ARGS_BINARY_SCHEMA_DIR "${FB_DEFAULT_BINARY_SCHEMA_DIR}") + fb_log("using default BINARY_SCHEMA_DIR = ${ARGS_BINARY_SCHEMA_DIR}") + endif() + + # List accumulated in macros 'make_*_command' calls + set(ALL_GEN_FILES "") + + file(GLOB MOD_FB_CORE_FILES LIST_DIRECTORIES false ${ARGS_FB_DIR}/*.fbs) + foreach(FB_FILE ${MOD_FB_CORE_FILES}) + + make_cpp_gen_command( + FB_FILE ${FB_FILE} + RESULT_DIR ${ARGS_SOURCE_GEN_DIR} + ) + + if (NOT ${ARGS_BINARY_SCHEMA_DIR} STREQUAL "") + make_binary_schemas_command( + FB_FILE ${FB_FILE} + RESULT_DIR ${ARGS_BINARY_SCHEMA_DIR} + ) + endif() + + endforeach() + + foreach(FB_MODULE ${ARGS_FB_MODULES}) + file(GLOB FB_MODULE_FILES LIST_DIRECTORIES false ${ARGS_FB_DIR}/${FB_MODULE}/*.fbs) + + foreach(FB_FILE ${FB_MODULE_FILES}) + + make_cpp_gen_command( + FB_FILE ${FB_FILE} + FB_MODULE ${FB_MODULE} + RESULT_DIR ${ARGS_SOURCE_GEN_DIR} + ) + + if (NOT ${ARGS_BINARY_SCHEMA_DIR} STREQUAL "") + make_binary_schemas_command( + FB_FILE ${FB_FILE} + FB_MODULE ${FB_MODULE} + RESULT_DIR ${ARGS_BINARY_SCHEMA_DIR} + ) + endif() + + endforeach() + + endforeach() + + add_custom_target(${ARGS_TARGET} DEPENDS ${ALL_GEN_FILES} + COMMENT "Generating C++ code from Flatbuffers spec") + + set_property(TARGET ${ARGS_TARGET} + PROPERTY SOURCE_GEN_DIR + ${ARGS_SOURCE_GEN_DIR} + ) + + set_property(TARGET ${ARGS_TARGET} + PROPERTY BINARY_SCHEMA_DIR + ${ARGS_BINARY_SCHEMA_DIR} + ) + +endfunction() + + +# ======= Typescript Generation ======================================== + +# --- Helper macro: +# Generates Flatbuffers Typescript generation command for FB_FILE in FB_MODULE +# Result placed in RESULT_DIR +# Side effect: Variable ALL_GEN_FILE appended with generated file. +macro(make_ts_gen_command) + set(PARSE_ONE_VALUE_ARGS FB_FILE FB_MODULE RESULT_DIR) + cmake_parse_arguments(MOD_ARG "" "${PARSE_ONE_VALUE_ARGS}" "" ${ARGN}) + fb_log("TS MOD: FB_FILE = ${MOD_ARG_FB_FILE}") + fb_log("TS MOD: FB_MODULE = ${MOD_ARG_FB_MODULE}") + fb_log("TS MOD: RESULT_DIR = ${MOD_ARG_RESULT_DIR}") + if (NOT ${MOD_ARG_FB_MODULE} STREQUAL "") + set(GEN_DIR ${MOD_ARG_RESULT_DIR}/${MOD_ARG_FB_MODULE}) + else() + set(GEN_DIR ${MOD_ARG_RESULT_DIR}) + endif() + get_filename_component(FB_NAME ${MOD_ARG_FB_FILE} NAME_WE) + set(GEN_FILE ${GEN_DIR}/${FB_NAME}_generated.ts) + add_custom_command( + OUTPUT ${GEN_FILE} + DEPENDS ${MOD_ARG_FB_FILE} + COMMAND + ${FLATC} --ts + -o ${GEN_DIR} + ${MOD_ARG_FB_FILE} + ) + list(APPEND ALL_GEN_FILES ${GEN_FILE}) +endmacro() + +# Typescript Flatbuffer generators +# Params: TARGET (REQUIRED), FB_DIR, FB_MODULES, SOURCE_GEN_DIR +# Properties available on generated TARGET: SOURCE_GEN_DIR +function(build_ts_fb_modules) + + initialize_osf_fb_utils_defaults() + + set(PARSE_OPTIONS "") + set(PARSE_ONE_VALUE_ARGS + TARGET + FB_DIR + SOURCE_GEN_DIR + ) + set(PARSE_MULTI_VALUE_ARGS FB_MODULES) + cmake_parse_arguments(ARGS "${PARSE_OPTIONS}" "${PARSE_ONE_VALUE_ARGS}" + "${PARSE_MULTI_VALUE_ARGS}" ${ARGN} ) + + fb_log("... BUILDING TS FB MODULES ............ ") + fb_log("ARGS_TARGET = ${ARGS_TARGET}") + fb_log("ARGS_FB_DIR = ${ARGS_FB_DIR}") + fb_log("ARGS_FB_MODULES = ${ARGS_FB_MODULES}") + fb_log("ARG_SOURCE_GEN_DIR = ${ARGS_SOURCE_GEN_DIR}") + + if (NOT DEFINED ARGS_FB_DIR) + set(ARGS_FB_DIR "${FB_DEFAULT_ROOT_DIR}") + fb_log("using default FB_DIR = ${ARGS_FB_DIR}") + endif() + + if (NOT DEFINED ARGS_SOURCE_GEN_DIR) + set(ARGS_SOURCE_GEN_DIR "${FB_DEFAULT_SOURCE_GEN_DIR}/ts") + fb_log("using default SOURCE_GEN_DIR = ${ARGS_SOURCE_GEN_DIR}") + endif() + + # List accumulated in macros 'make_*_command' calls + set(ALL_GEN_FILES "") + + file(GLOB MOD_FB_CORE_FILES LIST_DIRECTORIES false ${ARGS_FB_DIR}/*.fbs) + foreach(FB_FILE ${MOD_FB_CORE_FILES}) + + make_ts_gen_command( + FB_FILE ${FB_FILE} + FB_MODULE ${FB_MODULE} + RESULT_DIR ${ARGS_SOURCE_GEN_DIR} + ) + + endforeach() + + foreach(FB_MODULE ${ARGS_FB_MODULES}) + + file(GLOB FB_MODULE_FILES LIST_DIRECTORIES false ${ARGS_FB_DIR}/${FB_MODULE}/*.fbs) + + foreach(FB_FILE ${FB_MODULE_FILES}) + + make_ts_gen_command( + FB_FILE ${FB_FILE} + FB_MODULE ${FB_MODULE} + RESULT_DIR ${ARGS_SOURCE_GEN_DIR} + ) + + endforeach() + + endforeach() + + add_custom_target(${ARGS_TARGET} DEPENDS ${ALL_GEN_FILES} + COMMENT "Generating Typescript code from Flatbuffers spec") + + set_property(TARGET ${ARGS_TARGET} + PROPERTY SOURCE_GEN_DIR + ${ARGS_SOURCE_GEN_DIR} + ) + +endfunction() + +# ======= Python Generation ============================================= + +# Python Flatbuffer generators +# Params: TARGET (REQUIRED), FB_DIR, SOURCE_GEN_DIR +# Properties available on generated TARGET: SOURCE_GEN_DIR +function(build_py_fb_modules) + + initialize_osf_fb_utils_defaults() + + set(PARSE_OPTIONS "") + set(PARSE_ONE_VALUE_ARGS + TARGET + FB_DIR + SOURCE_GEN_DIR + ) + set(PARSE_MULTI_VALUE_ARGS "") + cmake_parse_arguments(ARGS "${PARSE_OPTIONS}" "${PARSE_ONE_VALUE_ARGS}" + "${PARSE_MULTI_VALUE_ARGS}" ${ARGN} ) + fb_log("... BUILDING PYTHON FB MODULES ............ ") + fb_log("ARGS_TARGET = ${ARGS_TARGET}") + fb_log("ARGS_FB_DIR = ${ARGS_FB_DIR}") + fb_log("ARG_SOURCE_GEN_DIR = ${ARGS_SOURCE_GEN_DIR}") + + if (NOT DEFINED ARGS_FB_DIR) + set(ARGS_FB_DIR "${FB_DEFAULT_ROOT_DIR}") + fb_log("using default FB_DIR = ${ARGS_FB_DIR}") + endif() + + if (NOT DEFINED ARGS_SOURCE_GEN_DIR) + set(ARGS_SOURCE_GEN_DIR "${FB_DEFAULT_SOURCE_GEN_DIR}/cpp") + fb_log("using default SOURCE_GEN_DIR = ${ARGS_SOURCE_GEN_DIR}") + endif() + + file(GLOB_RECURSE ALL_FBS_FILES LIST_DIRECTORIES false ${ARGS_FB_DIR}/*.fbs) + + # Using random file names to allow multiproject use of this function + string(RANDOM LENGTH 4 PY_GEN_RANDOM) + set(PY_GEN_TIMESTAMP_FILE ${CMAKE_CURRENT_BINARY_DIR}/py_gen_${PY_GEN_RANDOM}.timestamp) + add_custom_command( + OUTPUT ${PY_GEN_TIMESTAMP_FILE} + DEPENDS ${ALL_FBS_FILES} + COMMAND ${FLATC} --python -o ${ARGS_SOURCE_GEN_DIR} ${ALL_FBS_FILES} + COMMAND ${CMAKE_COMMAND} -E touch ${PY_GEN_TIMESTAMP_FILE} + ) + + add_custom_target(${ARGS_TARGET} DEPENDS ${PY_GEN_TIMESTAMP_FILE} + COMMENT "Generating Python code from Flatbuffers spec") + + set_property(TARGET ${ARGS_TARGET} + PROPERTY SOURCE_GEN_DIR + ${ARGS_SOURCE_GEN_DIR} + ) + +endfunction() diff --git a/ouster_osf/fb/chunk.fbs b/ouster_osf/fb/chunk.fbs new file mode 100644 index 00000000..3d6a301d --- /dev/null +++ b/ouster_osf/fb/chunk.fbs @@ -0,0 +1,27 @@ +namespace ouster.osf.v2; + +// See 'msgs/*.fb' files for messages and corresponding stream specs + +table StampedMessage { + + // timestamp in nanoseconds + ts:uint64 (key); + + // 'id' should always refer to the existing Metadata.entries[] field + // that is then used to reconstruct the 'StampedMessage.buffer' field to + // the corresponding msg Flatbuffer object. + // (e.g metadata.entries[].id == StampedMessage.id) + id:uint32; + + // Flatbuffer encoded object that described in Metadata entry (by 'id') + buffer:[uint8]; +} + +table Chunk { + // List of messages in chunk (sorting by ts is not guaranteed) + messages:[StampedMessage]; +} + +file_identifier "OSF!"; +file_extension "osfc"; +root_type Chunk; diff --git a/ouster_osf/fb/header.fbs b/ouster_osf/fb/header.fbs new file mode 100644 index 00000000..3fca9871 --- /dev/null +++ b/ouster_osf/fb/header.fbs @@ -0,0 +1,37 @@ +namespace ouster.osf.v2; + + +enum HEADER_STATUS:uint8 { + UNKNOWN = 0, + + // INVALID is used during writing and generally means that + // file wasn't finished properly + INVALID, + + // VALID means that the OSF file is properly finished and + // reader might expect the `metadata_offset` pointing to valid metadata + // object + VALID +} + +table Header { + version:uint64; + status:HEADER_STATUS; + + // Offset to the Metadata object from the beginning of the file + metadata_offset:uint64 = 1; + + // Length of the file in bytes, should be used for validation + // that file was constructed and finished correctly + file_length:uint64 = 1; + + // metadata_offset and file_length defaults to 1 (and not 0), so + // we can write actual zero without forcing Flatbuffer to do so. + // Because when we use the default value the value is actually + // not inserted in the buffer and so it is smaller but we want + // it to have a stable byte size. +} + +file_identifier "OSF$"; +file_extension "osfh"; +root_type Header; \ No newline at end of file diff --git a/ouster_osf/fb/metadata.fbs b/ouster_osf/fb/metadata.fbs new file mode 100644 index 00000000..acd44a20 --- /dev/null +++ b/ouster_osf/fb/metadata.fbs @@ -0,0 +1,38 @@ +namespace ouster.osf.v2; + +struct ChunkOffset { + // lowest timestamp of a StampedMessage in a Chunk + start_ts:uint64; + // highest timestamp of a StampedMessage in a Chunk + end_ts:uint64; + // offset from the beginning of Chunks (the address following Header's CRC32) + offset:uint64; +} + +// Contains file information (streams metadata, file/session data, etc) +table MetadataEntry { + // used in references from other metadata entries + // and from StampedMessage.id + id:uint32 (key); + + // type identifier of the stream or metas, + // (e.g. 'ouster/os_sensor/LidarSensor') used to dispatch to the fb spec + // for parsing 'buffer' field. + type:string; + + // Flatbuffer encoded object of 'type' (see 'type' field description) + buffer:[uint8]; +} + +table Metadata { + id:string; + start_ts:uint64; + end_ts:uint64; + chunks:[ChunkOffset]; + entries:[MetadataEntry]; // required to be sorted and unique + // by MetadataEntry.id for fast lookups +} + +file_identifier "OSF#"; +file_extension "osfs"; +root_type Metadata; \ No newline at end of file diff --git a/ouster_osf/fb/os_sensor/extrinsics.fbs b/ouster_osf/fb/os_sensor/extrinsics.fbs new file mode 100644 index 00000000..5b17c894 --- /dev/null +++ b/ouster_osf/fb/os_sensor/extrinsics.fbs @@ -0,0 +1,14 @@ +namespace ouster.osf.v2; + +// ============ Extrinsics ======================================== + +table Extrinsics { + extrinsics:[double]; // vector of 16, row-major representaton of [4x4] + // matrix + ref_id:uint32; // reference metadata id of the object it's related to + name:string; // optional name of the extrinsics source or originator +} + +// MetadataEntry.type: ouster/v1/os_sensor/Extrinsics +root_type Extrinsics; +file_identifier "oExt"; \ No newline at end of file diff --git a/ouster_osf/fb/os_sensor/lidar_scan_stream.fbs b/ouster_osf/fb/os_sensor/lidar_scan_stream.fbs new file mode 100644 index 00000000..9e369986 --- /dev/null +++ b/ouster_osf/fb/os_sensor/lidar_scan_stream.fbs @@ -0,0 +1,88 @@ +namespace ouster.osf.v2; + +// sensor::ChanField enum mapping +enum CHAN_FIELD:uint8 { + UNKNOWN = 0, + RANGE = 1, + RANGE2 = 2, + SIGNAL = 3, + SIGNAL2 = 4, + REFLECTIVITY = 5, + REFLECTIVITY2 = 6, + NEAR_IR = 7, + FLAGS = 8, + FLAGS2 = 9, + RAW_HEADERS = 40, + RAW32_WORD5 = 45, + RAW32_WORD6 = 46, + RAW32_WORD7 = 47, + RAW32_WORD8 = 48, + RAW32_WORD9 = 49, + CUSTOM0 = 50, + CUSTOM1 = 51, + CUSTOM2 = 52, + CUSTOM3 = 53, + CUSTOM4 = 54, + CUSTOM5 = 55, + CUSTOM6 = 56, + CUSTOM7 = 57, + CUSTOM8 = 58, + CUSTOM9 = 59, + RAW32_WORD1 = 60, + RAW32_WORD2 = 61, + RAW32_WORD3 = 62, + 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]; +} + +// Single lidar field spec +struct ChannelField { + chan_field:CHAN_FIELD; + chan_field_type:CHAN_FIELD_TYPE; +} + +table LidarScanMsg { + // ====== LidarScan Channels ======================= + // encoded ChanField data + channels:[ChannelData]; + // corresponding ChanField description to what is contained in channels[] above + field_types:[ChannelField]; + + // ===== LidarScan Headers ========================= + header_timestamp:[uint64]; + header_measurement_id:[uint16]; + header_status:[uint32]; + frame_id:int32; + // pose vector of 4x4 matrices per every timestamp element (i.e. number of + // elements in a vector should be 16 x timestamp.size()), every 4x4 matrix + // has a col-major storage + pose:[double]; +} + +// Scan data from a lidar sensor. One scan is a sweep of a sensor (360 degree). +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. + // NOTE: For LidarScanMsg decoding field types from + // LidarScanMsg.field_types[] should be used. + field_types:[ChannelField]; +} + +// MetadataEntry.type: ouster/v1/os_sensor/LidarScanStream +root_type LidarScanStream; +file_identifier "oLSS"; \ No newline at end of file diff --git a/ouster_osf/fb/os_sensor/lidar_sensor.fbs b/ouster_osf/fb/os_sensor/lidar_sensor.fbs new file mode 100644 index 00000000..353c4e8f --- /dev/null +++ b/ouster_osf/fb/os_sensor/lidar_sensor.fbs @@ -0,0 +1,11 @@ +namespace ouster.osf.v2; + +// ============ Lidar Sensor ======================================== + +table LidarSensor { + metadata:string; +} + +// MetadataEntry.type: ouster/v1/os_sensor/LidarSensor +root_type LidarSensor; +file_identifier "oLS_"; \ No newline at end of file diff --git a/ouster_osf/fb/streaming/streaming_info.fbs b/ouster_osf/fb/streaming/streaming_info.fbs new file mode 100644 index 00000000..5d1c0798 --- /dev/null +++ b/ouster_osf/fb/streaming/streaming_info.fbs @@ -0,0 +1,41 @@ +namespace ouster.osf.v2; + +table StreamStats { + // refers to metadata.entries[id] that describes a stream + stream_id:uint32; + // host timestamp of the first message in `stream_id` stream + // in the whole OSF file + start_ts:uint64; + // host timestamp of the last message in `stream_id` stream + // in the whole OSF file + end_ts:uint64; + // total number of messages in a `stream_id` in the whole OSF file + message_count:uint64; + // avg size of the messages in bytes for a `stream_id` in the whole + // OSF file + message_avg_size:uint32; +} + +table ChunkInfo { + // offset of the chunk, matches the offset of `metadata.chunks[].offset` and + // serves as a key to address specific Chunk. (offsets always unique per OSF file) + offset:uint64; + // type of messages present in a chunk + stream_id:uint32; + // number of messages in a chunk + message_count:uint32; +} + +// If StreamingInfo is present in metadata it marks that chunks were stored in a +// particular way and the file can be readily used for streaming messages data for +// visualization (web-viz/Data-App etc.) with a guaranteed order by timestamp. + +table StreamingInfo { + // chunk information that describes the message/streams per chunk + chunks:[ChunkInfo]; + // stream statistics per stream for the whole OSF file + stream_stats:[StreamStats]; +} + +// MetadataEntry.type: ouster/v1/streaming/StreamingInfo +root_type StreamingInfo; \ No newline at end of file diff --git a/ouster_osf/include/ouster/osf/basics.h b/ouster_osf/include/ouster/osf/basics.h new file mode 100644 index 00000000..ea907f11 --- /dev/null +++ b/ouster_osf/include/ouster/osf/basics.h @@ -0,0 +1,161 @@ +/** + * Copyright (c) 2021, Ouster, Inc. + * All rights reserved. + * + * @file basics.h + * @brief basic functions for OSF + * + */ +#pragma once + +#include "ouster/lidar_scan.h" +#include "ouster/types.h" + +#include "chunk_generated.h" +#include "header_generated.h" +#include "metadata_generated.h" + +// OSF basic types for LidarSensor and LidarScan/Imu Streams +#include "os_sensor/lidar_scan_stream_generated.h" +#include "os_sensor/lidar_sensor_generated.h" + +namespace ouster { + +/** + * %OSF v2 space + */ +namespace osf { + +// current fb generated code in ouster::osf::gen +namespace gen { +using namespace v2; +} + +enum OSF_VERSION { + V_INVALID = 0, + V_1_0, // Original version of the OSF (2019/9/16) + V_1_1, // Add gps/imu/car trajectory to the OSF (2019/11/14) + V_1_2, // Change gps_waypoint type to Table in order to + // support Python language generator + V_1_3, // Add extension for Message in osfChunk + // and for Session in osfSession (2020/03/18) + V_1_4, // Gen2/128 support (2020/08/11) + + V_2_0 = 20 // Second Generation OSF v2 +}; + +/// Chunking strategies. Refer to RFC0018 for more details. +enum ChunksLayout { + LAYOUT_STANDARD = 0, ///< not used currently + LAYOUT_STREAMING = 1 ///< default layout (the only one for a user) +}; + +std::string to_string(ChunksLayout chunks_layout); +ChunksLayout chunks_layout_of_string(const std::string& s); + +// stable common types mapped to ouster::osf +using v2::HEADER_STATUS; + +/** Common timestamp for all time in ouster::osf */ +using ts_t = std::chrono::nanoseconds; + +/** + * Standard Flatbuffers prefix size + * @todo [pb]: Rename this beast? + */ +static constexpr uint32_t FLATBUFFERS_PREFIX_LENGTH = 4; + +/** Return string representation of header */ +std::string to_string(const HEADER_STATUS status); + +/** Debug method to get hex buf values in string */ +std::string to_string(const uint8_t* buf, const size_t count, + const size_t max_show_count = 0); + +/// Open read test file to a string. +std::string read_text_file(const std::string& filename); + +/** + * Reads the prefix size of the Flatbuffers buffer. First 4 bytes. + * @param buf pointer to Flatbuffers buffer stared with prefixed size + * @return the size recovered from the stored prefix size + */ +uint32_t get_prefixed_size(const uint8_t* buf); + +/** + * Calculates the full size of the block (prefixed_size + size + CRC32). + * @param buf pointer to Flatbuffers buffer stared with prefixed size + * @return the calculated size of the block + */ +uint32_t get_block_size(const uint8_t* buf); + +/** + * Check the prefixed size buffer CRC32 fields. + * + * @param buf is structured as size prefixed Flatbuffer buffer, i.e. first + * 4 bytes is the size of the buffer (excluding 4 bytes of the size), + * and the 4 bytes that follows right after the 4 + + * is the CRC32 bytes. + * @param max_size total number of bytes that can be accessed in the buffer, + * as a safety precaution if buffer is not well formed, or if + * first prefixed size bytes are broken. + * @return true if CRC field is correct, false otherwise + * + */ +bool check_prefixed_size_block_crc( + const uint8_t* buf, + const uint32_t max_size = std::numeric_limits::max()); + +/** + * Makes the closure to batch lidar_packets and emit LidarScan object. + * Result returned through callback handler(ts, LidarScan). + * LidarScan uses user modified field types + */ +template +std::function make_build_ls( + const ouster::sensor::sensor_info& info, + const LidarScanFieldTypes& ls_field_types, ResultHandler&& handler) { + const auto w = info.format.columns_per_frame; + const auto h = info.format.pixels_per_column; + + std::shared_ptr ls(nullptr); + if (ls_field_types.empty()) { + auto default_ls_field_types = get_field_types(info); + ls = std::make_shared(w, h, default_ls_field_types.begin(), + default_ls_field_types.end()); + + } else { + ls = std::make_shared(w, h, ls_field_types.begin(), + 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; + } + }; +} + +/** + * The above make_build_ls() function overload. In this function, LidarScan + * uses default field types by the profile + */ +template +std::function make_build_ls( + const ouster::sensor::sensor_info& info, ResultHandler&& handler) { + return make_build_ls(info, {}, handler); +} + +} // namespace osf +} // namespace ouster diff --git a/ouster_osf/include/ouster/osf/crc32.h b/ouster_osf/include/ouster/osf/crc32.h new file mode 100644 index 00000000..1209da72 --- /dev/null +++ b/ouster_osf/include/ouster/osf/crc32.h @@ -0,0 +1,39 @@ +/** + * Copyright (c) 2021, Ouster, Inc. + * All rights reserved. + * + * @file crc32.h + * @brief crc32 utility + * + */ +#pragma once + +#include +#include + +namespace ouster { +namespace osf { + +/// Size of the CRC field in a buffer +const uint32_t CRC_BYTES_SIZE = 4; + +/** + * Caclulate CRC value for the buffer of given size. (ZLIB version) + * @param buf pointer to the data buffer + * @param size size of the buffer in bytes + * @return CRC32 value + */ +uint32_t crc32(const uint8_t* buf, uint32_t size); + +/** + * Caclulate and append CRC value for the buffer of given size and append + * it to the initial crc value. (ZLIB version) + * @param initial_crc initial crc value to append to + * @param buf pointer to the data buffer + * @param size size of the buffer in bytes + * @return CRC32 value + */ +uint32_t crc32(uint32_t initial_crc, const uint8_t* buf, uint32_t size); + +} // namespace osf +} // namespace ouster \ No newline at end of file diff --git a/ouster_osf/include/ouster/osf/file.h b/ouster_osf/include/ouster/osf/file.h new file mode 100644 index 00000000..5bac0080 --- /dev/null +++ b/ouster_osf/include/ouster/osf/file.h @@ -0,0 +1,157 @@ +/** + * Copyright (c) 2021, Ouster, Inc. + * All rights reserved. + * + * @file file.h + * @brief common OSF file resource for Reader and Writer operations + * + */ +#pragma once + +#include +#include + +#include "ouster/osf/basics.h" + +namespace ouster { +namespace osf { + +enum class OpenMode : uint8_t { READ = 0, WRITE = 1 }; + +/** State of %OSF file */ +enum class FileState : uint8_t { GOOD = 0, BAD = 1 }; + +/** Chunk buffer type to store raw byte buffers of data. */ +using ChunkBuffer = std::vector; + +/** + * Interface to abstract the way of how we handle file system read/write + * operations. + */ +class OsfFile { + public: + explicit OsfFile(); + + /** + * Opens the file. + * @note Only OpenMode::READ is supported + */ + explicit OsfFile(const std::string& filename, + OpenMode mode = OpenMode::READ); + ~OsfFile(); + + // Header Info + uint64_t size() const { return size_; }; + std::string filename() const { return filename_; } + OSF_VERSION version(); + uint64_t metadata_offset(); + uint64_t chunks_offset(); + + /** Checks the validity of header and session/file_info blocks. */ + bool valid(); + + /** + * Get the goodness of the file. + * @todo Need to have more states here (eod, valid, error, etc) + */ + bool good() const { return state_ == FileState::GOOD; } + + // Convenience operators + bool operator!() const { return !good(); }; + explicit operator bool() const { return good(); }; + + /** + * Sequential access to the file. + * This is mimicking the regular file access with the offset + */ + uint64_t offset() const { return offset_; } + + /** + * File seek (in mmap mode it's just moving the offset_ pointer + * without any file system opeations.) + * @param pos position in the file + */ + OsfFile& seek(const uint64_t pos); + + /** + * Read from file (in current mmap mode it's copying data from + * mmap address to the 'buf' address). + * + * @todo Handle errors in future and get the way to read them back + * with FileState etc. + */ + OsfFile& read(uint8_t* buf, const uint64_t count); + + bool is_memory_mapped() const; + + /** + * Mmap access to the file content with the specified offset from the + * beginning of the file. + */ + const uint8_t* buf(const uint64_t offset = 0) const; + + /** + * Clears file handle and allocated resources. In current mmap + * implementation it's unmapping memory and essentially invalidates the + * memory addresses that might be captured within MessageRefs + * and Reader. + * @sa ouster::osf::MessageRef, ouster::osf::Reader + */ + void close(); + + /** Debug helper method to dump OsfFile state to a string. */ + std::string to_string(); + + // Copy policy + // Don't allow the copying of the file handler + OsfFile(const OsfFile&) = delete; + OsfFile& operator=(const OsfFile&) = delete; + + // Move policy + // But it's ok to move with the ownership transfer of the underlying file + // handler (mmap). + OsfFile(OsfFile&& other); + OsfFile& operator=(OsfFile&& other); + + std::shared_ptr read_chunk(uint64_t offset); + + uint8_t* get_header_chunk_ptr(); + uint8_t* get_metadata_chunk_ptr(); + + private: + // Convenience method to set error and print it's content. + // TODO[pb]: Adding more error states will probably extend the set of this + // function. + void error(const std::string& msg = std::string()); + + // Opened filename as it was passed in contructor. + std::string filename_; + + // Current offset to the file. (not used in mmaped implementation) but used + // for copying(reading) blocks of memory from the file to the specified + // memory. + uint64_t offset_; + + // Size of the opened file in bytes + uint64_t size_; + + // Mmaped memory address pointed to the beginning of the file (byte 0) + uint8_t* file_buf_; + + // File reading access + std::ifstream file_stream_; + std::shared_ptr header_chunk_; + std::shared_ptr metadata_chunk_; + + // Last read chunk cached, to save the double read on the sequence of verify + // and then read iterator access (used only when compiled with + // OUSTER_OSF_NO_MMAP, and in mmap version we rely on the OS/kernel caching) + std::shared_ptr chunk_cache_; + uint64_t chunk_cache_offset_; + + // Internal state + FileState state_; +}; + +} // namespace osf +} // namespace ouster \ No newline at end of file diff --git a/ouster_osf/include/ouster/osf/layout_standard.h b/ouster_osf/include/ouster/osf/layout_standard.h new file mode 100644 index 00000000..403279f9 --- /dev/null +++ b/ouster_osf/include/ouster/osf/layout_standard.h @@ -0,0 +1,47 @@ +/** + * Copyright (c) 2021, Ouster, Inc. + * All rights reserved. + * + * @file layout_standard.h + * @brief OSF Standard Layout strategy. + * + */ +#pragma once + +#include "ouster/osf/writer.h" + +namespace ouster { +namespace osf { + +constexpr uint32_t STANDARD_DEFAULT_CHUNK_SIZE = 5 * 1024 * 1024; // not strict ... + +/** + * Standard Layout chunking strategy + * + * When messages laid out into chunks in the order as they come and not + * exceeding `chunk_size` (if possible). However if a single + * message size is bigger than specified `chunk_size` it's still recorded. + */ +class StandardLayoutCW : public ChunksWriter { + public: + StandardLayoutCW(Writer& writer, + uint32_t chunk_size = STANDARD_DEFAULT_CHUNK_SIZE); + void saveMessage(const uint32_t stream_id, const ts_t ts, + const std::vector& msg_buf) override; + + void finish() override; + + uint32_t chunk_size() const override { return chunk_size_; } + + private: + void finish_chunk(); + + const uint32_t chunk_size_; + ChunkBuilder chunk_builder_{}; + + Writer& writer_; + +}; + +} // namespace osf +} // namespace ouster \ No newline at end of file diff --git a/ouster_osf/include/ouster/osf/layout_streaming.h b/ouster_osf/include/ouster/osf/layout_streaming.h new file mode 100644 index 00000000..c2f70926 --- /dev/null +++ b/ouster_osf/include/ouster/osf/layout_streaming.h @@ -0,0 +1,58 @@ +/** + * Copyright (c) 2021, Ouster, Inc. + * All rights reserved. + * + * @file layout_streaming.h + * @brief OSF Streaming Layout + * + */ +#pragma once + +#include "ouster/osf/writer.h" + +#include "ouster/osf/meta_streaming_info.h" + +namespace ouster { +namespace osf { + +constexpr uint32_t STREAMING_DEFAULT_CHUNK_SIZE = 2 * 1024 * 1024; // not strict ... + +/** + * Sreaming Layout chunking strategy + * + * TODO[pb]: sorting TBD as in RFC0018 but first pass should be good enough + * because usually messages from the same source and of the same type comes + * sorted from pcap/bag sources. + * + * When messages laid out into chunks in an ordered as they come (not full + * RFC0018 compliant, see TODO below), with every chunk holding messages + * exclusively of a single stream_id. Tries not to exceede `chunk_size` (if + * possible). However if a single message size is bigger than specified + * `chunk_size` it's still recorded. + */ +class StreamingLayoutCW : public ChunksWriter { + public: + StreamingLayoutCW(Writer& writer, + uint32_t chunk_size = STREAMING_DEFAULT_CHUNK_SIZE); + void saveMessage(const uint32_t stream_id, const ts_t ts, + const std::vector& msg_buf) override; + + void finish() override; + + uint32_t chunk_size() const override { return chunk_size_; } + + private: + void stats_message(const uint32_t stream_id, const ts_t ts, + const std::vector& msg_buf); + void finish_chunk(uint32_t stream_id, + const std::shared_ptr& chunk_builder); + + const uint32_t chunk_size_; + std::map> chunk_builders_{}; + std::vector> chunk_stream_id_{}; + std::map stream_stats_{}; + Writer& writer_; +}; + +} // namespace osf +} // namespace ouster \ No newline at end of file diff --git a/ouster_osf/include/ouster/osf/meta_extrinsics.h b/ouster_osf/include/ouster/osf/meta_extrinsics.h new file mode 100644 index 00000000..8c082f56 --- /dev/null +++ b/ouster_osf/include/ouster/osf/meta_extrinsics.h @@ -0,0 +1,72 @@ +/** + * Copyright (c) 2021, Ouster, Inc. + * All rights reserved. + * + * @file meta_extrinsics.h + * @brief Metadata entry Extrinsics + * + */ +#pragma once + +#include +#include + +#include "ouster/types.h" +#include "ouster/osf/metadata.h" + +namespace ouster { +namespace osf { + +/** + * Metadata entry to store sensor Extrinsics. + * + * @verbatim + * Fields: + * extrinsics: mat4d - 4x4 homogeneous transform + * ref_meta_id: uint32_t - reference to other metadata entry, typically + * LidarSensor + * name: string - named id if needed, to support multiple extrinsics per + * object (i.e. LidarSensor, or Gps) with name maybe used + * to associate extrinsics to some external system of + * records or just name the source originator of the + * extrinsics information. + * + * OSF type: + * ouster/v1/os_sensor/Extrinsics + * + * Flatbuffer definition file: + * fb/os_sensor/extrinsics.fbs + * @endverbatim + * + */ +class Extrinsics : public MetadataEntryHelper { + public: + explicit Extrinsics(const mat4d& extrinsics, uint32_t ref_meta_id = 0, + const std::string& name = "") + : extrinsics_(extrinsics), ref_meta_id_{ref_meta_id}, name_{name} {} + const mat4d& extrinsics() const { return extrinsics_; } + const std::string& name() const { return name_; } + uint32_t ref_meta_id() const { return ref_meta_id_; } + + std::vector buffer() const final; + + static std::unique_ptr from_buffer( + const std::vector& buf); + + std::string repr() const override; + + private: + mat4d extrinsics_; + uint32_t ref_meta_id_; + std::string name_; +}; + +template <> +struct MetadataTraits { + static const std::string type() { + return "ouster/v1/os_sensor/Extrinsics"; + } +}; + +} // namespace osf +} // namespace ouster \ No newline at end of file diff --git a/ouster_osf/include/ouster/osf/meta_lidar_sensor.h b/ouster_osf/include/ouster/osf/meta_lidar_sensor.h new file mode 100644 index 00000000..b029b787 --- /dev/null +++ b/ouster_osf/include/ouster/osf/meta_lidar_sensor.h @@ -0,0 +1,88 @@ +/** + * Copyright (c) 2021, Ouster, Inc. + * All rights reserved. + * + * @file meta_lidar_sensor.h + * @brief Metadata entry LidarSensor + * + */ +#pragma once + +#include +#include + +#include "ouster/types.h" +#include "ouster/osf/metadata.h" + +namespace ouster { +namespace osf { + +/** + * Metadata entry to store lidar sensor_info, i.e. Ouster sensor configuration. + * + * @verbatim + * Fields: + * metadata: string - lidar metadata in json + * + * OSF type: + * ouster/v1/os_sensor/LidarSensor + * + * Flatbuffer definition file: + * fb/os_sensor/lidar_sensor.fbs + * @endverbatim + * + */ +class LidarSensor : public MetadataEntryHelper { + using sensor_info = ouster::sensor::sensor_info; + public: + + /// TODO]pb]: This is soft DEPRECATED until we have an updated sensor_info, + /// since we are not encouraging storing the serialized metadata + explicit LidarSensor(const sensor_info& si) + : sensor_info_(si), metadata_(sensor::to_string(si)) { + throw std::invalid_argument( + "\nERROR: `osf::LidarSensor()` constructor accepts only metadata_json " + "(full string of the file metadata.json or what was received from " + "sensor) and not a `sensor::sensor_info` object.\n\n" + "We are so sorry that we deprecated it's so hardly but the thing is " + "that `sensor::sensor_info` object doesn't equal the original " + "metadata.json file (or string) that we used to construct it.\n" + "However, Data App when tries to get metadata from OSF looks for " + "fields (like `image_rev`) that only present in metadata.json but " + "not `sensor::sensor_info` which effectively leads to OSF that " + "couldn't be uploaded to Data App.\n"); + } + + explicit LidarSensor(const std::string& sensor_metadata) + : sensor_info_(sensor::parse_metadata(sensor_metadata)), + metadata_(sensor_metadata) {} + + const sensor_info& info() const { return sensor_info_; } + + const std::string& metadata() const { return metadata_; } + + // === Simplified with MetadataEntryHelper: type()+clone() + // std::string type() const override; + // std::unique_ptr clone() const override; + + std::vector buffer() const final; + + static std::unique_ptr from_buffer( + const std::vector& buf); + + std::string repr() const override; + + private: + sensor_info sensor_info_; + const std::string metadata_; +}; + +template <> +struct MetadataTraits { + static const std::string type() { + return "ouster/v1/os_sensor/LidarSensor"; + } +}; + +} // namespace osf +} // namespace ouster \ No newline at end of file diff --git a/ouster_osf/include/ouster/osf/meta_streaming_info.h b/ouster_osf/include/ouster/osf/meta_streaming_info.h new file mode 100644 index 00000000..2063ad7a --- /dev/null +++ b/ouster_osf/include/ouster/osf/meta_streaming_info.h @@ -0,0 +1,101 @@ +/** + * Copyright (c) 2021, Ouster, Inc. + * All rights reserved. + * + * @file meta_streaming_info.h + * @brief Metadata entry StreamingInfo + * + */ +#pragma once + +#include +#include + +#include "ouster/types.h" +#include "ouster/osf/metadata.h" + +namespace ouster { +namespace osf { + +struct ChunkInfo { + uint64_t offset; + uint32_t stream_id; + uint32_t message_count; +}; + +struct StreamStats { + uint32_t stream_id; + ts_t start_ts; + ts_t end_ts; + uint64_t message_count; + uint32_t message_avg_size; + StreamStats() = default; + StreamStats(uint32_t s_id, ts_t t, uint32_t msg_size) + : stream_id{s_id}, + start_ts{t}, + end_ts{t}, + message_count{1}, + message_avg_size{msg_size} {}; + void update(ts_t t, uint32_t msg_size) { + if (start_ts > t) start_ts = t; + if (end_ts < t) end_ts = t; + ++message_count; + int avg_size = static_cast(message_avg_size); + avg_size = avg_size + (static_cast(msg_size) - avg_size) / + static_cast(message_count); + message_avg_size = static_cast(avg_size); + } +}; + +std::string to_string(const ChunkInfo& chunk_info); +std::string to_string(const StreamStats& stream_stats); + +/** + * Metadata entry to store StreamingInfo, to support StreamingLayout (RFC 0018) + * + * @verbatim + * Fields: + * chunks_info: chunk -> stream_id map + * stream_stats: stream statistics of messages in file + * + * OSF type: + * ouster/v1/streaming/StreamingInfo + * + * Flatbuffer definition file: + * fb/streaming/streaming_info.fbs + * @endverbatim + * + */ +class StreamingInfo : public MetadataEntryHelper { + public: + + StreamingInfo() {} + + StreamingInfo( + const std::vector>& chunks_info, + const std::vector>& stream_stats) + : chunks_info_{chunks_info.begin(), chunks_info.end()}, + stream_stats_{stream_stats.begin(), stream_stats.end()} {} + + std::map& chunks_info() { return chunks_info_; } + std::map& stream_stats() { return stream_stats_; } + + std::vector buffer() const override final; + static std::unique_ptr from_buffer( + const std::vector& buf); + std::string repr() const override; + + private: + std::map chunks_info_{}; + std::map stream_stats_{}; +}; + +template <> +struct MetadataTraits { + static const std::string type() { + return "ouster/v1/streaming/StreamingInfo"; + } +}; + +} // namespace osf +} // namespace ouster \ No newline at end of file diff --git a/ouster_osf/include/ouster/osf/metadata.h b/ouster_osf/include/ouster/osf/metadata.h new file mode 100644 index 00000000..2082a169 --- /dev/null +++ b/ouster_osf/include/ouster/osf/metadata.h @@ -0,0 +1,432 @@ +/** + * Copyright (c) 2021, Ouster, Inc. + * All rights reserved. + * + * @file metadata.h + * @brief Core MetadataEntry class with meta store, registry etc. + * + */ +#pragma once + +#include +#include +#include +#include +#include +#include + +#include "flatbuffers/flatbuffers.h" +#include "ouster/osf/basics.h" + +namespace ouster { +namespace osf { + +/** + * Need to be specialized for every derived MetadataEntry class that can be + * stored/recovered as metadata object. + * @sa metadata_type(), MetadataEntry + */ +template +struct MetadataTraits { + static const std::string type() { return nullptr; } +}; + +/** + * Helper function that returns the MetadataEntry type of concrete metadata. + */ +template +inline const std::string metadata_type() { + typedef typename std::remove_const::type no_const_type; + typedef typename std::remove_reference::type no_cvref_type; + typedef typename std::remove_pointer::type almost_pure_type; + typedef typename std::remove_const::type pure_type_M; + return MetadataTraits::type(); +} + +/** + * Base abstract metadata entry type for every metadata that can be stored as + * OSF metadata. + * + * Metadata object that is stored/serialized to OSF is a triplet: + * `{id, type, buffer}` + * + * `id` - is a unique identifier per OSF file and used for references from other + * metadata objects or from messages (chunk.StampedMessage.id in chunk.fbs) + * to link messages with the streams. + * + * `type` - string that is unique per OSF generation (i.e. v2) and used to link + * datum buffer representation to the concrete metadata object. + * + * Type is specified when concrete metadata type class defined via + * MetadataTraits struct specialization, example: + * + * @code{.cpp} + * template <> + * struct MetadataTraits { + * static const std::string type() { + * return "ouster/v1/something/MyMeta"; + * } + * }; + * @endcode + * + * `buffer` - byte representation of the metadata content whatever it is defined + * by concrete metadata type. Every metadata object should have a recipe how + * to serialize itself to the bytes buffer by overwriting the buffer() function. + * And the recipe how to recover itserf by providing static + * from_buffer(buf, type) function. + * + */ +class MetadataEntry { + public: + /** + * Function type to recover metadata object from buffer. + */ + using from_buffer_func = + std::unique_ptr (*)(const std::vector&); + + /** + * Type of the metadata, used to identify the object type in serialized OSF + * and as key in deserialization registry + */ + virtual std::string type() const = 0; + + /** + * Same as type with the difference that type() can be dynamic and + * static_type() should always be defined in compile time. + * NOTE: Introduced as a convenience/(HACK?) to simpler reconstruct and cast + * dynamic objects from MetadataEntryRef + */ + virtual std::string static_type() const = 0; + + /** + * Should be provided by derived class and is used in handling polymorphic + * objects and avoid object slicing + */ + virtual std::unique_ptr clone() const = 0; + + /** + * byte represantation of the internal derived metadata type, used as + * serialization function when saving to OSF file + */ + virtual std::vector buffer() const = 0; + + /** + * recover metadata object from the bytes representation if possible. + * If recovery is not possible returns nullptr + */ + static std::unique_ptr from_buffer( + const std::vector& buf, const std::string type_str); + + /** + * string representation of the internal metadata object, used in + * to_string() for debug/info outputs. + */ + virtual std::string repr() const; + + /** + * string representation of the whole metadata entry with type and id + */ + virtual std::string to_string() const; + + void setId(uint32_t id) { id_ = id; } + uint32_t id() const { return id_; } + + /** + * Casting of the base class to concrete derived metadata entry type. + * Always creates new object with allocation via clone() if the pointer/ref + * is a polymorphic object, or as reconstruction from buffer() + * representation when it used from MetadataEntryRef (i.e. wrapper on + * underlying bytes) + */ + template + std::unique_ptr as() const { + if (type() == metadata_type()) { + std::unique_ptr m; + if (type() == static_type()) { + m = clone(); + } else { + m = T::from_buffer(buffer()); + } + if (m != nullptr) { + m->setId(id()); + // NOTE: Little bit crazy unique_ptr cast (not absolutely + // correct because of no deleter handled). But works + // for our case because we don't mess with it. + return std::unique_ptr(dynamic_cast(m.release())); + } + } + return nullptr; + } + + /** + * Implementation details that emits buffer() content as proper + * Flatbuffer MetadataEntry object. + */ + flatbuffers::Offset make_entry( + flatbuffers::FlatBufferBuilder& fbb) const; + + /** + * Registry that holds from_buffer function by type string and used + * during deserialization. + */ + static std::map& get_registry(); + + virtual ~MetadataEntry() = default; + + private: + // id as its stored in metadata OSF and used for linking between other + // metadata object and messages to streams + uint32_t id_{0}; +}; + +/** + * Safe and convenient cast of shared_ptr to concrete derived + * class using either shortcut (dynamic_pointer_cast) when it's save to do so + * or reconstructs a new copy of the object from underlying data. + */ +template +std::shared_ptr metadata_pointer_as( + const std::shared_ptr& m) { + if (m->type() != metadata_type()) return nullptr; + if (m->type() == m->static_type()) { + return std::dynamic_pointer_cast(m); + } else { + return m->template as(); + } +}; + +/** + * Registrar class helper to add static from_buffer() function of the concrete + * derived metadata class to the registry. + * + */ +template +struct RegisterMetadata { + virtual ~RegisterMetadata() { + if (!registered_) { + std::cerr << "ERROR: Can't be right! We shouldn't be here. " + "Duplicate metadata types?" + << std::endl; + std::abort(); + } + } + static bool register_type_decoder() { + 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; + return false; + } + registry.insert(std::make_pair(type, MetadataDerived::from_buffer)); + return true; + } + static const bool registered_; +}; +template +const bool RegisterMetadata::registered_ = + RegisterMetadata::register_type_decoder(); + +/** + * Helper class used as base class for concrete derived metadata types + * and provides type()/static_type()/clone() functions as boilerplate. + * + * Also registers the from_buffer() function for deserializer registry via + * RegisterMetadata helper trick. + * + */ +template +class MetadataEntryHelper : public MetadataEntry, + RegisterMetadata { + public: + std::string type() const override { + return metadata_type(); + } + std::string static_type() const override { + return metadata_type(); + } + std::unique_ptr clone() const override { + return std::make_unique( + *dynamic_cast(this)); + } +}; + +/** + * MetadataEntry wrapper for byte Flatbuffers bytes representation. Used during + * deserialization and acts as regular polymorphic metadata type (almost). + * + * Doesn't own the memory of the underlying buffer. + * + * Reconstructs itself to the concrete metadata type with: + * + * as_type() - using the stored type() to recofer deserialization function + * + * as() OR metadata_pointer_as() - using the + * specified derived metadata class. + */ +class MetadataEntryRef : public MetadataEntry { + public: + /** + * Creates the metadata reference from Flatbuffers v2::MetadataEntry buffer. + * No copy involved. + */ + explicit MetadataEntryRef(const uint8_t* buf) : buf_{buf} { + const gen::MetadataEntry* meta_entry = + reinterpret_cast(buf_); + buf_type_ = meta_entry->type()->str(); + setId(meta_entry->id()); + } + + std::string type() const override { return buf_type_; } + std::string static_type() const override { + return metadata_type(); + } + + std::unique_ptr clone() const override { + return std::make_unique(*this); + } + + std::vector buffer() const final; + + /** + * Reconstructs the object as concrete metadata of type() from the + * buffer() using registered deserialization function from_buffer() of + * current type + */ + std::unique_ptr as_type() const; + + private: + void setId(uint32_t id) { MetadataEntry::setId(id); } + const uint8_t* buf_; + std::string buf_type_{}; +}; + +/** + * Implementation detail for MetadataEntryRef to distinguish it from any + * possible metadata type + */ +template <> +struct MetadataTraits { + static const std::string type() { return "impl/MetadataEntryRef"; } +}; + +/** + * Collection of metadata entries, used as metadata provider in Reader and + * Writer. + * + * Provide functions to retrieve concrete metadata types by id or by type. + * + * Also can serialize itself to Flatbuffers collection of metadata. + * + */ +class MetadataStore { + using MetadataEntriesMap = + std::map>; + + public: + using key_type = MetadataEntriesMap::key_type; + + uint32_t add(MetadataEntry&& entry) { return add(entry); } + + uint32_t add(MetadataEntry& entry) { + if (entry.id() == 0) { + /// @todo [pb]: Figure out the whole sequence of ids in addMetas in + /// 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; + return entry.id(); + } else if (next_meta_id_ == entry.id()) { + // Find next available next_meta_id_ so we avoid id collisions + ++next_meta_id_; + auto next_it = metadata_entries_.lower_bound(next_meta_id_); + while (next_it != metadata_entries_.end() && + next_it->first == next_meta_id_) { + ++next_meta_id_; + next_it = metadata_entries_.lower_bound(next_meta_id_); + } + } + + metadata_entries_.emplace(entry.id(), entry.clone()); + return entry.id(); + } + + template + std::shared_ptr get() const { + auto it = metadata_entries_.begin(); + while (it != metadata_entries_.end()) { + if (auto m = metadata_pointer_as(it->second)) { + return m; + } + ++it; + } + return nullptr; + } + + template + size_t count() const { + auto it = metadata_entries_.begin(); + size_t cnt = 0; + while (it != metadata_entries_.end()) { + if (it->second->type() == metadata_type()) + ++cnt; + ++it; + } + return cnt; + } + + template + std::shared_ptr get(const uint32_t metadata_id) const { + auto meta_entry = get(metadata_id); + return metadata_pointer_as(meta_entry); + } + + std::shared_ptr get(const uint32_t metadata_id) const { + auto it = metadata_entries_.find(metadata_id); + if (it == metadata_entries_.end()) return nullptr; + return it->second; + } + + template + std::map> find() const { + std::map> res; + auto it = metadata_entries_.begin(); + while (it != metadata_entries_.end()) { + if (auto m = metadata_pointer_as(it->second)) { + res.insert(std::make_pair(it->first, m)); + } + ++it; + } + return res; + } + + size_t size() const { return metadata_entries_.size(); } + + const MetadataEntriesMap& entries() const { return metadata_entries_; } + + std::vector> + make_entries(flatbuffers::FlatBufferBuilder& fbb) const; + + private: + void assignId(MetadataEntry& entry) { entry.setId(next_meta_id_++); } + + uint32_t next_meta_id_{1}; + MetadataEntriesMap metadata_entries_{}; +}; + +/** + * Tag helper for Stream types that need to bind (link) together message + * ObjectType and the corresponding metadata entry (StreamMeta) that form + * together the stream definition. + */ +template +struct MessageStream { + using obj_type = ObjectType; + using meta_type = StreamMeta; +}; + +} // namespace osf +} // namespace ouster diff --git a/ouster_osf/include/ouster/osf/operations.h b/ouster_osf/include/ouster/osf/operations.h new file mode 100644 index 00000000..6f5b4821 --- /dev/null +++ b/ouster_osf/include/ouster/osf/operations.h @@ -0,0 +1,48 @@ +/** + * Copyright (c) 2021, Ouster, Inc. + * All rights reserved. + * + * @file operations.h + * @brief High level OSF operations + * + */ +#pragma once + +#include + +#include "ouster/osf/basics.h" + +namespace ouster { +namespace osf { + +/** + * Outputs OSF v2 metadata + header info in JSON format. + * + * @param file OSF file (only v2 supported) + * @param full flag print full information (i.e. chunks_offset and decoded + * metas) + * @return JSON formatted string of the OSF metadata + header + * + */ +std::string dump_metadata(const std::string& file, bool full = true); + +/** + * Reads OSF file and prints (STDOUT) messages types, timestamps and + * overall statistics per message type. + * + * @param file OSF file + * @param with_decoding decode known messages (used to time a + * reading + decoding together) + * + */ +void parse_and_print(const std::string& file, bool with_decoding = false); + +/** + * Convert pcap with a single sensor stream to OSF. + */ +bool pcap_to_osf(const std::string& pcap_filename, + const std::string& meta_filename, int lidar_port, + const std::string& osf_filename, int chunk_size = 0); + +} // namespace osf +} // namespace ouster diff --git a/ouster_osf/include/ouster/osf/pcap_source.h b/ouster_osf/include/ouster/osf/pcap_source.h new file mode 100644 index 00000000..0bd2e7f3 --- /dev/null +++ b/ouster_osf/include/ouster/osf/pcap_source.h @@ -0,0 +1,100 @@ +/** + * 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 + using LidarDataHandler = + std::function; + + /// General pcap packet handler + using PacketHandler = std::function; + + // Predicate to control the bag run loop + using PacketInfoPredicate = + std::function; + + /** + * Opens pcap file and checks available packets inside with + * heuristics applied to guess Ouster lidar port with data. + */ + 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 + */ + void addLidarDataHandler(int dst_port, + const ouster::sensor::sensor_info& info, + LidarDataHandler&& lidar_handler); + + /** + * The addLidarDataHandler() function overload. In this function, LidarScan + * uses user modified field types + */ + 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. + */ + void runWhile(const PacketInfoPredicate& pred); + + /** + * Close pcap file + */ + ~PcapRawSource(); + + private: + PcapRawSource(const PcapRawSource&) = delete; + PcapRawSource& operator=(const PcapRawSource&) = delete; + + void handleCurrentPacket(const sensor_utils::packet_info& pinfo); + + std::string pcap_filename_; + ouster::sensor::sensor_info info_; + std::shared_ptr pcap_handle_{ + nullptr}; + std::map packet_handlers_{}; +}; + +} // namespace osf +} // namespace ouster diff --git a/ouster_osf/include/ouster/osf/reader.h b/ouster_osf/include/ouster/osf/reader.h new file mode 100644 index 00000000..2a31e436 --- /dev/null +++ b/ouster_osf/include/ouster/osf/reader.h @@ -0,0 +1,558 @@ +/** + * Copyright (c) 2021, Ouster, Inc. + * All rights reserved. + * + * @file reader.h + * @brief OSF file Reader + * + */ +#pragma once + +#include +#include + +#include "ouster/osf/file.h" +#include "ouster/osf/metadata.h" +#include "ouster/types.h" + +namespace ouster { +namespace osf { + +enum class ChunkValidity { UNKNOWN = 0, VALID, INVALID }; + +/** + * Chunks state. Validity info and next offset for forward iteration. + */ +struct ChunkState { + uint64_t offset; + uint64_t next_offset; + ts_t start_ts; + ts_t end_ts; + ChunkValidity status; +}; + +struct ChunkInfoNode { + uint64_t offset; + uint64_t next_offset; + uint32_t stream_id; + uint32_t message_count; + uint32_t message_start_idx; +}; + +/** + * Chunks state map. Validity info and next offset. + */ +class ChunksPile { + public: + using ChunkStateIter = std::unordered_map::iterator; + using ChunkInfoIter = std::unordered_map::iterator; + using StreamChunksMap = + std::unordered_map>>; + + ChunksPile(){}; + + void add(uint64_t offset, ts_t start_ts, ts_t end_ts); + ChunkState* get(uint64_t offset); + void add_info(uint64_t offset, uint32_t stream_id, uint32_t message_count); + ChunkInfoNode* get_info(uint64_t offset); + ChunkInfoNode* get_info_by_message_idx(uint32_t stream_id, + uint32_t message_idx); + ChunkState* get_by_lower_bound_ts(uint32_t stream_id, const ts_t ts); + ChunkState* next(uint64_t offset); + ChunkState* next_by_stream(uint64_t offset); + + ChunkState* first(); + + ChunkStateIter begin(); + ChunkStateIter end(); + + size_t size() const; + + bool has_info() const; + bool has_message_idx() const; + + StreamChunksMap& stream_chunks() { return stream_chunks_; } + + // builds internal links between ChunkInfoNode per stream + void link_stream_chunks(); + + private: + std::unordered_map pile_{}; + std::unordered_map pile_info_{}; + + // ordered list of chunks offsets per stream id (only when ChunkInfo + // is present) + StreamChunksMap stream_chunks_{}; +}; + +std::string to_string(const ChunkState& chunk_state); +std::string to_string(const ChunkInfoNode& chunk_info); + +class Reader; +class MessageRef; +class ChunkRef; +class ChunksPile; +class ChunksRange; +struct MessagesStandardIter; +struct MessagesStreamingIter; +struct MessagesChunkIter; + +class MessagesStreamingRange; + +/** + * Chunks forward iterator in order of offset. + */ +struct ChunksIter { + using iterator_category = std::forward_iterator_tag; + using value_type = const ChunkRef; + using difference_type = std::ptrdiff_t; + using pointer = const std::unique_ptr; + using reference = const ChunkRef&; + + ChunksIter(); + ChunksIter(const ChunksIter& other); + ChunksIter& operator=(const ChunksIter& other) = default; + + const ChunkRef operator*() const; + const std::unique_ptr operator->() const; + ChunksIter& operator++(); + ChunksIter operator++(int); + bool operator==(const ChunksIter& other) const; + bool operator!=(const ChunksIter& other) const; + + std::string to_string() const; + + private: + ChunksIter(const uint64_t begin_addr, const uint64_t end_addr, + Reader* reader); + // move iterator to the next chunk of "verified" chunks set + void next(); + void next_any(); + bool is_cleared(); + + uint64_t current_addr_; + uint64_t end_addr_; + Reader* reader_; + friend class ChunksRange; +}; // ChunksIter + + +/** + * Chunks range + */ +class ChunksRange { + public: + ChunksIter begin() const; + ChunksIter end() const; + + std::string to_string() const; + + private: + ChunksRange(const uint64_t begin_addr, const uint64_t end_addr, + Reader* reader); + + uint64_t begin_addr_; + uint64_t end_addr_; + Reader* reader_; + friend class Reader; +}; // ChunksRange + +/** + * Messages range. + */ +class MessagesStandardRange { + public: + MessagesStandardIter begin() const; + MessagesStandardIter end() const; + + std::string to_string() const; + + private: + MessagesStandardRange(const ChunksIter begin_it, const ChunksIter end_it); + + ChunksIter begin_chunk_it_; + ChunksIter end_chunk_it_; + friend class Reader; +}; // MessagesStandardRange + +/** + * Messages forward iterator to read all messages across chunks span. + */ +struct MessagesStandardIter { + using iterator_category = std::forward_iterator_tag; + using value_type = const MessageRef; + using difference_type = std::ptrdiff_t; + using pointer = const std::unique_ptr; + using reference = const MessageRef&; + + MessagesStandardIter(); + MessagesStandardIter(const MessagesStandardIter& other); + MessagesStandardIter& operator=(const MessagesStandardIter& other) = + default; + + const MessageRef operator*() const; + std::unique_ptr operator->() const; + MessagesStandardIter& operator++(); + MessagesStandardIter operator++(int); + bool operator==(const MessagesStandardIter& other) const; + bool operator!=(const MessagesStandardIter& other) const; + + std::string to_string() const; + + private: + MessagesStandardIter(const ChunksIter begin_it, const ChunksIter end_it, + const size_t msg_idx); + // move iterator to the next msg that passes is_cleared() + void next(); + void next_any(); + // true if the current msg pointer passes is_cleared() test (i.e. valid) + bool is_cleared(); + + ChunksIter current_chunk_it_; + ChunksIter end_chunk_it_; + size_t msg_idx_; + friend class MessagesStandardRange; +}; // MessagesStandardIter + +/** + * %OSF Reader that simply reads sequentially messages from the OSF file. + * + * @todo Add filtered reads, and other nice things... + */ +class Reader { + public: + /** + * Creates reader from %OSF file resource. + */ + Reader(OsfFile& osf_file); + + /** + * Creates reader from %OSF file name. + */ + Reader(const std::string& file); + + /** + * Reads the messages from the first OSF chunk in sequental order + * till the end. Doesn't support RandomAccess. + */ + MessagesStreamingRange messages(); + + MessagesStreamingRange messages(const ts_t start_ts, const ts_t end_ts); + + MessagesStreamingRange messages(const std::vector& stream_ids); + + MessagesStreamingRange messages(const std::vector& stream_ids, + const ts_t start_ts, const ts_t end_ts); + + /** + * Find the timestamp of the message by its index and stream_id. + * + * Requires the OSF with message_counts inside, i.e. has_message_idx() + * return ``True``, otherwise return value is always empty (nullopt). + * + * @param stream_id[in] stream id on which the message_idx search is + * performed + * @param message_idx[in] the message index (i.e. rank/number) to search for + * @return message timestamp that corresponds to the message_idx in the + * stream_id + */ + nonstd::optional ts_by_message_idx(uint32_t stream_id, + uint32_t message_idx); + + /** + * Whether OSF contains the message counts that are needed for + * ``ts_by_message_idx()`` + * + * Message counts was added a bit later to the OSF core + * (ChunkInfo struct), so this function will be obsolete over time. + */ + bool has_message_idx() const { return chunks_.has_message_idx(); }; + + MessagesStandardRange messages_standard(); + + /** + * Reads chunks and returns the iterator to valid chunks only. + * NOTE: Every chunk is read in full and validated. (i.e. it's not just + * iterator over chunks index) + */ + ChunksRange chunks(); + + /** metadata id */ + std::string id() const; + + /** metadata start ts */ + ts_t start_ts() const; + + /** metadata end ts */ + ts_t end_ts() const; + + /** Metadata store to get access to all metadata entries. */ + const MetadataStore& meta_store() const { return meta_store_; } + + /** if it can be read by stream and in non-decreasing timestamp order. */ + bool has_stream_info() const; + + private: + + void read_metadata(); + void read_chunks_info(); // i.e. StreamingInfo.chunks[] information + + void print_metadata_entries(); + + // Checks the flatbuffers validity of a chunk by chunk offset. + bool verify_chunk(uint64_t chunk_offset); + + OsfFile file_; + + MetadataStore meta_store_{}; + + ChunksPile chunks_{}; + + // absolute offset to the beginning of the chunks in a file. + uint64_t chunks_base_offset_{0}; + std::vector metadata_buf_{}; + + // NOTE: These classes need an access to private member `chunks_` ... + friend class ChunkRef; + friend struct ChunksIter; + friend struct MessagesStreamingIter; +}; // Reader + +/** + * Thin interface class that holds the pointer to the message + * and reconstructs underlying data to the corresponding object type given + * the Stream type. + */ +class MessageRef { + public: + using ts_t = osf::ts_t; + + /** + * The only way to create the MessageRef is to point to the corresponding + * byte buffer of the message in OSF file. + * @param meta_provider the metadata store that is used in types + * reconstruction + */ + MessageRef(const uint8_t* buf, const MetadataStore& meta_provider) + : buf_(buf), meta_provider_(meta_provider), chunk_buf_{nullptr} {} + + MessageRef(const uint8_t* buf, const MetadataStore& meta_provider, + std::shared_ptr> chunk_buf) + : buf_(buf), meta_provider_(meta_provider), chunk_buf_{chunk_buf} {} + + /** Message stream id */ + uint32_t id() const; + + /** Timestamp of the message */ + ts_t ts() const; + + /// @todo [pb] Type of the stored data (meta of the stream?) + // std::string stream_type() const; + + /** Pointer to the underlying data */ + const uint8_t* buf() const { return buf_; } + + /** Debug string representation */ + std::string to_string() const; + + /** Checks whether the message belongs to the specified Stream type */ + template + bool is() const { + auto meta = + meta_provider_.get(id()); + return (meta != nullptr); + } + + bool is(const std::string& type_str) const; + + /** Reconstructs the underlying data to the class (copies data) */ + template + std::unique_ptr decode_msg() const { + auto meta = + meta_provider_.get(id()); + + if (meta == nullptr) { + // Stream and metadata entry id is inconsistent + return nullptr; + } + + return Stream::decode_msg(buffer(), *meta, meta_provider_); + } + + std::vector buffer() const; + + bool operator==(const MessageRef& other) const; + bool operator!=(const MessageRef& other) const; + + private: + const uint8_t* buf_; + const MetadataStore& meta_provider_; + + std::shared_ptr chunk_buf_; +}; // MessageRef + +/** + * Thin interface class that holds the pointer to the chunk and hides the + * messages reading routines. It expects that Chunk was "verified" before + * creating a ChunkRef. + * + */ +class ChunkRef { + public: + ChunkRef(); + ChunkRef(const uint64_t offset, Reader* reader); + + bool operator==(const ChunkRef& other) const; + bool operator!=(const ChunkRef& other) const; + + ChunkState* state() { return reader_->chunks_.get(chunk_offset_); } + const ChunkState* state() const { + return reader_->chunks_.get(chunk_offset_); + } + + ChunkInfoNode* info() { return reader_->chunks_.get_info(chunk_offset_); } + const ChunkInfoNode* info() const { + return reader_->chunks_.get_info(chunk_offset_); + } + + MessagesChunkIter begin() const; + MessagesChunkIter end() const; + + const MessageRef operator[](size_t msg_idx) const; + + std::unique_ptr messages(size_t msg_idx) const; + + /** Debug string representation */ + std::string to_string() const; + + uint64_t offset() const { return chunk_offset_; } + ts_t start_ts() const { return state()->start_ts; } + ts_t end_ts() const { return state()->end_ts; } + + size_t size() const; + + bool valid() const; + + private: + const uint8_t* get_chunk_ptr() const; + + uint64_t chunk_offset_; + Reader* reader_; + + std::shared_ptr chunk_buf_; +}; // ChunkRef + + +/** + * Convenienv iterator over all messages in a chunk. + */ +struct MessagesChunkIter { + using iterator_category = std::forward_iterator_tag; + using value_type = const MessageRef; + using difference_type = std::ptrdiff_t; + using pointer = const std::unique_ptr; + using reference = const MessageRef&; + + MessagesChunkIter(); + MessagesChunkIter(const MessagesChunkIter& other); + MessagesChunkIter& operator=(const MessagesChunkIter& other) = default; + + const MessageRef operator*() const; + std::unique_ptr operator->() const; + MessagesChunkIter& operator++(); + MessagesChunkIter operator++(int); + MessagesChunkIter& operator--(); + MessagesChunkIter operator--(int); + bool operator==(const MessagesChunkIter& other) const; + bool operator!=(const MessagesChunkIter& other) const; + + std::string to_string() const; + + private: + MessagesChunkIter(const ChunkRef chunk_ref, const size_t msg_idx); + void next(); + void prev(); + + ChunkRef chunk_ref_; + size_t msg_idx_; + friend class ChunkRef; +}; // MessagesChunkIter + + +class MessagesStreamingRange { + public: + MessagesStreamingIter begin() const; + MessagesStreamingIter end() const; + + std::string to_string() const; + + private: + // using range [start_ts, end_ts] <---- not inclusive .... !!! + MessagesStreamingRange(const ts_t start_ts, const ts_t end_ts, + const std::vector& stream_ids, + Reader* reader); + + ts_t start_ts_; + ts_t end_ts_; + std::vector stream_ids_; + Reader* reader_; + friend class Reader; +}; // MessagesStreamingRange + +/** + * Iterator over all messages in Streaming Layout order for specified + * timestamp range. + */ +struct MessagesStreamingIter { + using iterator_category = std::forward_iterator_tag; + using value_type = const MessageRef; + using difference_type = std::ptrdiff_t; + using pointer = const std::unique_ptr; + using reference = const MessageRef&; + + using opened_chunk_type = std::pair; + + struct greater_chunk_type { + bool operator()(const opened_chunk_type& a, + const opened_chunk_type& b) { + return a.first[a.second].ts() > b.first[b.second].ts(); + } + }; + + MessagesStreamingIter(); + MessagesStreamingIter(const MessagesStreamingIter& other); + MessagesStreamingIter& operator=(const MessagesStreamingIter& other) = + default; + + const MessageRef operator*() const; + std::unique_ptr operator->() const; + MessagesStreamingIter& operator++(); + MessagesStreamingIter operator++(int); + bool operator==(const MessagesStreamingIter& other) const; + bool operator!=(const MessagesStreamingIter& other) const; + + std::string to_string() const; + + void print_and_finish(); + + private: + // using range [start_ts, end_ts) <---- not inclusive .... !!! + MessagesStreamingIter(const ts_t start_ts, const ts_t end_ts, + const std::vector& stream_ids, + Reader* reader); + void next(); + + ts_t curr_ts_; + ts_t end_ts_; + std::vector stream_ids_; + uint32_t stream_ids_hash_; + Reader* reader_; + std::priority_queue, + greater_chunk_type> + curr_chunks_{}; + friend class Reader; + friend class MessagesStreamingRange; +}; // MessagesStreamingIter + +} // namespace osf +} // namespace ouster \ No newline at end of file diff --git a/ouster_osf/include/ouster/osf/stream_lidar_scan.h b/ouster_osf/include/ouster/osf/stream_lidar_scan.h new file mode 100644 index 00000000..901a5907 --- /dev/null +++ b/ouster_osf/include/ouster/osf/stream_lidar_scan.h @@ -0,0 +1,144 @@ +/** + * Copyright (c) 2021, Ouster, Inc. + * All rights reserved. + * + * @file stream_lidar_scan.h + * @brief Stream of LidarScan + * + */ +#pragma once + +#include "ouster/osf/basics.h" +#include "ouster/osf/metadata.h" +#include "ouster/osf/writer.h" + +#include "ouster/osf/meta_lidar_sensor.h" + +namespace ouster { +namespace osf { + +// Cast `ls_src` LidarScan to a subset of fields with possible different +// underlying ChanFieldTypes. +// @return a copy of `ls_src` with transformed fields +LidarScan slice_with_cast(const LidarScan& ls_src, + const LidarScanFieldTypes& field_types); + +// Zeros field +struct zero_field { + template + void operator()(Eigen::Ref> field_dest) { + field_dest.setZero(); + } +}; + +/** + * Metadata entry for LidarScanStream to store reference to a sensor and + * field_types + * + * @verbatim + * Fields: + * sensor_meta_id: metadata_ref - reference to LidarSensor metadata that + * describes the sensor configuration. + * field_types: LidarScan fields specs + * + * OSF type: + * ouster/v1/os_sensor/LidarScanStream + * + * Flatbuffer definition file: + * fb/os_sensor/lidar_scan_stream.fbs + * @endverbatim + * + */ +class LidarScanStreamMeta : public MetadataEntryHelper { + public: + + LidarScanStreamMeta(const uint32_t sensor_meta_id, + const LidarScanFieldTypes field_types = {}) + : sensor_meta_id_{sensor_meta_id}, + field_types_{field_types.begin(), field_types.end()} {} + + uint32_t sensor_meta_id() const { return sensor_meta_id_; } + + const LidarScanFieldTypes& field_types() const { return field_types_; } + + // Simplified with MetadataEntryHelper: type()+clone() + // std::string type() const override; + // std::unique_ptr clone() const override; + + std::vector buffer() const final; + + static std::unique_ptr from_buffer( + const std::vector& buf); + + std::string repr() const override; + + private: + uint32_t sensor_meta_id_{0}; + LidarScanFieldTypes field_types_; +}; + +template <> +struct MetadataTraits { + static const std::string type() { + return "ouster/v1/os_sensor/LidarScanStream"; + } +}; + +/** + * LidarScanStream that encodes LidarScan objects into the messages. + * + * @verbatim + * Object type: ouster::sensor::LidarScan + * Meta type: LidarScanStreamMeta (sensor_meta_id, field_types) + * + * Flatbuffer definition file: + * fb/os_sensor/lidar_scan_stream.fbs + * @endverbatim + * + */ +class LidarScanStream : public MessageStream { + public: + LidarScanStream(Writer& writer, const uint32_t sensor_meta_id, + const LidarScanFieldTypes& field_types = {}); + + /** + * Saves the object to the writer applying the coding/serizlization algorithm + * defined in make_msg() function. The function is the same for all streams + * types ... + * + * @todo [pb]: Probably should be abstracted/extracted from all streams + * we also might want to have the corresponding function to read back + * sequentially from Stream that doesn't seem like fit into this model... + */ + void save(const ouster::osf::ts_t ts, const obj_type& lidar_scan); + + /** Encode/serialize the object to the buffer of bytes */ + std::vector make_msg(const obj_type& lidar_scan); + + /** + * Decode/deserialize the object from bytes buffer using the concrete + * metadata type for the stream. + * metadata_provider is used to reconstruct any references to other + * metadata entries dependencies (like sensor_meta_id) + */ + static std::unique_ptr decode_msg( + const std::vector& buf, const meta_type& meta, + const MetadataStore& meta_provider); + + const meta_type& meta() const { return meta_; } + + private: + Writer& writer_; + + meta_type meta_; + + uint32_t stream_meta_id_{0}; + + uint32_t sensor_meta_id_{0}; + + sensor::sensor_info sensor_info_{}; + +}; + +} // namespace osf +} // namespace ouster diff --git a/ouster_osf/include/ouster/osf/writer.h b/ouster_osf/include/ouster/osf/writer.h new file mode 100644 index 00000000..9b7bacc0 --- /dev/null +++ b/ouster_osf/include/ouster/osf/writer.h @@ -0,0 +1,169 @@ +/** + * Copyright (c) 2021, Ouster, Inc. + * All rights reserved. + * + * @file writer.h + * @brief OSF file Writer + * + */ +#pragma once + +#include + +#include "ouster/osf/basics.h" +#include "ouster/osf/metadata.h" + +namespace ouster { +namespace osf { + +/** + * Chunks writing strategy that decides when and how exactly write chunks + * to a file. See RFC 0018 for Standard and Streaming Layout description. + */ +class ChunksWriter { + public: + virtual void saveMessage(const uint32_t stream_id, const ts_t ts, + const std::vector& buf) = 0; + virtual void finish() = 0; + virtual uint32_t chunk_size() const = 0; + virtual ~ChunksWriter() = default; +}; + +/** + * %OSF Writer provides the base universal interface to store the collection + * of metadata entries, streams and corresponding objects. + * + * Examples: + * @ref writer_test.cpp, writer_custom_test.cpp + * + */ +class Writer { + public: + explicit Writer(const std::string& file_name); + + Writer(const std::string& file_name, const std::string& metadata_id, + uint32_t chunk_size = 0); + + template + uint32_t addMetadata(MetaParams&& ...params) { + MetaType entry(std::forward(params)...); + return meta_store_.add(entry); + } + + uint32_t addMetadata(MetadataEntry&& entry) { + return addMetadata(entry); + } + + uint32_t addMetadata(MetadataEntry& entry) { + return meta_store_.add(entry); + } + + template + std::shared_ptr getMetadata( + const uint32_t metadata_id) const { + return meta_store_.get(metadata_id); + } + + std::shared_ptr getMetadata( + const uint32_t metadata_id) const { + return meta_store_.get(metadata_id); + } + + /** + * Creating streams by passing itself as first argument of the ctor and + * following the all other parameters. + */ + template + Stream createStream(StreamParams&& ...params) { + return Stream(*this, std::forward(params)...); + } + + /** + * %Writer accepts messages in the form of bytes buffers with linked meta_id + * and timestamp. + * @todo [pb]: It should be hidden into private/protected, but I don't see + * yet how to do it and give an access to every derived Stream objects. + */ + void saveMessage(const uint32_t stream_id, const ts_t ts, + const std::vector& buf); + + const MetadataStore& meta_store() const { return meta_store_; } + + const std::string& metadata_id() const { return metadata_id_; } + void setMetadataId(const std::string& id) { metadata_id_ = id; } + + const std::string& filename() const { return file_name_; } + + ChunksLayout chunks_layout() const {return chunks_layout_; } + uint32_t chunk_size() const; + + // writes buf to the file with CRC32 appended and return the number of + // bytes writen to the file + uint64_t append(const uint8_t* buf, const uint64_t size); + + uint64_t emit_chunk(const ts_t start_ts, const ts_t end_ts, + const std::vector& chunk_buf); + + /** Finish file with a proper metadata object, and header */ + void close(); + + ~Writer(); + + // copy/move = delete everything + Writer(const Writer&) = delete; + Writer& operator=(const Writer&) = delete; + Writer(Writer&&) = delete; + Writer& operator=(Writer&&) = delete; + + private: + + // helper to construct the Metadata OSF Block at the end of writing + std::vector make_metadata() const; + + std::string file_name_; + + uint32_t header_size_{0}; + int64_t pos_{-1}; + bool started_{false}; + bool finished_{false}; + + std::vector chunks_{}; + ts_t start_ts_{ts_t::max()}; + ts_t end_ts_{ts_t::min()}; + uint64_t next_chunk_offset_{0}; + + std::string metadata_id_{}; + + ChunksLayout chunks_layout_{ChunksLayout::LAYOUT_STANDARD}; + + MetadataStore meta_store_{}; + + // ChunksWriter is reponsible for chunking strategy + std::shared_ptr chunks_writer_{nullptr}; +}; + +class ChunkBuilder { + public: + ChunkBuilder(){}; + void saveMessage(const uint32_t stream_id, const ts_t ts, + const std::vector& msg_buf); + void reset(); + std::vector finish(); + uint32_t size() const; + uint32_t messages_count() const; + ts_t start_ts() const { return start_ts_; } + ts_t end_ts() const { return end_ts_; } + + private: + void update_start_end(const ts_t ts); + + bool finished_{false}; + + flatbuffers::FlatBufferBuilder fbb_{0x7fff}; + ts_t start_ts_{ts_t::max()}; + ts_t end_ts_{ts_t::min()}; + std::vector> messages_{}; +}; + +} // namespace osf +} // namespace ouster \ No newline at end of file diff --git a/ouster_osf/src/basics.cpp b/ouster_osf/src/basics.cpp new file mode 100644 index 00000000..8f838fe5 --- /dev/null +++ b/ouster_osf/src/basics.cpp @@ -0,0 +1,145 @@ +/** + * Copyright(c) 2021, Ouster, Inc. + * All rights reserved. + */ + +#include +#include +#include +#include + +#include "ouster/osf/basics.h" +#include "ouster/osf/crc32.h" + +#include "nonstd/optional.hpp" + +namespace ouster { +namespace osf { + +using nonstd::make_optional; +using nonstd::nullopt; +using nonstd::optional; + +namespace impl { + +// TODO[pb]: Review some time later these enum/strings converters ... +// Copying all functions to handling enums from ouster-example +// later we will not copy and share code in some other way + +template +using Table = std::array, N>; + +extern const Table chunks_layout_strings{ + {{ChunksLayout::LAYOUT_STANDARD, "STANDARD"}, + {ChunksLayout::LAYOUT_STREAMING, "STREAMING"}}}; + +} // namespace impl + +/* String conversion */ + +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 std::strcmp(p.second, v) == 0; + }); + + return res == end ? nullopt : make_optional(res->first); +} + +std::string to_string(ChunksLayout chunks_layout) { + auto res = lookup(impl::chunks_layout_strings, chunks_layout); + return res ? res.value() : "UNKNOWN"; +} + +ChunksLayout chunks_layout_of_string(const std::string& s) { + auto res = rlookup(impl::chunks_layout_strings, s.c_str()); + return res ? res.value() : ChunksLayout::LAYOUT_STANDARD; +} + +std::string to_string(const HEADER_STATUS status) { + return v2::EnumNamesHEADER_STATUS()[static_cast(status)]; +} + +std::string to_string(const uint8_t* buf, const size_t count, + const size_t max_show_count) { + std::stringstream ss; + ss << std::hex; + size_t show_count = count; + if (max_show_count != 0 && max_show_count < count) + show_count = max_show_count; + for (size_t i = 0; i < show_count; ++i) { + if (i > 0) ss << " "; + ss << std::setfill('0') << std::setw(2) << static_cast(buf[i]); + } + if (show_count < count) { + ss << " ... and " << std::dec << (count - show_count) << " more ..."; + } + return ss.str(); +} + +std::string read_text_file(const std::string& filename) { + std::stringstream buf{}; + std::ifstream ifs{}; + ifs.open(filename); + buf << ifs.rdbuf(); + ifs.close(); + + if (!ifs) { + std::stringstream ss; + ss << "Failed to read file: " << filename; + throw std::runtime_error(ss.str()); + } + + return buf.str(); +} + +uint32_t get_prefixed_size(const uint8_t* buf) { + return buf[0] + (buf[1] << 8u) + (buf[2] << 16u) + (buf[3] << 24u); +} + +uint32_t get_block_size(const uint8_t* buf) { + return get_prefixed_size(buf) + FLATBUFFERS_PREFIX_LENGTH + CRC_BYTES_SIZE; +} + +bool check_prefixed_size_block_crc(const uint8_t* buf, + const uint32_t buf_length) { + 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; + return false; + } + + const uint32_t crc_stored = + get_prefixed_size(buf + prefixed_size + FLATBUFFERS_PREFIX_LENGTH); + const uint32_t crc_calculated = + osf::crc32(buf, prefixed_size + FLATBUFFERS_PREFIX_LENGTH); + + 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; + } + return res; +} + +} // namespace osf +} // namespace ouster diff --git a/ouster_osf/src/compat_ops.cpp b/ouster_osf/src/compat_ops.cpp new file mode 100644 index 00000000..1a51b5dc --- /dev/null +++ b/ouster_osf/src/compat_ops.cpp @@ -0,0 +1,309 @@ +/** + * Copyright(c) 2021, Ouster, Inc. + * All rights reserved. + */ + +#include "compat_ops.h" + +#include +#include + +#ifdef _WIN32 +#include +#include +#include +#include +#else +#include +#include +#include +#include +#include +#endif + +namespace ouster { +namespace osf { + +static const std::string FILE_SEPS = "\\/"; + +namespace { + +#ifdef _WIN32 +// ErrorMessage support function. +// Retrieves the system error message for the GetLastError() code. +// Note: caller must use LocalFree() on the returned LPCTSTR buffer. +LPCTSTR ErrorMessage(DWORD error) { + LPVOID lpMsgBuf; + FormatMessageA(FORMAT_MESSAGE_ALLOCATE_BUFFER | FORMAT_MESSAGE_FROM_SYSTEM | + FORMAT_MESSAGE_IGNORE_INSERTS, + NULL, error, NULL, (LPTSTR)&lpMsgBuf, 0, NULL); + return ((LPCTSTR)lpMsgBuf); +} + +// PrintError support function. +// 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); + LocalFree((LPVOID)errMsg); +} + +// Get the last system error and return it in a string (not wide string) +std::string LastErrorMessageStr() { + LPCTSTR errMsg = ErrorMessage(GetLastError()); + // NOTE[pb]: errMsg can be a char or wchar and seems the below copy + // construct is OK for MSVC in both cases. (or no? idk) + std::string res{errMsg}; + LocalFree((LPVOID)errMsg); + return res; +} +#endif + +} // anonym namespace + +/// Get the last system error and return it in a string +std::string get_last_error() { +#ifdef _WIN32 + return LastErrorMessageStr(); +#else + return std::string(std::strerror(errno)); +#endif +} + +/// Checking the the path is it directory or not. +bool is_dir(const std::string& path) { +#ifdef _WIN32 + DWORD attrs = GetFileAttributesA(path.c_str()); + if (attrs == INVALID_FILE_ATTRIBUTES) { + return false; + } + if (attrs & FILE_ATTRIBUTE_DIRECTORY) { + return true; + } + return false; +#else + struct stat statbuf; + if (stat(path.c_str(), &statbuf) != 0) { + if (errno != ENOENT) printf("ERROR: stat: %s", std::strerror(errno)); + return false; + } + return S_ISDIR(statbuf.st_mode); +#endif +} + +/// Check path existence on the system +bool path_exists(const std::string& path) { +#ifdef _WIN32 + // taken from here: https://devblogs.microsoft.com/oldnewthing/20071023-00/?p=24713 + DWORD attrs = GetFileAttributesA(path.c_str()); + return (attrs != INVALID_FILE_ATTRIBUTES); +#else + struct stat sb; + return !(stat(path.c_str(), &sb) != 0); +#endif +} + +bool is_file_sep(char c) { + return (FILE_SEPS.find(c) != std::string::npos); +} + +/// Path concatenation with OS specific path separator +// NOTE: Trying not to be too smart here ... and it's impossible function +// to make it fully correct, so use it wisely. +std::string path_concat(const std::string& path1, const std::string& path2) { + if (path1.empty()) return path2; + if (path2.empty()) return path1; + + if (is_file_sep(path2.front())) return path2; +#ifdef _WIN32 + if (path2.size() > 1 && path2.at(1) == ':') return path2; +#endif + + size_t p1_last_slash = path1.size(); + while (p1_last_slash > 0 && is_file_sep(path1.at(p1_last_slash - 1))) { + p1_last_slash--; + } + return path1.substr(0, p1_last_slash) + FILE_SEP + path2; +} + +/// Get the path to unique temp directory and create it. +bool make_tmp_dir(std::string& tmp_path) { +#ifdef _WIN32 + unsigned int uRetVal = 0; + char lpTempPath[MAX_PATH]; + uRetVal = GetTempPathA(MAX_PATH, lpTempPath); + if (uRetVal > MAX_PATH || uRetVal == 0) { + return false; + } + char pTmpPath[MAX_PATH]; + if (!tmpnam_s(pTmpPath)) { + if (CreateDirectoryA(pTmpPath, NULL)) { + tmp_path = pTmpPath; + return true; + } + } + PrintError(TEXT("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; + return false; + }; + tmp_path = tmpdir; + return true; +#endif +} + +/// Make directory +bool make_dir(const std::string& path) { +#ifdef _WIN32 + 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()); + return false; + } + return true; +#endif +} + +/// Get environment variable +bool get_env_var(const std::string& name, std::string& value) { +#ifdef _WIN32 + const unsigned int BUFSIZE = 4096; + char var_value[BUFSIZE]; + unsigned int ret_val = + GetEnvironmentVariableA(name.c_str(), var_value, BUFSIZE); + if (ret_val != 0 && ret_val < BUFSIZE) { + value.assign(var_value); + return true; + } + value.clear(); + return false; +#else + char* var_value; + if ((var_value = std::getenv(name.c_str())) != nullptr) { + value = var_value; + return true; + } + value.clear(); + return false; +#endif +} + +/// Unlink path (i.e. almost like remove) +bool unlink_path(const std::string& path) { +#ifdef _WIN32 + return (_unlink(path.c_str()) == 0); +#else + return (unlink(path.c_str()) == 0); +#endif +} + +// Remove directory +bool remove_dir(const std::string& path) { +#ifdef _WIN32 + return (_rmdir(path.c_str()) == 0); +#else + return (rmdir(path.c_str()) == 0); +#endif +} + +/// Get file size +int64_t file_size(const std::string& path) { +#ifdef _WIN32 + LARGE_INTEGER fsize; + WIN32_FILE_ATTRIBUTE_DATA file_attr_data; + if (!GetFileAttributesExA(path.c_str(), GetFileExInfoStandard, + &file_attr_data)) { + return -1; + } + if (file_attr_data.dwFileAttributes & FILE_ATTRIBUTE_DIRECTORY) { + return -2; + } + fsize.LowPart = file_attr_data.nFileSizeLow; + fsize.HighPart = file_attr_data.nFileSizeHigh; + return fsize.QuadPart; +#else + struct stat st; + if (stat(path.c_str(), &st) < 0) { + return -1; + }; + if (!S_ISREG(st.st_mode)) { + return -2; + } + return st.st_size; +#endif +} + +/// File mapping open (read-only operations) +uint8_t* mmap_open(const std::string& path) { +#ifdef _WIN32 + HANDLE hFile; + uint8_t* pBuf; + hFile = CreateFileA(path.c_str(), GENERIC_READ, FILE_SHARE_READ, NULL, + OPEN_EXISTING, + FILE_ATTRIBUTE_NORMAL | FILE_FLAG_OVERLAPPED, NULL); + if (hFile == INVALID_HANDLE_VALUE) { + return NULL; + } + HANDLE hFileMap; + hFileMap = CreateFileMappingA(hFile, NULL, PAGE_READONLY, 0, 0, NULL); + if (hFileMap == NULL) { + CloseHandle(hFile); + return NULL; + } + + pBuf = static_cast( + MapViewOfFile(hFileMap, FILE_MAP_READ, 0, 0, 0)); + + // TODO: check errors? + CloseHandle(hFileMap); + CloseHandle(hFile); + + return pBuf; +#else + struct stat st; + if (stat(path.c_str(), &st) < 0) { + return nullptr; + }; + if (!S_ISREG(st.st_mode)) { + return nullptr; + } + if (st.st_size == 0) { + return nullptr; + } + + int fd = open(path.c_str(), O_RDONLY); + if (fd < 0) { + return nullptr; + } + + void* map_osf_file = mmap(0, st.st_size, PROT_READ, MAP_SHARED, fd, 0); + if (map_osf_file == MAP_FAILED) { + ::close(fd); + return nullptr; + } + return static_cast(map_osf_file); +#endif +} + +/// File mapping close +bool mmap_close(uint8_t* file_buf, const uint64_t file_size) { + if (file_buf == nullptr || file_size == 0) return false; +#ifdef _WIN32 + return (UnmapViewOfFile(file_buf) != 0); +#else + return (munmap(static_cast(file_buf), file_size) >= 0); +#endif +} + + + + +} // namespace osf +} // namespace ouster \ No newline at end of file diff --git a/ouster_osf/src/compat_ops.h b/ouster_osf/src/compat_ops.h new file mode 100644 index 00000000..65891aa4 --- /dev/null +++ b/ouster_osf/src/compat_ops.h @@ -0,0 +1,57 @@ +/** + * Copyright(c) 2021, Ouster, Inc. + * All rights reserved. + */ + +#pragma once + +#include + +namespace ouster { +namespace osf { + +#ifdef _WIN32 +constexpr char FILE_SEP = '\\'; +#else +constexpr char FILE_SEP = '/'; +#endif + +/// Checking the the path is it directory or not. +bool is_dir(const std::string& path); + +/// Check path existence on the system +bool path_exists(const std::string& path); + +/// Path concatenation with OS specific path separator +std::string path_concat(const std::string& path1, const std::string& path2); + +/// Get the path to unique temp directory and create it. +bool make_tmp_dir(std::string& tmp_path); + +/// Make directory +bool make_dir(const std::string& path); + +/// Get environment variable +bool get_env_var(const std::string& name, std::string& value); + +// Unlink path +bool unlink_path(const std::string& path); + +// Remove directory +bool remove_dir(const std::string& path); + +// Get file size +int64_t file_size(const std::string& path); + +// File mapping open +uint8_t* mmap_open(const std::string& path); + +// File mapping close +bool mmap_close(uint8_t* file_buf, const uint64_t file_size); + +/// Get the last system error and return it in a string (not wide string) +std::string get_last_error(); + + +} // namespace osf +} // namespace ouster \ No newline at end of file diff --git a/ouster_osf/src/crc32.cpp b/ouster_osf/src/crc32.cpp new file mode 100644 index 00000000..705ca45e --- /dev/null +++ b/ouster_osf/src/crc32.cpp @@ -0,0 +1,32 @@ +/** + * Copyright(c) 2021, Ouster, Inc. + * All rights reserved. + */ + +#include "ouster/osf/crc32.h" + +#include +#include +#include +#include +#include + +#include + +namespace ouster { +namespace osf { + +const uint32_t CRC_INITIAL_VALUE = 0L; + +// =============== ZLIB functions wrappers ==================== + +uint32_t crc32(const uint8_t* buf, uint32_t size) { + return crc32_z(CRC_INITIAL_VALUE, buf, size); +} + +uint32_t crc32(uint32_t initial_crc, const uint8_t* buf, uint32_t size) { + return crc32_z(initial_crc, buf, size); +} + +} // namespace osf +} // namespace ouster \ No newline at end of file diff --git a/ouster_osf/src/fb_utils.cpp b/ouster_osf/src/fb_utils.cpp new file mode 100644 index 00000000..8a48bd0c --- /dev/null +++ b/ouster_osf/src/fb_utils.cpp @@ -0,0 +1,167 @@ +/** + * Copyright(c) 2021, Ouster, Inc. + * All rights reserved. + */ + +#include +#include + +#include "ouster/osf/basics.h" +#include "ouster/osf/crc32.h" + +#include "fb_utils.h" + +namespace ouster { +namespace osf { + +bool check_osf_metadata_buf(const uint8_t* buf, const uint32_t buf_size) { + // note: fb verifier checks for exact size of buffer equal to prefixed size + auto verifier = + flatbuffers::Verifier(buf, buf_size - ouster::osf::CRC_BYTES_SIZE); + return check_prefixed_size_block_crc(buf, buf_size) && + gen::VerifySizePrefixedMetadataBuffer(verifier); +} + +bool check_osf_chunk_buf(const uint8_t* buf, const uint32_t buf_size) { + // note: fb verifier checks for exact size of buffer equal to prefixed size + auto verifier = + flatbuffers::Verifier(buf, buf_size - ouster::osf::CRC_BYTES_SIZE); + return check_prefixed_size_block_crc(buf, buf_size) && + gen::VerifySizePrefixedChunkBuffer(verifier); +} + +template +std::vector vector_from_fb_vector(const flatbuffers::Vector* fb_vec) { + if (fb_vec == nullptr) return {}; + return {fb_vec->data(), fb_vec->data() + fb_vec->size()}; +} + +template std::vector vector_from_fb_vector( + const flatbuffers::Vector* fb_vec); +template std::vector vector_from_fb_vector( + const flatbuffers::Vector* fb_vec); +template std::vector vector_from_fb_vector( + const flatbuffers::Vector* fb_vec); + +// ============ File operations ========================== + +uint64_t buffer_to_file(const uint8_t* buf, const uint64_t size, + const std::string& filename, bool append) { + uint64_t saved_size = 0; + + uint32_t crc_res = osf::crc32(buf, size); + + std::fstream file_stream; + if (append) + file_stream.open(filename, std::fstream::out | std::fstream::app | + std::fstream::binary); + else { + file_stream.open(filename, std::fstream::out | std::fstream::trunc | + std::fstream::binary); + } + + if (file_stream.is_open()) { + file_stream.write(reinterpret_cast(buf), size); + if (!file_stream.good()) return 0; + file_stream.write(reinterpret_cast(&crc_res), sizeof(uint32_t)); + if (!file_stream.good()) return 0; + file_stream.close(); + saved_size = size + 4; + } else { + std::cerr << "fail to open " << filename << std::endl; + } + return saved_size; +} + +uint64_t builder_to_file(flatbuffers::FlatBufferBuilder& builder, + const std::string& filename, bool append) { + // Get buffer and save to file + const uint8_t* buf = builder.GetBufferPointer(); + uint32_t size = builder.GetSize(); + return buffer_to_file(buf, size, filename, append); +} + +uint64_t start_osf_file(const std::string& filename) { + auto header_fbb = flatbuffers::FlatBufferBuilder(1024); + auto header = ouster::osf::gen::CreateHeader( + header_fbb, ouster::osf::OSF_VERSION::V_2_0, + ouster::osf::HEADER_STATUS::INVALID, 0, 0); + header_fbb.FinishSizePrefixed(header, ouster::osf::gen::HeaderIdentifier()); + return builder_to_file(header_fbb, filename, false); +} + +uint64_t finish_osf_file(const std::string& filename, + const uint64_t metadata_offset, + const uint32_t metadata_size) { + auto header_fbb = flatbuffers::FlatBufferBuilder(1024); + auto header = ouster::osf::gen::CreateHeader( + header_fbb, ouster::osf::OSF_VERSION::V_2_0, + ouster::osf::HEADER_STATUS::VALID, metadata_offset, + metadata_offset + metadata_size); + header_fbb.FinishSizePrefixed(header, ouster::osf::gen::HeaderIdentifier()); + + const uint8_t* buf = header_fbb.GetBufferPointer(); + uint32_t size = header_fbb.GetSize(); + + uint32_t crc_res = osf::crc32(buf, size); + + uint64_t saved_size = 0; + + std::ofstream file_stream; + // TODO[pb]: Need to check that file exists here and it contains the OSF + // header before overwrite it ... + file_stream.open(filename, std::fstream::out | std::fstream::in | + std::fstream::ate | std::fstream::binary); + if (file_stream.is_open()) { + file_stream.seekp(0); + + file_stream.write(reinterpret_cast(buf), size); + // TODO[pb]: It's an exception here .... add error processing + if (!file_stream.good()) return saved_size; + saved_size += size; + + file_stream.write(reinterpret_cast(&crc_res), sizeof(uint32_t)); + if (!file_stream.good()) return saved_size; + saved_size += sizeof(uint32_t); + + file_stream.close(); + } else { + std::cout << "fail to open " << filename << std::endl; + } + + return saved_size; +} + +void print_metadata_buf(const uint8_t* buf, const uint32_t buf_size) { + (void)buf_size; + auto a = ouster::osf::gen::GetSizePrefixedMetadata(buf); + std::cout << "=== Metadata: =====================" << std::endl; + std::cout << "id = " << a->id()->str() << std::endl; + std::cout << "start_ts = " << a->start_ts() << std::endl; + std::cout << "end_ts = " << a->end_ts() << std::endl; + auto cs = a->chunks(); + std::cout << "chunks.size = " << cs->size() << std::endl; + for (uint32_t i = 0; i < cs->size(); ++i) { + auto c = cs->Get(i); + std::cout << " chunks[" << i << "] = " << c->start_ts() << ", " + << c->end_ts() << ", " << c->offset() << std::endl; + } + auto ms = a->entries(); + std::cout << "entries.size = " << ms->size() << std::endl; + for (uint32_t i = 0; i < ms->size(); ++i) { + auto e = ms->Get(i); + std::cout << " entry[" << i << "] = " << e->id() + << ", type = " << e->type()->str() << std::endl; + auto buffer = e->buffer(); + std::cout << " buffer_size = " << buffer->size() << ", vals = ["; + std::cout << osf::to_string(buffer->Data(), + static_cast(buffer->size()), + 100) + << "]" << std::endl; + ; + } +} + + +} // namespace osf +} // namespace ouster \ No newline at end of file diff --git a/ouster_osf/src/fb_utils.h b/ouster_osf/src/fb_utils.h new file mode 100644 index 00000000..ebe79ab2 --- /dev/null +++ b/ouster_osf/src/fb_utils.h @@ -0,0 +1,122 @@ +/** + * Copyright(c) 2021, Ouster, Inc. + * All rights reserved. + */ + +#pragma once + +#include "chunk_generated.h" +#include "header_generated.h" +#include "metadata_generated.h" + +// OSF v2 basic types for LidarSensor and LidarScan/Imu Streams +#include "os_sensor/lidar_scan_stream_generated.h" +#include "os_sensor/lidar_sensor_generated.h" + +namespace ouster { +namespace osf { + +inline const gen::Metadata* get_osf_metadata_from_buf(const uint8_t* buf) { + return ouster::osf::gen::GetSizePrefixedMetadata(buf); +} + +inline const gen::Header* get_osf_header_from_buf(const uint8_t* buf) { + return gen::GetSizePrefixedHeader(buf); +} + +/** + * Verifies the validity of Header buffer and whether it's safe to read it. + * It's just checking the well formed Flatbuffer table (not CRC32 check here) + * + * @param buf Header buffer, size prefixed + * @param buf_size buffer size (with prefix size bytes but not including CRC32) + * @return true if buffer is valid and can be read + */ +inline bool verify_osf_header_buf(const uint8_t* buf, const uint32_t buf_size) { + auto verifier = flatbuffers::Verifier(buf, buf_size); + return gen::VerifySizePrefixedHeaderBuffer(verifier); +} + +/** + * Checks the validity of a Metadata buffer and whether it's safe to read it. + * It's checking the well formed Flatbuffer table and CRC32. + * + * @param buf metadata buffer, size prefixed + * @param buf_size buffer size (with CRC32 and prefix size bytes) + * @return true if buffer is valid and can be read + */ +bool check_osf_metadata_buf(const uint8_t* buf, const uint32_t buf_size); + +/** + * Checks the validity of a Chunk buffer and whether it's safe to read it. + * It's checking the well formed Flatbuffer table and CRC32. + * + * @param buf metadata buffer, size prefixed + * @param buf_size buffer size (with CRC32 and prefix size bytes) + * @return true if buffer is valid and can be read + */ +bool check_osf_chunk_buf(const uint8_t* buf, const uint32_t buf_size); + +/** transforms Flatbuffers vector to a std::vector. */ +template +std::vector vector_from_fb_vector(const flatbuffers::Vector* fb_vec); + +// ============ File operations ========================== + +/** + * Saves the buffer content to the file with additional 4 bytes of calculated + * CRC32 field in the end. Successfull operation writes size + 4 bytes to the + * file. + * + * @param buf pointer to the data to save, full content of the buffer used + * to calculate CRC + * @param size number of bytes to read from buffer and store to the file + * @param filename full path to the file + * @param append if true appends the content to the end of the file, + * otherwise - overwrite the file with the current buffer. + * @return number of bytes actuallt written to the file. Successfull write is + * size + 4 bytes (4 bytes for CRC field) + * + */ +uint64_t buffer_to_file(const uint8_t* buf, const uint64_t size, + const std::string& filename, bool append = false); + +/** + * Saves the content of Flatbuffer builder to the file with CRC32 field + * appended to the actual bytes. Usually it's a size prefixed finished builder + * but not necessarily + * + * @param builder Flatbuffers builder + * @param filename filename to save bytes + * @param append if true appends the content to the end of the file, + * otherwise - overwrite the file with the current buffer. + * @return number of bytes actuallt written to the file. Successfull write is + * size + 4 bytes (4 bytes for CRC field) + */ +uint64_t builder_to_file(flatbuffers::FlatBufferBuilder& builder, + const std::string& filename, bool append = false); + +/** + * Starts the OSF v2 file with a header (in INVALID state). + * + * @param filename of the file to be created. Overwrite if file exists. + * + */ +uint64_t start_osf_file(const std::string& filename); + +/** + * Finish OSF v2 file with updated offset to metadata and filesize. As a + * result file left in VALID state. + * + * @param filename of the file to be created. Overwrite if file exists. + * @return number of bytes actuallt written to the file. + */ +uint64_t finish_osf_file(const std::string& filename, + const uint64_t metadata_offset, + const uint32_t metadata_size); + +/** Debug method to print Flatbuffers Metadata buffer */ +void print_metadata_buf(const uint8_t* buf, const uint32_t buf_size); + +} // namespace osf +} // namespace ouster diff --git a/ouster_osf/src/file.cpp b/ouster_osf/src/file.cpp new file mode 100644 index 00000000..e9bd2072 --- /dev/null +++ b/ouster_osf/src/file.cpp @@ -0,0 +1,379 @@ +/** + * Copyright(c) 2021, Ouster, Inc. + * All rights reserved. + */ + +#include "ouster/osf/file.h" + +#include +#include +#include +#include +#include +#include + +#include "ouster/osf/crc32.h" +#include "compat_ops.h" +#include "fb_utils.h" + +namespace ouster { +namespace osf { + +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()); +} +#else +#define print_error(a, b) ((void)0) +#endif + +} + +// ======== Construction ============ + +OsfFile::OsfFile() + : filename_(), + offset_(0), + size_(0), + file_buf_(nullptr), + file_stream_{}, + header_chunk_{nullptr}, + metadata_chunk_{nullptr}, + chunk_cache_{nullptr}, + chunk_cache_offset_{std::numeric_limits::max()}, + state_(FileState::BAD) {} + +OsfFile::OsfFile(const std::string& filename, OpenMode mode) : OsfFile() { + filename_ = filename; + + // TODO[pb]: Extract to open function + if (mode == OpenMode::READ) { + if (is_dir(filename_)) { + error("got a dir, but expected a file"); + return; + } + + int64_t sz = file_size(filename_); + if (sz <= 0) { + error(); + return; + } + // TODO[pb]: This leads to incorrect file size for 4Gb+ files on the + // 32 bit systems like Emscripten/WASM. We need to check + // size_t everywhere and replace it so it can hold file + // sizes and file offsets bigger than 4Gb in 32 bit systems. + size_ = static_cast(sz); + +#ifdef OUSTER_OSF_NO_MMAP + // TODO[pb]: Maybe consider adding a runtime parameter to open file + // with mmap or open/read? Also better handling/removing of class + // members for OsfFile can be done so we are not copying empty + // values when NO_MMAP is absent... But I can't make my mind + // about RUNTIME/COMPILATION time parametrization and for now + // will leave it in a half backed state: COMPILE time directive + // but some fields will be left empty and copied/check during runtime + // there is no hit in performance/memory due to this leftovers + // that I could spot. + file_stream_ = + std::ifstream(filename_, std::ios::in | std::ios::binary); + if (!file_stream_.good()) { + error(); + return; + } +#else + file_buf_ = mmap_open(filename_); + if (!file_buf_) { + error(); + return; + } +#endif + + state_ = FileState::GOOD; + } else { + // Write is not yet implemented within this class. And other modes + // too. + error("write mode not implemented"); + return; + } +} + +OSF_VERSION OsfFile::version() { + if (!good()) { + return OSF_VERSION::V_INVALID; + } + auto osf_header = get_osf_header_from_buf(get_header_chunk_ptr()); + return static_cast(osf_header->version()); +} + +uint64_t OsfFile::metadata_offset() { + if (!good()) throw std::logic_error("bad osf file"); + auto osf_header = get_osf_header_from_buf(get_header_chunk_ptr()); + return osf_header->metadata_offset(); +} + +uint64_t OsfFile::chunks_offset() { + if (!good()) throw std::logic_error("bad osf file"); + const uint32_t header_size = get_prefixed_size(get_header_chunk_ptr()); + if (version() < OSF_VERSION::V_2_0) { + throw std::logic_error("bad osf file: only version >= 20 supported"); + } + return FLATBUFFERS_PREFIX_LENGTH + header_size + osf::CRC_BYTES_SIZE; +} + +bool OsfFile::valid() { + if (!good()) { + return false; + } + + uint32_t header_size = + get_prefixed_size(get_header_chunk_ptr()) + FLATBUFFERS_PREFIX_LENGTH; + + // Check flatbuffers osfHeader validity + if (!verify_osf_header_buf( + get_header_chunk_ptr(), + header_size)) { + print_error(filename_, "OSF header verification has failed."); + return false; + } + + if (!check_prefixed_size_block_crc(get_header_chunk_ptr(), + header_size + CRC_BYTES_SIZE)) { + print_error(filename_, "OSF header has an invalid CRC."); + return false; + } + + auto osf_header = get_osf_header_from_buf(get_header_chunk_ptr()); + if (osf_header->status() != v2::HEADER_STATUS::VALID) { + print_error(filename_, "OSF header is not valid."); + return false; + } + + if (osf_header->file_length() != size_) { + print_error(filename_, "OSF header file size field is incorrect."); + return false; + } + + uint64_t metadata_offset = osf_header->metadata_offset(); + + if (osf_header->version() < OSF_VERSION::V_2_0) { + // Check flatbuffers osfSession validity [V1] + print_error(filename_, "OSF prior version 2.0 is not supported!"); + return false; + } + + // Check flatbuffers metadata validity + if (!ouster::osf::check_osf_metadata_buf(get_metadata_chunk_ptr(), + size_ - metadata_offset)) { + print_error(filename_, "OSF metadata verification has failed."); + return false; + } + + return true; +} + +// ========= Geneal Data Access ============= + +OsfFile& OsfFile::seek(const uint64_t pos) { + if (!good()) throw std::logic_error("bad osf file"); + if (pos > size_) { + std::stringstream ss; + ss << "seek for " << pos << " but the file size is " << size_; + throw std::out_of_range(ss.str()); + } + if (file_stream_.is_open()) { + file_stream_.seekg(pos); + } + offset_ = pos; + return *this; +} + +OsfFile& OsfFile::read(uint8_t* buf, const uint64_t count) { + // TODO[pb]: Check for errors in full implementation + // and set error flags + // TODO[pb]: Read from disk if it's not mmap? (buffering, etc to be + // considered later) + + // Now we just copy from the mapped region from the current offset + // and advance offset further. + if (!good()) throw std::logic_error("bad osf file"); + if (offset_ + count > size_) { + std::stringstream ss; + ss << "read till " << (offset_ + count) << " but the file size is " + << size_; + throw std::out_of_range(ss.str()); + } + if (file_stream_.is_open()) { + file_stream_.read(reinterpret_cast(buf), count); + offset_ = file_stream_.tellg(); + } else if (file_buf_ != nullptr) { + std::memcpy(buf, file_buf_ + offset_, count); + offset_ += count; + } + return *this; +} + +// ===== Mmapped access to the file content memory ===== + +const uint8_t* OsfFile::buf(const uint64_t offset) const { + if (!good()) throw std::logic_error("bad osf file"); + if (!is_memory_mapped()) throw std::logic_error("not a mmap file"); + if (offset >= size_) + throw std::out_of_range("out of range osf file access"); + return file_buf_ + offset; +} + +bool OsfFile::is_memory_mapped() const { + return file_buf_ != nullptr; +} + +// ======= Helpers ============= + +void OsfFile::error(const std::string& msg) { + state_ = FileState::BAD; + if (!msg.empty()) { + print_error(filename_, msg); + } else { + print_error(filename_, get_last_error()); + } +} + +std::string OsfFile::to_string() { + std::stringstream ss; + ss << "OsfFile [filename = '" << filename_ << "', " + << "state = " << static_cast(state_) << ", " + << "version = " << version() << ", " + << "size = " << size_ << ", offset = " << offset_; + if (this->good()) { + auto osf_header = get_osf_header_from_buf(get_header_chunk_ptr()); + ss << ", osf.file_length = " << osf_header->file_length() << ", " + << "osf.metadata_offset = " << osf_header->metadata_offset() << ", " + << "osf.status = " << static_cast(osf_header->status()); + } + ss << "]"; + return ss.str(); +} + +// ======= Move semantics =================== + +OsfFile::OsfFile(OsfFile&& other) + : filename_(other.filename_), + offset_(other.offset_), + size_(other.size_), + file_buf_(other.file_buf_), + file_stream_(std::move(other.file_stream_)), + header_chunk_(std::move(other.header_chunk_)), + metadata_chunk_(std::move(other.metadata_chunk_)), + state_(other.state_) { + other.file_buf_ = nullptr; + other.state_ = FileState::BAD; +} + +OsfFile& OsfFile::operator=(OsfFile&& other) { + if (this != &other) { + close(); + filename_ = other.filename_; + offset_ = other.offset_; + size_ = other.size_; + file_buf_ = other.file_buf_; + file_stream_ = std::move(other.file_stream_); + header_chunk_ = std::move(other.header_chunk_); + metadata_chunk_ = std::move(other.metadata_chunk_); + state_ = other.state_; + other.file_buf_ = nullptr; + other.state_ = FileState::BAD; + } + return *this; +}; + +// ========= Release resources ================= + +void OsfFile::close() { + if (file_buf_) { + if (!mmap_close(file_buf_, size_)) { + error(); + return; + } + file_buf_ = nullptr; + state_ = FileState::BAD; + } + if (file_stream_.is_open()) { + file_stream_.close(); + if (file_stream_.fail()) { + error(); + return; + } + state_ = FileState::BAD; + } +} + +OsfFile::~OsfFile() { + // Release file memory mapping + close(); +} + +std::shared_ptr OsfFile::read_chunk(const uint64_t offset) { + if (!good()) { + return nullptr; + } + // check whether it was read last and we have it in cache already + if (chunk_cache_offset_ == offset && chunk_cache_) { + return chunk_cache_; + } + auto chunk_buf = std::make_shared(FLATBUFFERS_PREFIX_LENGTH); + seek(offset); + read(chunk_buf->data(), FLATBUFFERS_PREFIX_LENGTH); + uint32_t full_chunk_size = get_prefixed_size(chunk_buf->data()) + + FLATBUFFERS_PREFIX_LENGTH + CRC_BYTES_SIZE; + if (offset + full_chunk_size > size_) { + std::stringstream ss; + ss << "read till " << (offset + full_chunk_size) + << " but the file size is " << size_; + throw std::out_of_range(ss.str()); + } + chunk_buf->resize(full_chunk_size); + read(chunk_buf->data() + FLATBUFFERS_PREFIX_LENGTH, + full_chunk_size - FLATBUFFERS_PREFIX_LENGTH); + + // update cached chunk + if (chunk_cache_) { + chunk_cache_.swap(chunk_buf); + } else { + chunk_cache_ = std::move(chunk_buf); + } + chunk_cache_offset_ = offset; + + return chunk_cache_; +} + +uint8_t* OsfFile::get_header_chunk_ptr() { + if (!file_stream_.good()) { + if (header_chunk_) header_chunk_.reset(); + return nullptr; + } + if (header_chunk_) return header_chunk_->data(); + + auto tmp_offset = offset_; + header_chunk_ = read_chunk(0); + seek(tmp_offset); + return header_chunk_->data(); +} + +uint8_t* OsfFile::get_metadata_chunk_ptr() { + uint64_t meta_offset = metadata_offset(); + if (!file_stream_.good()) { + if (metadata_chunk_) metadata_chunk_.reset(); + return nullptr; + } + if (metadata_chunk_) return metadata_chunk_->data(); + + auto tmp_offset = offset_; + metadata_chunk_ = read_chunk(meta_offset); + seek(tmp_offset); + return metadata_chunk_->data(); +} + +} // namespace OSF +} // namespace ouster \ No newline at end of file diff --git a/ouster_osf/src/json_utils.cpp b/ouster_osf/src/json_utils.cpp new file mode 100644 index 00000000..2a3a9cfa --- /dev/null +++ b/ouster_osf/src/json_utils.cpp @@ -0,0 +1,31 @@ +/** + * Copyright(c) 2021, Ouster, Inc. + * All rights reserved. + */ + +#include "json_utils.h" + +#include + +namespace ouster { +namespace osf { + +bool parse_json(const std::string json_str, Json::Value& output) { + // Parse Json + Json::CharReaderBuilder jbuilder{}; + jbuilder["collectComments"] = false; + std::stringstream source{json_str}; + std::string jerrs; + return Json::parseFromStream(jbuilder, source, &output, &jerrs); +} + +std::string json_string(const Json::Value& root) { + Json::StreamWriterBuilder builder; + builder["enableYAMLCompatibility"] = true; + builder["precision"] = 6; + builder["indentation"] = " "; + return Json::writeString(builder, root); +} + +} // namespace osf +} // namespace ouster diff --git a/ouster_osf/src/json_utils.h b/ouster_osf/src/json_utils.h new file mode 100644 index 00000000..a4da6bb6 --- /dev/null +++ b/ouster_osf/src/json_utils.h @@ -0,0 +1,18 @@ +/** + * Copyright(c) 2021, Ouster, Inc. + * All rights reserved. + */ + +#pragma once + +#include "json/json.h" + +namespace ouster { +namespace osf { + +bool parse_json(const std::string json_str, Json::Value& output); + +std::string json_string(const Json::Value& root); + +} // namespace osf +} // namespace ouster \ No newline at end of file diff --git a/ouster_osf/src/layout_standard.cpp b/ouster_osf/src/layout_standard.cpp new file mode 100644 index 00000000..2aa5da16 --- /dev/null +++ b/ouster_osf/src/layout_standard.cpp @@ -0,0 +1,44 @@ +/** + * Copyright(c) 2021, Ouster, Inc. + * All rights reserved. + */ + +#include "ouster/osf/layout_standard.h" + +#include + +#include "ouster/osf/writer.h" + +namespace ouster { +namespace osf { + +StandardLayoutCW::StandardLayoutCW(Writer& writer, uint32_t chunk_size) + : chunk_size_{chunk_size ? chunk_size : STANDARD_DEFAULT_CHUNK_SIZE}, + writer_{writer} {} + +void StandardLayoutCW::saveMessage(const uint32_t stream_id, const ts_t ts, + const std::vector& msg_buf) { + if (chunk_builder_.size() + msg_buf.size() > chunk_size_) { + finish_chunk(); + } + + chunk_builder_.saveMessage(stream_id, ts, msg_buf); +} + +void StandardLayoutCW::finish_chunk() { + std::vector bb = chunk_builder_.finish(); + if (!bb.empty()) { + writer_.emit_chunk(chunk_builder_.start_ts(), chunk_builder_.end_ts(), + bb); + } + + // Prepare for the new chunk messages + chunk_builder_.reset(); +} + +void StandardLayoutCW::finish() { + finish_chunk(); +} + +} // namespace osf +} // namespace ouster \ No newline at end of file diff --git a/ouster_osf/src/layout_streaming.cpp b/ouster_osf/src/layout_streaming.cpp new file mode 100644 index 00000000..1166b662 --- /dev/null +++ b/ouster_osf/src/layout_streaming.cpp @@ -0,0 +1,84 @@ +/** + * Copyright(c) 2021, Ouster, Inc. + * All rights reserved. + */ + +#include "ouster/osf/layout_streaming.h" + +#include + +#include "ouster/osf/writer.h" +#include "ouster/osf/meta_streaming_info.h" + +namespace ouster { +namespace osf { + +StreamingLayoutCW::StreamingLayoutCW(Writer& writer, uint32_t chunk_size) + : chunk_size_{chunk_size ? chunk_size : STREAMING_DEFAULT_CHUNK_SIZE}, + writer_{writer} {} + +void StreamingLayoutCW::saveMessage(const uint32_t stream_id, const ts_t ts, + const std::vector& msg_buf) { + if (!chunk_builders_.count(stream_id)) { + chunk_builders_.insert({stream_id, std::make_shared()}); + } + + auto chunk_builder = chunk_builders_[stream_id]; + + // checking non-decreasing invariant of chunks and messages + if (chunk_builder->end_ts() > ts ) { + std::stringstream err; + err << "ERROR: Can't write wirh a decreasing timestamp: " << ts.count() + << " for stream_id: " << stream_id + << " ( previous recorded timestamp: " + << chunk_builder->end_ts().count() << ")"; + throw std::logic_error(err.str()); + } + + if (chunk_builder->size() + msg_buf.size() > chunk_size_) { + finish_chunk(stream_id, chunk_builder); + } + + chunk_builder->saveMessage(stream_id, ts, msg_buf); + + // update running statistics per stream + stats_message(stream_id, ts, msg_buf); +} + +void StreamingLayoutCW::stats_message(const uint32_t stream_id, const ts_t ts, + const std::vector& msg_buf) { + auto msg_size = static_cast(msg_buf.size()); + auto stats_it = stream_stats_.find(stream_id); + if (stats_it == stream_stats_.end()) { + stream_stats_.insert({stream_id, StreamStats(stream_id, ts, msg_size)}); + } else { + stats_it->second.update(ts, msg_size); + } +} + +void StreamingLayoutCW::finish_chunk( + uint32_t stream_id, const std::shared_ptr& chunk_builder) { + std::vector bb = chunk_builder->finish(); + if (!bb.empty()) { + uint64_t chunk_offset = writer_.emit_chunk(chunk_builder->start_ts(), + chunk_builder->end_ts(), bb); + chunk_stream_id_.emplace_back( + chunk_offset, ChunkInfo{chunk_offset, stream_id, + chunk_builder->messages_count()}); + } + + // Prepare for the new chunk messages + chunk_builder->reset(); +} + +void StreamingLayoutCW::finish() { + for (auto& cb_it : chunk_builders_) { + finish_chunk(cb_it.first, cb_it.second); + } + + writer_.addMetadata(StreamingInfo{ + chunk_stream_id_, {stream_stats_.begin(), stream_stats_.end()}}); +} + +} // namespace osf +} // namespace ouster \ No newline at end of file diff --git a/ouster_osf/src/meta_extrinsics.cpp b/ouster_osf/src/meta_extrinsics.cpp new file mode 100644 index 00000000..b145b32c --- /dev/null +++ b/ouster_osf/src/meta_extrinsics.cpp @@ -0,0 +1,66 @@ +/** + * Copyright(c) 2021, Ouster, Inc. + * All rights reserved. + */ + +#include "ouster/osf/meta_extrinsics.h" + +#include "flatbuffers/flatbuffers.h" +#include "os_sensor/extrinsics_generated.h" + +namespace ouster { +namespace osf { + +std::vector Extrinsics::buffer() const { + flatbuffers::FlatBufferBuilder fbb = flatbuffers::FlatBufferBuilder(256); + std::vector extrinsic_vec(16); + + // Changing column-major data layout to row-major before storing to OSF buf + // Not using internal mat4d::data() reprensentation here because we + // don't always control how it is created. + for (size_t i = 0; i < 4; i++) { + for (size_t j = 0; j < 4; j++) { + extrinsic_vec[4 * i + j] = extrinsics_(i, j); + } + } + auto ext_offset = osf::gen::CreateExtrinsicsDirect( + fbb, &extrinsic_vec, ref_meta_id_, + name_.empty() ? nullptr : name_.c_str()); + osf::gen::FinishSizePrefixedExtrinsicsBuffer(fbb, ext_offset); + const uint8_t* buf = fbb.GetBufferPointer(); + const uint32_t size = fbb.GetSize(); + return {buf, buf + size}; +}; + +std::unique_ptr Extrinsics::from_buffer( + const std::vector& buf) { + auto ext_fb = gen::GetSizePrefixedExtrinsics(buf.data()); + if (!ext_fb) return nullptr; + std::string name; + if (ext_fb->name()) { + name = ext_fb->name()->str(); + } + mat4d ext_mat = mat4d::Identity(); + if (ext_fb->extrinsics() && ext_fb->extrinsics()->size() == 16) { + for (uint32_t i = 0; i < ext_fb->extrinsics()->size(); ++i) { + uint32_t r = i / 4; + uint32_t c = i % 4; + ext_mat(r, c) = ext_fb->extrinsics()->Get(i); + } + } + return std::make_unique(ext_mat, ext_fb->ref_id(), name); +} + +std::string Extrinsics::repr() const { + std::stringstream ss; + ss << "ExtrinsicsMeta: ref_id = " << ref_meta_id_ << ", name = " << name_ << ", extrinsics ="; + for (size_t i = 0; i < 4; ++i) { + for (size_t j = 0; j < 4; ++j) { + ss << " " << extrinsics_(i, j); + } + } + return ss.str(); +}; + +} // namespace osf +} // namespace ouster diff --git a/ouster_osf/src/meta_lidar_sensor.cpp b/ouster_osf/src/meta_lidar_sensor.cpp new file mode 100644 index 00000000..7ca56cbd --- /dev/null +++ b/ouster_osf/src/meta_lidar_sensor.cpp @@ -0,0 +1,68 @@ +/** + * Copyright(c) 2021, Ouster, Inc. + * All rights reserved. + */ + +#include "ouster/osf/meta_lidar_sensor.h" + +#include "flatbuffers/flatbuffers.h" +#include "ouster/osf/basics.h" + +#include "json_utils.h" +#include "fb_utils.h" + +namespace ouster { +namespace osf { + +// === Lidar Sensor stream/msgs functions ==================== + +flatbuffers::Offset create_lidar_sensor( + flatbuffers::FlatBufferBuilder& fbb, const std::string& sensor_metadata) { + auto ls_offset = + ouster::osf::gen::CreateLidarSensorDirect(fbb, sensor_metadata.c_str()); + return ls_offset; +} + +std::unique_ptr restore_lidar_sensor( + const std::vector buf) { + auto lidar_sensor = v2::GetSizePrefixedLidarSensor(buf.data()); + + std::string sensor_metadata{}; + if (lidar_sensor->metadata()) sensor_metadata = lidar_sensor->metadata()->str(); + + return std::make_unique(sensor_metadata); +} + +std::vector LidarSensor::buffer() const { + flatbuffers::FlatBufferBuilder fbb = flatbuffers::FlatBufferBuilder(32768); + auto ls_offset = create_lidar_sensor(fbb, metadata_); + fbb.FinishSizePrefixed(ls_offset, ouster::osf::gen::LidarSensorIdentifier()); + const uint8_t* buf = fbb.GetBufferPointer(); + const uint32_t size = fbb.GetSize(); + return {buf, buf + size}; +}; + +std::unique_ptr LidarSensor::from_buffer( + const std::vector& buf) { + auto sensor_metadata = restore_lidar_sensor(buf); + if (sensor_metadata) { + return std::make_unique(*sensor_metadata); + } + return nullptr; +} + +std::string LidarSensor::repr() const { + Json::Value lidar_sensor_obj{}; + Json::Value sensor_info_obj{}; + + if (!parse_json(metadata_, sensor_info_obj)) { + lidar_sensor_obj["sensor_info"] = metadata_; + } else { + lidar_sensor_obj["sensor_info"] = sensor_info_obj; + } + + return json_string(lidar_sensor_obj); +}; + +} // namespace osf +} // namespace ouster diff --git a/ouster_osf/src/meta_streaming_info.cpp b/ouster_osf/src/meta_streaming_info.cpp new file mode 100644 index 00000000..5758b329 --- /dev/null +++ b/ouster_osf/src/meta_streaming_info.cpp @@ -0,0 +1,146 @@ +/** + * Copyright(c) 2021, Ouster, Inc. + * All rights reserved. + */ + +#include "ouster/osf/meta_streaming_info.h" + +#include +#include + +#include "json/json.h" + +#include "ouster/osf/meta_streaming_info.h" +#include "streaming/streaming_info_generated.h" + +#include "json_utils.h" + +namespace ouster { +namespace osf { + +std::string to_string(ChunkInfo chunk_info) { + std::stringstream ss; + ss << "{offset = " << chunk_info.offset + << ", stream_id = " << chunk_info.stream_id + << ", message_count = " << chunk_info.message_count + << "}"; + return ss.str(); +} + +std::string to_string(const StreamStats& stream_stats) { + std::stringstream ss; + ss << "{stream_id = " << stream_stats.stream_id + << ", start_ts = " << stream_stats.start_ts.count() + << ", end_ts = " << stream_stats.end_ts.count() + << ", message_count = " << stream_stats.message_count + << ", message_avg_size = " << stream_stats.message_avg_size << "}"; + return ss.str(); +} + +flatbuffers::Offset create_streaming_info( + flatbuffers::FlatBufferBuilder& fbb, + const std::map& chunks_info, + const std::map& stream_stats) { + + // Pack chunks vector + std::vector> chunks_info_vec; + for (const auto& chunk_info : chunks_info) { + const auto& ci = chunk_info.second; + auto ci_offset = gen::CreateChunkInfo(fbb, ci.offset, ci.stream_id, + ci.message_count); + chunks_info_vec.push_back(ci_offset); + } + + // Pack stream_stats vector + std::vector> stream_stats_vec; + for (const auto& stream_stat : stream_stats) { + auto stat = stream_stat.second; + auto ss_offset = gen::CreateStreamStats( + fbb, stat.stream_id, stat.start_ts.count(), stat.end_ts.count(), + stat.message_count, stat.message_avg_size); + stream_stats_vec.push_back(ss_offset); + } + + auto si_offset = ouster::osf::gen::CreateStreamingInfoDirect( + fbb, &chunks_info_vec, &stream_stats_vec); + return si_offset; +} + +std::vector StreamingInfo::buffer() const { + flatbuffers::FlatBufferBuilder fbb = flatbuffers::FlatBufferBuilder(32768); + auto si_offset = create_streaming_info(fbb, chunks_info_, stream_stats_); + fbb.FinishSizePrefixed(si_offset); + const uint8_t* buf = fbb.GetBufferPointer(); + const size_t size = fbb.GetSize(); + return {buf, buf + size}; +}; + +std::unique_ptr StreamingInfo::from_buffer( + const std::vector& buf) { + + auto streaming_info = gen::GetSizePrefixedStreamingInfo(buf.data()); + + std::unique_ptr si = std::make_unique(); + + auto& chunks_info = si->chunks_info(); + if (streaming_info->chunks() && streaming_info->chunks()->size()) { + std::transform( + streaming_info->chunks()->begin(), streaming_info->chunks()->end(), + std::inserter(chunks_info, chunks_info.end()), + [](const gen::ChunkInfo* ci) { + return std::make_pair(ci->offset(), + ChunkInfo{ci->offset(), ci->stream_id(), + ci->message_count()}); + }); + } + + auto& stream_stats = si->stream_stats(); + if (streaming_info->stream_stats() && + streaming_info->stream_stats()->size()) { + std::transform(streaming_info->stream_stats()->begin(), + streaming_info->stream_stats()->end(), + std::inserter(stream_stats, stream_stats.end()), + [](const gen::StreamStats* stat) { + StreamStats ss{}; + ss.stream_id = stat->stream_id(); + ss.start_ts = ts_t{stat->start_ts()}; + ss.end_ts = ts_t{stat->end_ts()}; + ss.message_count = stat->message_count(); + ss.message_avg_size = stat->message_avg_size(); + return std::make_pair(stat->stream_id(), ss); + }); + } + + return si; +} + +std::string StreamingInfo::repr() const { + Json::Value si_obj{}; + + si_obj["chunks"] = Json::arrayValue; + for (const auto& ci : chunks_info_) { + Json::Value chunk_info{}; + chunk_info["offset"] = static_cast(ci.second.offset); + chunk_info["stream_id"] = ci.second.stream_id; + chunk_info["message_count"] = ci.second.message_count; + si_obj["chunks"].append(chunk_info); + } + + si_obj["stream_stats"] = Json::arrayValue; + for (const auto& stat : stream_stats_) { + Json::Value ss{}; + ss["stream_id"] = stat.first; + ss["start_ts"] = + static_cast(stat.second.start_ts.count()); + ss["end_ts"] = static_cast(stat.second.end_ts.count()); + ss["message_count"] = + static_cast(stat.second.message_count); + ss["message_avg_size"] = stat.second.message_avg_size; + si_obj["stream_stats"].append(ss); + } + + return json_string(si_obj); +}; + +} // namespace osf +} // namespace ouster \ No newline at end of file diff --git a/ouster_osf/src/metadata.cpp b/ouster_osf/src/metadata.cpp new file mode 100644 index 00000000..90d3a720 --- /dev/null +++ b/ouster_osf/src/metadata.cpp @@ -0,0 +1,94 @@ +/** + * Copyright(c) 2021, Ouster, Inc. + * All rights reserved. + */ + +#include "ouster/osf/metadata.h" +#include "fb_utils.h" + +namespace ouster { +namespace osf { + +std::string MetadataEntry::repr() const { + auto b = this->buffer(); + std::stringstream ss; + ss << "MetadataEntry: " + << (b.size() ? osf::to_string(b.data(), b.size(), 50) : ""); + return ss.str(); +}; + +std::string MetadataEntry::to_string() const { + std::stringstream ss; + ss << "MetadataEntry: [" + << "id = " << id() << ", type = " << type() << ", buffer = {" + << this->repr() << "}" + << "]"; + return ss.str(); +} + +flatbuffers::Offset MetadataEntry::make_entry( + flatbuffers::FlatBufferBuilder& fbb) const { + auto buf = this->buffer(); + return ouster::osf::gen::CreateMetadataEntryDirect( + fbb, id(), type().c_str(), &buf); +} + +std::unique_ptr MetadataEntry::from_buffer( + const std::vector& buf, const std::string type_str) { + 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; + return nullptr; + } + auto m = registered_type->second(buf); + if (m == nullptr) { + std::cout << "UNRECOVERABLE FROM BUFFER as type: " << type_str + << std::endl; + return nullptr; + } + return m; +}; + +std::map& +MetadataEntry::get_registry() { + static std::map registry_; + return registry_; +} + +std::vector MetadataEntryRef::buffer() const { + const gen::MetadataEntry* meta_entry = + reinterpret_cast(buf_); + return vector_from_fb_vector(meta_entry->buffer()); +} + +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; + return nullptr; + } + auto m = registered_type->second(buffer()); + if (m == nullptr) { + std::cout << "UNRECOVERABLE FROM BUFFER: " << to_string() << std::endl; + return nullptr; + } + m->setId(id()); + return m; +} + +std::vector> +MetadataStore::make_entries(flatbuffers::FlatBufferBuilder& fbb) const { + using FbEntriesVector = + std::vector>; + FbEntriesVector entries; + for (const auto& md : metadata_entries_) { + auto entry_offset = md.second->make_entry(fbb); + entries.push_back(entry_offset); + } + return entries; +} + +} // namespace osf +} // namespace ouster \ No newline at end of file diff --git a/ouster_osf/src/operations.cpp b/ouster_osf/src/operations.cpp new file mode 100644 index 00000000..563b9407 --- /dev/null +++ b/ouster_osf/src/operations.cpp @@ -0,0 +1,231 @@ +/** + * Copyright(c) 2021, Ouster, Inc. + * All rights reserved. + */ + +#include "ouster/osf/operations.h" + +#include +#include +#include + +#include "fb_utils.h" +#include "json/json.h" +#include "json_utils.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" + +namespace ouster { +namespace osf { + +std::string dump_metadata(const std::string& file, bool full) { + OsfFile osf_file(file); + auto osf_header = get_osf_header_from_buf(osf_file.get_header_chunk_ptr()); + + Json::Value root{}; + + root["header"]["size"] = static_cast(osf_file.size()); + root["header"]["version"] = static_cast(osf_file.version()); + root["header"]["status"] = to_string(osf_header->status()); + root["header"]["metadata_offset"] = + static_cast(osf_file.metadata_offset()); + root["header"]["chunks_offset"] = + static_cast(osf_file.chunks_offset()); + + Reader reader(file); + + root["metadata"]["id"] = reader.id(); + root["metadata"]["start_ts"] = + static_cast(reader.start_ts().count()); + root["metadata"]["end_ts"] = + static_cast(reader.end_ts().count()); + + auto osf_metadata = + get_osf_metadata_from_buf(osf_file.get_metadata_chunk_ptr()); + + if (full) { + root["metadata"]["chunks"] = Json::arrayValue; + for (size_t i = 0; i < osf_metadata->chunks()->size(); ++i) { + auto osf_chunk = osf_metadata->chunks()->Get(i); + Json::Value chunk{}; + chunk["start_ts"] = + static_cast(osf_chunk->start_ts()); + chunk["end_ts"] = static_cast(osf_chunk->end_ts()); + chunk["offset"] = static_cast(osf_chunk->offset()); + root["metadata"]["chunks"].append(chunk); + } + } + + const MetadataStore& meta_store = reader.meta_store(); + + root["metadata"]["entries"] = Json::arrayValue; + + for (const auto& me : meta_store.entries()) { + Json::Value meta_element{}; + meta_element["id"] = static_cast(me.first); + meta_element["type"] = me.second->type(); + + if (full) { + const std::string me_str = me.second->repr(); + Json::Value me_obj{}; + if (parse_json(me_str, me_obj)) { + meta_element["buffer"] = me_obj; + } else { + meta_element["buffer"] = me_str; + } + } + + root["metadata"]["entries"].append(meta_element); + } + + return json_string(root); +} + +void parse_and_print(const std::string& file, bool with_decoding) { + OsfFile osf_file{file}; + + using ouster::osf::LidarScanStream; + using ouster::osf::LidarSensor; + + std::cout << "OSF v2:" << std::endl; + std::cout << " file = " << osf_file.to_string() << std::endl; + + ouster::osf::Reader reader(osf_file); + + int ls_c = 0; + int other_c = 0; + + // TODO[pb]: Remove the SIGINT handlers from C++ wrapped function used in + // Python bindings + // https://pybind11.readthedocs.io/en/stable/faq.html#how-can-i-properly-handle-ctrl-c-in-long-running-functions + thread_local std::atomic_bool quit{false}; + auto sig = std::signal(SIGINT, [](int) { quit = true; }); + + for (const auto msg : reader.messages_standard()) { + if (msg.is()) { + std::cout << " Ls ts: " << msg.ts().count() + << ", stream_id = " << msg.id(); + ++ls_c; + + // Example of code to get an object + if (with_decoding) { + auto obj_ls = msg.decode_msg(); + if (obj_ls) { + std::cout << " [D]"; + } + } + + std::cout << std::endl; + + } else { + // UNKNOWN Message + std::cout << " UK ts: " << msg.ts().count() + << ", stream_id = " << msg.id() << std::endl; + ++other_c; + } + + if (quit) { + std::cout << "Stopped early via SIGINT!" << std::endl; + break; + } + } + + // restore signal handler + std::signal(SIGINT, sig); + + std::cout << "\nSUMMARY (OSF v2): \n"; + std::cout << " lidar_scan (Ls) count = " << ls_c << std::endl; + std::cout << " other (NOT IMPLEMENTED) count = " << other_c << std::endl; +} + +bool pcap_to_osf(const std::string& pcap_filename, + const std::string& meta_filename, int lidar_port, + const std::string& osf_filename, int chunk_size) { + std::cout << "Converting: " << std::endl + << " PCAP file: " << pcap_filename << std::endl + << " with json file: " << meta_filename << std::endl + << " to OSF file: " << osf_filename << std::endl + << " chunk_size: " + << (chunk_size ? std::to_string(chunk_size) : "DEFAULT") + << std::endl; + + PcapRawSource pcap_source{pcap_filename}; + + std::string sensor_metadata = read_text_file(meta_filename); + + auto info = sensor::parse_metadata(sensor_metadata); + + std::cout << "Using sensor data:\n" + << " lidar_port = " << lidar_port << std::endl; + + std::cout << "Processing PCAP packects to OSF messages "; + + Writer writer{osf_filename, "ouster-cli osf from_pcap", + static_cast(chunk_size)}; + + std::cout << "(chunk_size: " << writer.chunk_size() << "): ..." + << std::endl; + + auto field_types = get_field_types(info); + + // Overwrite field_types for Legacy UDP profile, so to reduce the LidarScan + // encoding sizes (saves about ~15% of disk/bandwidth) + if (info.format.udp_profile_lidar == + sensor::UDPProfileLidar::PROFILE_LIDAR_LEGACY) { + field_types.clear(); + field_types.emplace_back(sensor::ChanField::RANGE, + sensor::ChanFieldType::UINT32); + field_types.emplace_back(sensor::ChanField::SIGNAL, + sensor::ChanFieldType::UINT16); + field_types.emplace_back(sensor::ChanField::REFLECTIVITY, + sensor::ChanFieldType::UINT16); + field_types.emplace_back(sensor::ChanField::NEAR_IR, + sensor::ChanFieldType::UINT16); + } + std::cout << "LidarScan field_types: " << ouster::to_string(field_types) + << std::endl; + + auto sensor_meta_id = writer.addMetadata(sensor_metadata); + auto ls_stream = + writer.createStream(sensor_meta_id, field_types); + + int ls_cnt = 0; + + if (lidar_port > 0) { + pcap_source.addLidarDataHandler( + lidar_port, info, + [&ls_cnt, &ls_stream](const osf::ts_t ts, const LidarScan& ls) { + ls_cnt++; + ls_stream.save(ts, ls); + }); + } + + // TODO[pb]: Remove the SIGINT handlers from C++ wrapped function used in + // Python bindings + // https://pybind11.readthedocs.io/en/stable/faq.html#how-can-i-properly-handle-ctrl-c-in-long-running-functions + thread_local std::atomic_bool quit{false}; + auto sig = std::signal(SIGINT, [](int) { quit = true; }); + + pcap_source.runWhile( + [](const sensor_utils::packet_info&) { return !quit; }); + + // restore signal handler + std::signal(SIGINT, sig); + + writer.close(); + + std::cout << "Saved to OSF file:" << std::endl + << " Lidar Scan messages: " << ls_cnt << std::endl; + + return true; +} + +} // namespace osf +} // namespace ouster + diff --git a/ouster_osf/src/pcap_source.cpp b/ouster_osf/src/pcap_source.cpp new file mode 100644 index 00000000..aa80778b --- /dev/null +++ b/ouster_osf/src/pcap_source.cpp @@ -0,0 +1,79 @@ +/** + * 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 new file mode 100644 index 00000000..f0d08e19 --- /dev/null +++ b/ouster_osf/src/png_tools.cpp @@ -0,0 +1,1411 @@ +/** + * Copyright(c) 2021, Ouster, Inc. + * All rights reserved. + */ + +#include "png_tools.h" + +#include + +#include +#include +#include +#include +#include + +#include "ouster/lidar_scan.h" + +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) + * - comp level 2: 38s, 643M + * - comp level 3: 45s, 639M + * - comp level 4: 48s, 590M (47% speedup vs default, <1% size increase) + * - comp level 5: 61s, 589M + * - libpng default: 98s, 586M + * - comp level 9: 328s, 580M + * + * TODO: investigate other zlib options + */ +static constexpr int PNG_OSF_ZLIB_COMPRESSION_LEVEL = 4; + +/** + * Provides the data reader capabilities from std::vector for png_read IO + */ +struct VectorReader { + const std::vector& buffer; + uint32_t offset; + explicit VectorReader(const std::vector& buf) + : buffer(buf), offset(0) {} + void read(void* bytes, const uint32_t bytes_len) { + // Skip safety check and trust libpng? + if (offset >= buffer.size()) return; + uint32_t bytes_to_read = bytes_len; + if (offset + bytes_to_read > buffer.size()) { + bytes_to_read = buffer.size() - offset; + } + std::memcpy(bytes, buffer.data() + offset, bytes_to_read); + offset += bytes_to_read; + } +}; + +/** + * 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; + longjmp(png_jmpbuf(png_ptr), 1); +}; + +/** + * Custom png_write handler to write data to std::vector buffer + */ +void png_osf_write_data(png_structp png_ptr, png_bytep bytes, + png_size_t bytes_len) { + std::vector* res_buf = + reinterpret_cast*>(png_get_io_ptr(png_ptr)); + res_buf->insert(res_buf->end(), reinterpret_cast(bytes), + reinterpret_cast(bytes + bytes_len)); +}; + +/** + * Custom png_read handler to read data from std::vector (via VectorReader + * helper) + */ +void png_osf_read_data(png_structp png_ptr, png_bytep bytes, + png_size_t bytes_len) { + VectorReader* vec_read = + reinterpret_cast(png_get_io_ptr(png_ptr)); + vec_read->read(bytes, bytes_len); +}; + +// void user_read_data(png_structp png_ptr, png_bytep data, png_size_t length); + +/** + * It's needed for custom png IO operations... but I've never seen it's called. + * And also there are no need to flush writer to std::vector buufer in our case. + */ +void png_osf_flush_data(png_structp){}; + +/** + * Common png WRITE init routine, creates and setups png_ptr and png_info_ptr + */ +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"; + return true; + } + + *png_info_ptrp = png_create_info_struct(*png_ptrp); + if (!*png_info_ptrp) { + std::cout << "ERROR: no png_info_ptr\n"; + png_destroy_write_struct(png_ptrp, nullptr); + return true; + } + + return false; // SUCCESS +} + +/** + * Common png READ init routine, creates and setups png_ptr and png_info_ptr + */ +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"; + return true; + } + + *png_info_ptrp = png_create_info_struct(*png_ptrp); + if (!*png_info_ptrp) { + std::cout << "ERROR: no png_info_ptr\n"; + png_destroy_read_struct(png_ptrp, nullptr, nullptr); + return true; + } + + return false; // SUCCESS +} + +/** + * Common png write setup routine. + * Write destination is res_buf of std::vector type. + */ +void png_osf_write_start(png_structp png_ptr, png_infop png_info_ptr, + ScanChannelData& res_buf, const uint32_t width, + const uint32_t height, const int sample_depth, + const int color_type) { + // Use setjmp() on upper level for errors catching + png_set_write_fn(png_ptr, &res_buf, png_osf_write_data, png_osf_flush_data); + + png_set_compression_level(png_ptr, PNG_OSF_ZLIB_COMPRESSION_LEVEL); + + png_set_IHDR(png_ptr, png_info_ptr, width, height, sample_depth, color_type, + PNG_INTERLACE_NONE, PNG_COMPRESSION_TYPE_DEFAULT, + PNG_FILTER_TYPE_DEFAULT); + + png_write_info(png_ptr, png_info_ptr); +} + +// ========== Encode Functions =================================== + +ScanData scanEncodeFieldsSingleThread(const LidarScan& lidar_scan, + const std::vector& px_offset, + const LidarScanFieldTypes& field_types) { + // Prepare scan data of size that fits all field_types we are about to + // encode + ScanData fields_data(field_types.size()); + + size_t scan_idx = 0; + for (const auto& f : field_types) { + fieldEncode(lidar_scan, f, px_offset, fields_data, scan_idx); + scan_idx += 1; + } + + return fields_data; +} + +ScanData scanEncodeFields(const LidarScan& lidar_scan, + const std::vector& px_offset, + const LidarScanFieldTypes& field_types) { + // Prepare scan data of size that fits all field_types we are about to + // encode + ScanData fields_data(field_types.size()); + + unsigned int con_num = std::thread::hardware_concurrency(); + // looking for at least 4 cores if can't determine + if (!con_num) con_num = 4; + + const size_t fields_num = field_types.size(); + // Number of fields to pack into a single thread coder + size_t per_thread_num = (fields_num + con_num - 1) / con_num; + std::vector coders{}; + size_t scan_idx = 0; + for (size_t t = 0; t < con_num && t * per_thread_num < fields_num; ++t) { + // Per every thread we pack the `per_thread_num` field_types encodings + // job + const size_t start_idx = t * per_thread_num; + // Fields list for a thread to encode + LidarScanFieldTypes thread_fields{}; + // Scan indices for the corresponding fields where result will be stored + 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_idxs.push_back(scan_idx); + scan_idx += 1; + } + // Start an encoder thread with selected fields and corresponding + // indices list + coders.emplace_back( + std::thread{fieldEncodeMulti, std::cref(lidar_scan), + thread_fields, std::cref(px_offset), + std::ref(fields_data), thread_idxs}); + } + + for (auto& t : coders) t.join(); + + return fields_data; +} + +template +bool encode8bitImage(ScanChannelData& res_buf, + const Eigen::Ref>& img, + const std::vector& px_offset) { + return encode8bitImage(res_buf, destagger(img, px_offset)); +} + +template bool encode8bitImage(ScanChannelData&, + const Eigen::Ref>&, + const std::vector&); +template bool encode8bitImage( + ScanChannelData&, const Eigen::Ref>&, + const std::vector&); +template bool encode8bitImage( + ScanChannelData&, const Eigen::Ref>&, + const std::vector&); +template bool encode8bitImage( + ScanChannelData&, const Eigen::Ref>&, + const std::vector&); + +template +bool encode8bitImage(ScanChannelData& res_buf, + const Eigen::Ref>& img) { + const uint32_t width = static_cast(img.cols()); + const uint32_t height = static_cast(img.rows()); + + // 8 bit Gray + const int sample_depth = 8; + const int color_type = PNG_COLOR_TYPE_GRAY; + + // 8bit Encoding Sizes + std::vector row_data(width); // Gray, 8bit + + // libpng main structs + png_structp png_ptr; + png_infop png_info_ptr; + + if (png_osf_write_init(&png_ptr, &png_info_ptr)) { + return true; + } + + if (setjmp(png_jmpbuf(png_ptr))) { + png_destroy_write_struct(&png_ptr, &png_info_ptr); + return true; + } + + png_osf_write_start(png_ptr, png_info_ptr, res_buf, width, height, + sample_depth, color_type); + + for (size_t u = 0; u < height; ++u) { + for (size_t v = 0; v < width; ++v) { + // 8bit Encoding Logic + row_data[v] = static_cast(img(u, v)); + } + + png_write_row(png_ptr, + reinterpret_cast(row_data.data())); + } + + png_write_end(png_ptr, nullptr); + + png_destroy_write_struct(&png_ptr, &png_info_ptr); + + return false; // SUCCESS +} + +template bool encode8bitImage( + ScanChannelData&, const Eigen::Ref>&); +template bool encode8bitImage( + ScanChannelData&, const Eigen::Ref>&); +template bool encode8bitImage( + ScanChannelData&, const Eigen::Ref>&); +template bool encode8bitImage( + ScanChannelData&, const Eigen::Ref>&); + + +template +bool encode16bitImage(ScanChannelData& res_buf, + const Eigen::Ref>& img) { + const uint32_t width = static_cast(img.cols()); + const uint32_t height = static_cast(img.rows()); + + // 16 bit Gray + const int sample_depth = 16; + const int color_type = PNG_COLOR_TYPE_GRAY; + + // 16bit Encoding Sizes + std::vector row_data(width * 2); // Gray, 16bit + + // libpng main structs + png_structp png_ptr; + png_infop png_info_ptr; + + if (png_osf_write_init(&png_ptr, &png_info_ptr)) { + return true; + } + + if (setjmp(png_jmpbuf(png_ptr))) { + png_destroy_write_struct(&png_ptr, &png_info_ptr); + return true; + } + + png_osf_write_start(png_ptr, png_info_ptr, res_buf, width, height, + sample_depth, color_type); + + // Needed to transform provided little-endian samples to internal + // PNG big endian format + png_set_swap(png_ptr); + + for (size_t u = 0; u < height; ++u) { + for (size_t v = 0; v < width; ++v) { + const uint64_t key_val = img(u, v); + + // 16bit Encoding Logic + row_data[v * 2] = static_cast(key_val & 0xff); + row_data[v * 2 + 1] = static_cast((key_val >> 8u) & 0xff); + } + + png_write_row(png_ptr, + reinterpret_cast(row_data.data())); + } + + png_write_end(png_ptr, nullptr); + + png_destroy_write_struct(&png_ptr, &png_info_ptr); + + return false; // SUCCESS +} + +template bool encode16bitImage( + ScanChannelData&, const Eigen::Ref>&); +template bool encode16bitImage( + ScanChannelData&, const Eigen::Ref>&); +template bool encode16bitImage( + ScanChannelData&, const Eigen::Ref>&); +template bool encode16bitImage( + ScanChannelData&, const Eigen::Ref>&); + + +template +bool encode16bitImage(ScanChannelData& res_buf, + const Eigen::Ref>& img, + const std::vector& px_offset) { + return encode16bitImage(res_buf, destagger(img, px_offset)); +} + +template bool encode16bitImage(ScanChannelData&, + const Eigen::Ref>&, + const std::vector&); +template bool encode16bitImage( + ScanChannelData&, const Eigen::Ref>&, + const std::vector&); +template bool encode16bitImage( + ScanChannelData&, const Eigen::Ref>&, + const std::vector&); +template bool encode16bitImage( + ScanChannelData&, const Eigen::Ref>&, + const std::vector&); + +template +bool encode24bitImage(ScanChannelData& res_buf, + const Eigen::Ref>& img, + const std::vector& px_offset) { + return encode24bitImage(res_buf, destagger(img, px_offset)); +} + +template bool encode24bitImage(ScanChannelData&, + const Eigen::Ref>&, + const std::vector&); +template bool encode24bitImage( + ScanChannelData&, const Eigen::Ref>&, + const std::vector&); +template bool encode24bitImage( + ScanChannelData&, const Eigen::Ref>&, + const std::vector&); +template bool encode24bitImage( + ScanChannelData&, const Eigen::Ref>&, + const std::vector&); + +template +bool encode24bitImage(ScanChannelData& res_buf, + const Eigen::Ref>& img) { + const uint32_t width = static_cast(img.cols()); + const uint32_t height = static_cast(img.rows()); + + // 8bit RGB + const int sample_depth = 8; + const int color_type = PNG_COLOR_TYPE_RGB; + + // 24bit Encoding Sizes + std::vector row_data(width * 3); // RGB, 8bit + + // libpng main structs + png_structp png_ptr; + png_infop png_info_ptr; + + if (png_osf_write_init(&png_ptr, &png_info_ptr)) { + return true; + } + + if (setjmp(png_jmpbuf(png_ptr))) { + png_destroy_write_struct(&png_ptr, &png_info_ptr); + return true; + } + + png_osf_write_start(png_ptr, png_info_ptr, res_buf, width, height, + sample_depth, color_type); + + for (size_t u = 0; u < height; ++u) { + for (size_t v = 0; v < width; ++v) { + const uint64_t key_val = img(u, v); + + // 24bit Encoding Logic + row_data[v * 3 + 0] = static_cast(key_val & 0xff); + row_data[v * 3 + 1] = static_cast((key_val >> 8u) & 0xff); + row_data[v * 3 + 2] = static_cast((key_val >> 16u) & 0xff); + } + + png_write_row(png_ptr, + reinterpret_cast(row_data.data())); + } + + png_write_end(png_ptr, nullptr); + + png_destroy_write_struct(&png_ptr, &png_info_ptr); + + return false; // SUCCESS +} + +template bool encode24bitImage( + ScanChannelData&, const Eigen::Ref>&); +template bool encode24bitImage( + ScanChannelData&, const Eigen::Ref>&); +template bool encode24bitImage( + ScanChannelData&, const Eigen::Ref>&); +template bool encode24bitImage( + ScanChannelData&, const Eigen::Ref>&); + +template +bool encode32bitImage(ScanChannelData& res_buf, + const Eigen::Ref>& img, + const std::vector& px_offset) { + return encode32bitImage(res_buf, destagger(img, px_offset)); +} + +template bool encode32bitImage(ScanChannelData&, + const Eigen::Ref>&, + const std::vector&); +template bool encode32bitImage( + ScanChannelData&, const Eigen::Ref>&, + const std::vector&); +template bool encode32bitImage( + ScanChannelData&, const Eigen::Ref>&, + const std::vector&); +template bool encode32bitImage( + ScanChannelData&, const Eigen::Ref>&, + const std::vector&); + +template +bool encode32bitImage(ScanChannelData& res_buf, + const Eigen::Ref>& img) { + const uint32_t width = static_cast(img.cols()); + const uint32_t height = static_cast(img.rows()); + + // 8bit RGBA + const int sample_depth = 8; + const int color_type = PNG_COLOR_TYPE_RGB_ALPHA; + + // 32bit Encoding Sizes + std::vector row_data(width * 4); // RGBA, 8bit + + // libpng main structs + png_structp png_ptr; + png_infop png_info_ptr; + + if (png_osf_write_init(&png_ptr, &png_info_ptr)) { + return true; + } + + if (setjmp(png_jmpbuf(png_ptr))) { + png_destroy_write_struct(&png_ptr, &png_info_ptr); + return true; + } + + png_osf_write_start(png_ptr, png_info_ptr, res_buf, width, height, + sample_depth, color_type); + + for (size_t u = 0; u < height; ++u) { + for (size_t v = 0; v < width; ++v) { + const uint64_t key_val = img(u, v); + + // 32bit Encoding Logic + row_data[v * 4 + 0] = static_cast(key_val & 0xff); + row_data[v * 4 + 1] = static_cast((key_val >> 8u) & 0xff); + row_data[v * 4 + 2] = static_cast((key_val >> 16u) & 0xff); + row_data[v * 4 + 3] = static_cast((key_val >> 24u) & 0xff); + } + + png_write_row(png_ptr, + reinterpret_cast(row_data.data())); + } + + png_write_end(png_ptr, nullptr); + + png_destroy_write_struct(&png_ptr, &png_info_ptr); + + return false; // SUCCESS +} + +template bool encode32bitImage( + ScanChannelData&, const Eigen::Ref>&); +template bool encode32bitImage( + ScanChannelData&, const Eigen::Ref>&); +template bool encode32bitImage( + ScanChannelData&, const Eigen::Ref>&); +template bool encode32bitImage( + ScanChannelData&, const Eigen::Ref>&); + + +template +bool encode64bitImage(ScanChannelData& res_buf, + const Eigen::Ref>& img, + const std::vector& px_offset) { + return encode64bitImage(res_buf, destagger(img, px_offset)); +} + +template bool encode64bitImage(ScanChannelData&, + const Eigen::Ref>&, + const std::vector&); +template bool encode64bitImage( + ScanChannelData&, const Eigen::Ref>&, + const std::vector&); +template bool encode64bitImage( + ScanChannelData&, const Eigen::Ref>&, + const std::vector&); +template bool encode64bitImage( + ScanChannelData&, const Eigen::Ref>&, + const std::vector&); + + +template +bool encode64bitImage(ScanChannelData& res_buf, + const Eigen::Ref>& img) { + const uint32_t width = static_cast(img.cols()); + const uint32_t height = static_cast(img.rows()); + + // 16bit RGBA + const int sample_depth = 16; + const int color_type = PNG_COLOR_TYPE_RGB_ALPHA; + + // 64bit Encoding Sizes + std::vector row_data(width * 8); // RGBA, 16bit + + // libpng main structs + png_structp png_ptr; + png_infop png_info_ptr; + + if (png_osf_write_init(&png_ptr, &png_info_ptr)) { + return true; + } + + if (setjmp(png_jmpbuf(png_ptr))) { + png_destroy_write_struct(&png_ptr, &png_info_ptr); + return true; + } + + png_osf_write_start(png_ptr, png_info_ptr, res_buf, width, height, + sample_depth, color_type); + + // Needed to transform provided little-endian samples to internal + // PNG big endian format + png_set_swap(png_ptr); + + for (size_t u = 0; u < height; ++u) { + for (size_t v = 0; v < width; ++v) { + const uint64_t key_val = img(u, v); + + // 64bit Encoding Logic + row_data[v * 8 + 0] = static_cast(key_val & 0xff); + row_data[v * 8 + 1] = static_cast((key_val >> 8u) & 0xff); + row_data[v * 8 + 2] = static_cast((key_val >> 16u) & 0xff); + row_data[v * 8 + 3] = static_cast((key_val >> 24u) & 0xff); + row_data[v * 8 + 4] = static_cast((key_val >> 32u) & 0xff); + row_data[v * 8 + 5] = static_cast((key_val >> 40u) & 0xff); + row_data[v * 8 + 6] = static_cast((key_val >> 48u) & 0xff); + row_data[v * 8 + 7] = static_cast((key_val >> 56u) & 0xff); + } + + png_write_row(png_ptr, + reinterpret_cast(row_data.data())); + } + + png_write_end(png_ptr, nullptr); + + png_destroy_write_struct(&png_ptr, &png_info_ptr); + + return false; // SUCCESS +} + +template bool encode64bitImage( + ScanChannelData&, const Eigen::Ref>&); +template bool encode64bitImage( + ScanChannelData&, const Eigen::Ref>&); +template bool encode64bitImage( + ScanChannelData&, const Eigen::Ref>&); +template bool encode64bitImage( + ScanChannelData&, const Eigen::Ref>&); + + +bool fieldEncodeMulti( + const LidarScan& lidar_scan, + const LidarScanFieldTypes& field_types, + const std::vector& px_offset, + ScanData& scan_data, const std::vector& scan_idxs) { + if (field_types.size() != scan_idxs.size()) { + std::cerr << "ERROR: in fieldEncodeMulti field_types.size() should " + "match scan_idxs.size()" + << std::endl; + std::abort(); + } + auto res_err = false; + for (size_t i = 0; i < field_types.size(); ++i) { + 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; + } + res_err = res_err || err; + } + return res_err; +} + +bool fieldEncode( + const LidarScan& lidar_scan, + const std::pair field_type, + const std::vector& px_offset, + ScanData& scan_data, size_t scan_idx) { + if (scan_idx >= scan_data.size()) { + std::cerr << "ERROR: scan_data size is not sufficient to hold idx: " + << scan_idx << std::endl; + std::abort(); + } + bool res = true; + switch (field_type.second) { + case sensor::ChanFieldType::UINT8: + res = encode8bitImage(scan_data[scan_idx], + lidar_scan.field(field_type.first), + px_offset); + break; + case sensor::ChanFieldType::UINT16: + res = encode16bitImage(scan_data[scan_idx], + lidar_scan.field(field_type.first), + px_offset); + break; + case sensor::ChanFieldType::UINT32: + res = encode32bitImage(scan_data[scan_idx], + lidar_scan.field(field_type.first), + px_offset); + break; + case sensor::ChanFieldType::UINT64: + res = encode64bitImage(scan_data[scan_idx], + lidar_scan.field(field_type.first), + px_offset); + break; + default: + std::cerr << "ERROR: fieldEncode: UNKNOWN: ChanFieldType not yet " + "implemented" + << std::endl; + break; + } + if (res) { + std::cerr << "ERROR: fieldEncode: Can't encode field " + << sensor::to_string(field_type.first) << std::endl; + } + return res; +} + +ScanData scanEncode(const LidarScan& lidar_scan, + const std::vector& px_offset) { +#ifdef OUSTER_OSF_NO_THREADING + return scanEncodeFieldsSingleThread(lidar_scan, px_offset, + {lidar_scan.begin(), lidar_scan.end()}); +#else + return scanEncodeFields(lidar_scan, px_offset, + {lidar_scan.begin(), lidar_scan.end()}); +#endif +} + +// ========== Decode Functions =================================== + +bool scanDecode(LidarScan& lidar_scan, const ScanData& scan_data, + const std::vector& px_offset) { +#ifdef OUSTER_OSF_NO_THREADING + return scanDecodeFieldsSingleThread(lidar_scan, scan_data, px_offset); +#else + return scanDecodeFields(lidar_scan, scan_data, px_offset); +#endif +} + +bool fieldDecode( + LidarScan& lidar_scan, const ScanData& scan_data, size_t start_idx, + 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), + scan_data[start_idx], px_offset); + case sensor::ChanFieldType::UINT32: + return decode32bitImage( + lidar_scan.field(field_type.first), + scan_data[start_idx], px_offset); + case sensor::ChanFieldType::UINT64: + 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; + return true; + } + return true; // ERROR +} + +bool fieldDecodeMulti(LidarScan& lidar_scan, const ScanData& scan_data, + const std::vector& scan_idxs, + const LidarScanFieldTypes& field_types, + const std::vector& px_offset) { + if (field_types.size() != scan_idxs.size()) { + std::cerr << "ERROR: in fieldDecodeMulti field_types.size() should " + "match scan_idxs.size()" + << std::endl; + std::abort(); + } + auto res_err = false; + for (size_t i = 0; i < field_types.size(); ++i) { + 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; + } + res_err = res_err || err; + } + return res_err; +} + +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()); + 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; + 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; + return true; + } + ++next_idx; + } + return false; +} + +bool scanDecodeFields(LidarScan& lidar_scan, const ScanData& scan_data, + const std::vector& px_offset) { + LidarScanFieldTypes field_types(lidar_scan.begin(), lidar_scan.end()); + 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; + return true; + } + + unsigned int con_num = std::thread::hardware_concurrency(); + // looking for at least 4 cores if can't determine + if (!con_num) con_num = 4; + + // Number of fields to pack into a single thread coder + size_t per_thread_num = (fields_num + con_num - 1) / con_num; + std::vector coders{}; + size_t scan_idx = 0; + + for (size_t t = 0; t < con_num && t * per_thread_num < fields_num; ++t) { + // Per every thread we pack the `per_thread_num` field_types encodings + // job + const size_t start_idx = t * per_thread_num; + // Fields list for a thread to encode + LidarScanFieldTypes thread_fields{}; + // Scan indices for the corresponding fields where result will be stored + 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_idxs.push_back(scan_idx); + scan_idx += 1; // for UINT64 can be 2 (NOT IMPLEMENTED YET) + } + + // Start a decoder thread with selected fields and corresponding + // indices list + + coders.emplace_back(std::thread{fieldDecodeMulti, std::ref(lidar_scan), + std::cref(scan_data), thread_idxs, + thread_fields, std::cref(px_offset)}); + } + + for (auto& t : coders) t.join(); + + return false; +} + +template +bool decode24bitImage(Eigen::Ref> img, + const ScanChannelData& channel_buf, + const std::vector& px_offset) { + if (!decode24bitImage(img, channel_buf)) { + img = stagger(img, px_offset); + return false; // SUCCESS + } + return true; // ERROR +} + +template bool decode24bitImage(Eigen::Ref>, + const ScanChannelData&, + const std::vector&); +template bool decode24bitImage(Eigen::Ref>, + const ScanChannelData&, + const std::vector&); +template bool decode24bitImage(Eigen::Ref>, + const ScanChannelData&, + const std::vector&); +template bool decode24bitImage(Eigen::Ref>, + const ScanChannelData&, + const std::vector&); + +template +bool decode24bitImage(Eigen::Ref> img, + const ScanChannelData& channel_buf) { + // libpng main structs + png_structp png_ptr; + png_infop png_info_ptr; + + if (png_osf_read_init(&png_ptr, &png_info_ptr)) { + return true; + } + + if (setjmp(png_jmpbuf(png_ptr))) { + png_destroy_read_struct(&png_ptr, &png_info_ptr, nullptr); + return true; + } + + VectorReader channel_reader(channel_buf); + png_set_read_fn(png_ptr, &channel_reader, png_osf_read_data); + + // only used with Gray 16 bit to get little-endian LSB representation back + // for other color types PNG_TRANSFORM_SWAP_ENDIAN does nothing + int transforms = PNG_TRANSFORM_SWAP_ENDIAN; + png_read_png(png_ptr, png_info_ptr, transforms, nullptr); + + png_uint_32 width; + png_uint_32 height; + int sample_depth; + int color_type; + + png_get_IHDR(png_ptr, png_info_ptr, &width, &height, &sample_depth, + &color_type, nullptr, nullptr, nullptr); + + png_bytepp row_pointers = png_get_rows(png_ptr, png_info_ptr); + + // 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; + return true; + } + + if (sample_depth != 8) { + std::cout << "ERROR: encoded img contains data with incompatible " + "sample_depth: " + << sample_depth << ", expected: 8" << std::endl; + 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; + return true; + } + + // 24bit channel data decoding to LidarScan for key channel_index + for (size_t u = 0; u < height; u++) { + for (size_t v = 0; v < width; v++) { + img(u, v) = static_cast(row_pointers[u][v * 3 + 0]) + + (static_cast(row_pointers[u][v * 3 + 1]) << 8u) + + (static_cast(row_pointers[u][v * 3 + 2]) << 16u); + } + } + + png_destroy_read_struct(&png_ptr, &png_info_ptr, nullptr); + + return false; // SUCCESS +} + +template bool decode24bitImage(Eigen::Ref>, + const ScanChannelData&); +template bool decode24bitImage(Eigen::Ref>, + const ScanChannelData&); +template bool decode24bitImage(Eigen::Ref>, + const ScanChannelData&); +template bool decode24bitImage(Eigen::Ref>, + const ScanChannelData&); + +template +bool decode32bitImage(Eigen::Ref> img, + const ScanChannelData& channel_buf, + const std::vector& px_offset) { + if (!decode32bitImage(img, channel_buf)) { + img = stagger(img, px_offset); + return false; // SUCCESS + } + return true; // ERROR +} + +template bool decode32bitImage(Eigen::Ref>, + const ScanChannelData&, + const std::vector&); +template bool decode32bitImage(Eigen::Ref>, + const ScanChannelData&, + const std::vector&); +template bool decode32bitImage(Eigen::Ref>, + const ScanChannelData&, + const std::vector&); +template bool decode32bitImage(Eigen::Ref>, + const ScanChannelData&, + const std::vector&); + +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; + } + // libpng main structs + png_structp png_ptr; + png_infop png_info_ptr; + + if (png_osf_read_init(&png_ptr, &png_info_ptr)) { + return true; + } + + if (setjmp(png_jmpbuf(png_ptr))) { + png_destroy_read_struct(&png_ptr, &png_info_ptr, nullptr); + return true; + } + + VectorReader channel_reader(channel_buf); + png_set_read_fn(png_ptr, &channel_reader, png_osf_read_data); + + // only used with Gray 16 bit to get little-endian LSB representation back + // for other color types PNG_TRANSFORM_SWAP_ENDIAN does nothing + int transforms = PNG_TRANSFORM_SWAP_ENDIAN; + png_read_png(png_ptr, png_info_ptr, transforms, nullptr); + + png_uint_32 width; + png_uint_32 height; + int sample_depth; + int color_type; + + png_get_IHDR(png_ptr, png_info_ptr, &width, &height, &sample_depth, + &color_type, nullptr, nullptr, nullptr); + + png_bytepp row_pointers = png_get_rows(png_ptr, png_info_ptr); + + // 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; + return true; + } + + if (sample_depth != 8) { + std::cout << "ERROR: encoded img contains data with incompatible " + "sample_depth: " + << sample_depth << ", expected: 8" << std::endl; + 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; + return true; + } + + // 32bit channel data decoding to LidarScan for key channel_index + for (size_t u = 0; u < height; u++) { + for (size_t v = 0; v < width; v++) { + img(u, v) = static_cast(row_pointers[u][v * 4 + 0]) + + (static_cast(row_pointers[u][v * 4 + 1]) << 8u) + + (static_cast(row_pointers[u][v * 4 + 2]) << 16u) + + (static_cast(row_pointers[u][v * 4 + 3]) << 24u); + } + } + + png_destroy_read_struct(&png_ptr, &png_info_ptr, nullptr); + + return false; // SUCCESS +} + +template bool decode32bitImage(Eigen::Ref>, + const ScanChannelData&); +template bool decode32bitImage(Eigen::Ref>, + const ScanChannelData&); +template bool decode32bitImage(Eigen::Ref>, + const ScanChannelData&); +template bool decode32bitImage(Eigen::Ref>, + const ScanChannelData&); + +template +bool decode64bitImage(Eigen::Ref> img, + const ScanChannelData& channel_buf, + const std::vector& px_offset) { + if (!decode64bitImage(img, channel_buf)) { + img = stagger(img, px_offset); + return false; // SUCCESS + } + return true; // ERROR +} + +template bool decode64bitImage(Eigen::Ref>, + const ScanChannelData&, + const std::vector&); +template bool decode64bitImage(Eigen::Ref>, + const ScanChannelData&, + const std::vector&); +template bool decode64bitImage(Eigen::Ref>, + const ScanChannelData&, + const std::vector&); +template bool decode64bitImage(Eigen::Ref>, + const ScanChannelData&, + const std::vector&); + +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; + } + // libpng main structs + png_structp png_ptr; + png_infop png_info_ptr; + + if (png_osf_read_init(&png_ptr, &png_info_ptr)) { + return true; + } + + if (setjmp(png_jmpbuf(png_ptr))) { + png_destroy_read_struct(&png_ptr, &png_info_ptr, nullptr); + return true; + } + + VectorReader channel_reader(channel_buf); + png_set_read_fn(png_ptr, &channel_reader, png_osf_read_data); + + // only used with Gray 16 bit to get little-endian LSB representation back + // for other color types PNG_TRANSFORM_SWAP_ENDIAN does nothing + int transforms = PNG_TRANSFORM_SWAP_ENDIAN; + png_read_png(png_ptr, png_info_ptr, transforms, nullptr); + + png_uint_32 width; + png_uint_32 height; + int sample_depth; + int color_type; + + png_get_IHDR(png_ptr, png_info_ptr, &width, &height, &sample_depth, + &color_type, nullptr, nullptr, nullptr); + + png_bytepp row_pointers = png_get_rows(png_ptr, png_info_ptr); + + // 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; + return true; + } + + if (sample_depth != 16) { + std::cout << "ERROR: encoded img contains data with incompatible " + "sample_depth: " + << sample_depth << ", expected: 16" << std::endl; + 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; + return true; + } + + // 64bit channel data decoding to LidarScan for key channel_index + for (size_t u = 0; u < height; u++) { + for (size_t v = 0; v < width; v++) { + uint64_t val = + static_cast(row_pointers[u][v * 8 + 0]) + + (static_cast(row_pointers[u][v * 8 + 1]) << 8u) + + (static_cast(row_pointers[u][v * 8 + 2]) << 16u) + + (static_cast(row_pointers[u][v * 8 + 3]) << 24u) + + (static_cast(row_pointers[u][v * 8 + 4]) << 32u) + + (static_cast(row_pointers[u][v * 8 + 5]) << 40u) + + (static_cast(row_pointers[u][v * 8 + 6]) << 48u) + + (static_cast(row_pointers[u][v * 8 + 7]) << 56u); + img(u, v) = static_cast(val); + } + } + + png_destroy_read_struct(&png_ptr, &png_info_ptr, nullptr); + + return false; // SUCCESS +} + +template bool decode64bitImage(Eigen::Ref>, + const ScanChannelData&); +template bool decode64bitImage(Eigen::Ref>, + const ScanChannelData&); +template bool decode64bitImage(Eigen::Ref>, + const ScanChannelData&); +template bool decode64bitImage(Eigen::Ref>, + const ScanChannelData&); + +template +bool decode16bitImage(Eigen::Ref> img, + const ScanChannelData& channel_buf, + const std::vector& px_offset) { + if (!decode16bitImage(img, channel_buf)) { + img = stagger(img, px_offset); + return false; // SUCCESS + } + return true; // ERROR +} + +template bool decode16bitImage(Eigen::Ref>, + const ScanChannelData&, + const std::vector&); +template bool decode16bitImage(Eigen::Ref>, + const ScanChannelData&, + const std::vector&); +template bool decode16bitImage(Eigen::Ref>, + const ScanChannelData&, + const std::vector&); +template bool decode16bitImage(Eigen::Ref>, + const ScanChannelData&, + const std::vector&); + +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; + } + // libpng main structs + png_structp png_ptr; + png_infop png_info_ptr; + + if (png_osf_read_init(&png_ptr, &png_info_ptr)) { + return true; + } + + if (setjmp(png_jmpbuf(png_ptr))) { + png_destroy_read_struct(&png_ptr, &png_info_ptr, nullptr); + return true; + } + + VectorReader channel_reader(channel_buf); + png_set_read_fn(png_ptr, &channel_reader, png_osf_read_data); + + // only used with Gray 16 bit to get little-endian LSB representation back + // for other color types PNG_TRANSFORM_SWAP_ENDIAN does nothing + int transforms = PNG_TRANSFORM_SWAP_ENDIAN; + png_read_png(png_ptr, png_info_ptr, transforms, nullptr); + + png_uint_32 width; + png_uint_32 height; + int sample_depth; + int color_type; + + png_get_IHDR(png_ptr, png_info_ptr, &width, &height, &sample_depth, + &color_type, nullptr, nullptr, nullptr); + + png_bytepp row_pointers = png_get_rows(png_ptr, png_info_ptr); + + // 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; + return true; + } + + if (sample_depth != 16) { + std::cout << "ERROR: encoded img contains data with incompatible " + "sample_depth: " + << sample_depth << ", expected: 16" << std::endl; + 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; + return true; + } + + // 16bit channel data decoding to LidarScan for key channel_index + for (size_t u = 0; u < height; u++) { + for (size_t v = 0; v < width; v++) { + img(u, v) = static_cast(row_pointers[u][v * 2 + 0]) + + (static_cast(row_pointers[u][v * 2 + 1]) << 8u); + } + } + + png_destroy_read_struct(&png_ptr, &png_info_ptr, nullptr); + + return false; // SUCCESS +} + +template bool decode16bitImage(Eigen::Ref>, + const ScanChannelData&); +template bool decode16bitImage(Eigen::Ref>, + const ScanChannelData&); +template bool decode16bitImage(Eigen::Ref>, + const ScanChannelData&); +template bool decode16bitImage(Eigen::Ref>, + const ScanChannelData&); + +template +bool decode8bitImage(Eigen::Ref> img, + const ScanChannelData& channel_buf, + const std::vector& px_offset) { + if (!decode8bitImage(img, channel_buf)) { + img = stagger(img, px_offset); + return false; // SUCCESS + } + return true; // ERROR +} + +template bool decode8bitImage(Eigen::Ref>, + const ScanChannelData&, + const std::vector&); +template bool decode8bitImage(Eigen::Ref>, + const ScanChannelData&, + const std::vector&); +template bool decode8bitImage(Eigen::Ref>, + const ScanChannelData&, + const std::vector&); +template bool decode8bitImage(Eigen::Ref>, + const ScanChannelData&, + const std::vector&); + +template +bool decode8bitImage(Eigen::Ref> img, + const ScanChannelData& channel_buf) { + // libpng main structs + png_structp png_ptr; + png_infop png_info_ptr; + + if (png_osf_read_init(&png_ptr, &png_info_ptr)) { + return true; + } + + if (setjmp(png_jmpbuf(png_ptr))) { + png_destroy_read_struct(&png_ptr, &png_info_ptr, nullptr); + return true; + } + + VectorReader channel_reader(channel_buf); + png_set_read_fn(png_ptr, &channel_reader, png_osf_read_data); + + // only used with Gray 16 bit to get little-endian LSB representation back + // for other color types PNG_TRANSFORM_SWAP_ENDIAN does nothing + int transforms = PNG_TRANSFORM_SWAP_ENDIAN; + png_read_png(png_ptr, png_info_ptr, transforms, nullptr); + + png_uint_32 width; + png_uint_32 height; + int sample_depth; + int color_type; + + png_get_IHDR(png_ptr, png_info_ptr, &width, &height, &sample_depth, + &color_type, nullptr, nullptr, nullptr); + + png_bytepp row_pointers = png_get_rows(png_ptr, png_info_ptr); + + // 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; + return true; + } + + if (sample_depth != 8) { + std::cout << "ERROR: encoded img contains data with incompatible " + "sample_depth: " + << sample_depth << ", expected: 16" << std::endl; + 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; + return true; + } + + // 16bit channel data decoding to LidarScan for key channel_index + for (size_t u = 0; u < height; u++) { + for (size_t v = 0; v < width; v++) { + img(u, v) = row_pointers[u][v]; + } + } + + png_destroy_read_struct(&png_ptr, &png_info_ptr, nullptr); + + return false; // SUCCESS +} + +template bool decode8bitImage(Eigen::Ref>, + const ScanChannelData&); +template bool decode8bitImage(Eigen::Ref>, + const ScanChannelData&); +template bool decode8bitImage(Eigen::Ref>, + const ScanChannelData&); +template bool decode8bitImage(Eigen::Ref>, + const ScanChannelData&); + +// =================== Save to File Functions ==================== + +bool saveScanChannel(const ScanChannelData& channel_buf, + const std::string& filename) { + std::fstream file(filename, std::ios_base::out | std::ios_base::binary); + + if (file.good()) { + file.write(reinterpret_cast(channel_buf.data()), + channel_buf.size()); + if (file.good()) { + file.close(); + return false; // SUCCESS + } + } + + file.close(); + return true; // FAILURE +} + +} // namespace OSF +} // namespace ouster diff --git a/ouster_osf/src/png_tools.h b/ouster_osf/src/png_tools.h new file mode 100644 index 00000000..6dcdadf5 --- /dev/null +++ b/ouster_osf/src/png_tools.h @@ -0,0 +1,366 @@ +/** + * Copyright(c) 2021, Ouster, Inc. + * All rights reserved. + */ + +#pragma once + +#include +#include + +#include "ouster/lidar_scan.h" +#include "os_sensor/lidar_scan_stream_generated.h" + +namespace ouster { +namespace osf { + + +// Encoded single PNG buffer +using ScanChannelData = std::vector; + +// Encoded PNG buffers +using ScanData = std::vector; + +// FieldTypes container +using LidarScanFieldTypes = + std::vector>; + +/** + * libpng only versions for Encode/Decode LidarScan to PNG buffers + */ + +// ========== Decode Functions =================================== + +/** + * Decode the PNG buffers into LidarScan object. This is a dispatch function to + * the specific decoding functions. + * + * @param lidar_scan the output object that will be filled as a result of + * decoding + * @param scan_data PNG buffers to decode + * @param px_offset pixel shift per row used to reconstruct staggered range + * image form + * @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); + +/// 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); + +/// Decoding eUDP LidarScan, multithreaded version +bool scanDecodeFields(LidarScan& lidar_scan, const ScanData& scan_data, + const std::vector& px_offset); + +/** + * Decode a single field to lidar_scan + * + * @param lidar_scan the output object that will be filled as a result of + * decoding + * @param scan_data PNG buffers to decode + * @param scan_idx index in `scan_data` of the beginning of field buffers + * @param field_type the field of `lidar_scan` to fill in with the docoded + * result + * @param px_offset pixel shift per row used to reconstruct staggered range + * image form + * @return false (0) if operation is successful true (1) if error occured + */ +bool fieldDecode( + LidarScan& lidar_scan, const ScanData& scan_data, size_t scan_idx, + const std::pair field_type, + const std::vector& px_offset); + +/** + * Decode multiple fields to lidar_scan + * + * @param lidar_scan the output object that will be filled as a result of + * decoding + * @param scan_data PNG buffers to decode, sequentially in the order of + * field_types + * @param scan_idxs a vector of indices in `scan_data` of the beginning of field + * buffers that will be decoded. + * `field_types.size()` should be equal to `scan_idxs.size()` + * i.e. we need to provide the index for every field type + * in field_types where it's encoded data located. + * @param field_types a vector of filed_types of lidar scan to decode + * @param px_offset pixel shift per row used to reconstruct staggered range + * image form + * @return false (0) if operation is successful true (1) if error occured + */ +bool fieldDecodeMulti(LidarScan& lidar_scan, const ScanData& scan_data, + const std::vector& scan_idxs, + const LidarScanFieldTypes& field_types, + const std::vector& px_offset); + +template +bool decode8bitImage(Eigen::Ref> img, + const ScanChannelData& channel_buf); + +template +bool decode8bitImage(Eigen::Ref> img, + const ScanChannelData& channel_buf, + const std::vector& px_offset); + +/** + * Decode single PNG buffer (channel_buf) of 16 bit Gray encoding into + * img + * + * @param img the output img that will be filled as a result of + * decoding + * @param channel_buf single PNG buffer to decode + * @param px_offset pixel shift per row used to reconstruct staggered range + * image form + * @return false (0) if operation is successful true (1) if error occured + */ +template +bool decode16bitImage(Eigen::Ref> img, + const ScanChannelData& channel_buf, + const std::vector& px_offset); + +template +bool decode16bitImage(Eigen::Ref> img, + const ScanChannelData& channel_buf); + +/** + * Decode single PNG buffer (channel_buf) of 24 bit RGB (8 bit) encoding into + * img object. + * + * @param img the output img that will be filled as a result of + * decoding + * @param channel_buf single PNG buffer to decode + * @param px_offset pixel shift per row used to reconstruct staggered range + * image form + * @return false (0) if operation is successful true (1) if error occured + */ +template +bool decode24bitImage(Eigen::Ref> img, + const ScanChannelData& channel_buf, + const std::vector& px_offset); + +template +bool decode24bitImage(Eigen::Ref> img, + const ScanChannelData& channel_buf); + + +/** + * Decode single PNG buffer (channel_buf) of 32 bit RGBA (8 bit) encoding into + * img object. + * + * @param img the output img that will be filled as a result of + * decoding + * @param channel_buf single PNG buffer to decode + * @param px_offset pixel shift per row used to reconstruct staggered range + * image form + * @return false (0) if operation is successful true (1) if error occured + */ +template +bool decode32bitImage(Eigen::Ref> img, + const ScanChannelData& channel_buf, + const std::vector& px_offset); + +template +bool decode32bitImage(Eigen::Ref> img, + const ScanChannelData& channel_buf); + +/** + * Decode single PNG buffer (channel_buf) of 64 bit RGBA (16 bit) encoding into + * img object. + * + * @param img the output img that will be filled as a result of + * decoding + * @param channel_buf single PNG buffer to decode + * @param px_offset pixel shift per row used to reconstruct staggered range + * image form + * @return false (0) if operation is successful true (1) if error occured + */ +template +bool decode64bitImage(Eigen::Ref> img, + const ScanChannelData& channel_buf, + const std::vector& px_offset); + +template +bool decode64bitImage(Eigen::Ref> img, + const ScanChannelData& channel_buf); + +// ========== Encode Functions =================================== + +/** + * Encode LidarScan to PNG buffers storing all field_types present in an object. + * + * @param lidar_scan the LidarScan object to encode + * @param px_offset pixel shift per row used to destaggered LidarScan data + * @return encoded PNG buffers, empty() if error occured. + */ +ScanData scanEncode(const LidarScan& lidar_scan, + const std::vector& px_offset); + +/** + * Encode the lidar scan fields to PNGs channel buffers (ScanData). + * Single-threaded implementation. + * + * @param lidar_scan a lidar scan object to encode + * @param px_offset pixel shift per row used to construct de-staggered range + * image form + * @return encoded PNGs in ScanData in order of field_types + */ +ScanData scanEncodeFieldsSingleThread(const LidarScan& lidar_scan, + const std::vector& px_offset, + const LidarScanFieldTypes& field_types); + +/** + * Encode the lidar scan fields to PNGs channel buffers (ScanData). + * Multi-threaded implementation. + * + * @param lidar_scan a lidar scan object to encode + * @param px_offset pixel shift per row used to construct de-staggered range + * image form + * @return encoded PNGs in ScanData in order of field_types + */ +ScanData scanEncodeFields(const LidarScan& lidar_scan, + const std::vector& px_offset, + const LidarScanFieldTypes& field_types); + +/** + * Encode a single lidar scan field to PNGs channel buffer and place it to a + * specified `scan_data[scan_idx]` place + * + * @param lidar_scan a lidar scan object to encode + * @param field_type a filed_type of lidar scan to encode + * @param px_offset pixel shift per row used to construct de-staggered range + * image form + * @param scan_data channel buffers storage for the encoded lidar_scan + * @param scan_idx index in `scan_data` of the beginning of field buffers where + * the result of encoding will be inserted + * @return false (0) if operation is successful true (1) if error occured + */ +bool fieldEncode( + const LidarScan& lidar_scan, + const std::pair field_type, + const std::vector& px_offset, + ScanData& scan_data, size_t scan_idx); + +/** + * Encode multiple lidar scan fields to PNGs channel buffers and insert them to a + * specified places `scan_idxs` in `scan_data`. + * + * @param lidar_scan a lidar scan object to encode + * @param field_types a vector of filed_types of lidar scan to encode + * @param px_offset pixel shift per row used to construct de-staggered range + * image form + * @param scan_data channel buffers storage for the encoded lidar_scan + * @param scan_idxs a vector of indices in `scan_data` of the beginning of field + * buffers where the result of encoding will be inserted. + * `field_types.size()` should be equal to `scan_idxs.size()` + * @return false (0) if operation is successful true (1) if error occured + */ +bool fieldEncodeMulti( + const LidarScan& lidar_scan, + const LidarScanFieldTypes& field_types, + const std::vector& px_offset, + ScanData& scan_data, const std::vector& scan_idxs); + +template +bool encode8bitImage(ScanChannelData& res_buf, + const Eigen::Ref>& img, + const std::vector& px_offset); + +template +bool encode8bitImage(ScanChannelData& res_buf, + const Eigen::Ref>& img); + +/** + * Encode img object into a 16 bit, Gray, PNG buffer. + * + * @param res_buf the output buffer with a single encoded PNG + * @param img the image object to encode + * @param px_offset pixel shift per row used to destagger img data + * @return false (0) if operation is successful, true (1) if error occured + */ +template +bool encode16bitImage(ScanChannelData& res_buf, + const Eigen::Ref>& img, + const std::vector& px_offset); + +/** + * Encode 2D image of a typical lidar scan field channel into a 16 bit, Gray, + * PNG buffer. + * + * @param res_buf the output buffer with a single encoded PNG + * @param img 2D image or a single LidarScan field data + * @return false (0) if operation is successful, true (1) if error occured + */ +template +bool encode16bitImage(ScanChannelData& res_buf, + const Eigen::Ref>& img); + +/** + * Encode 2D image of a typical lidar scan field channel into a 32 bit, RGBA, + * PNG buffer. + * + * @param res_buf the output buffer with a single encoded PNG + * @param img 2D image or a single LidarScan field data + * @param px_offset pixel shift per row used to destagger img data + * @return false (0) if operation is successful, true (1) if error occured + */ +template +bool encode32bitImage(ScanChannelData& res_buf, + const Eigen::Ref>& img, + const std::vector& px_offset); + +template +bool encode32bitImage(ScanChannelData& res_buf, + const Eigen::Ref>& img); + +/** + * Encode 2D image of a typical lidar scan field channel into a 24 bit, RGB, + * PNG buffer. + * + * @param res_buf the output buffer with a single encoded PNG + * @param img 2D image or a single LidarScan field data + * @param px_offset pixel shift per row used to destagger img data + * @return false (0) if operation is successful, true (1) if error occured + */ +template +bool encode24bitImage(ScanChannelData& res_buf, + const Eigen::Ref>& img, + const std::vector& px_offset); + +template +bool encode24bitImage(ScanChannelData& res_buf, + const Eigen::Ref>& img); + +/** + * Encode 2D image of a typical lidar scan field channel into a 64 bit, RGBA, + * PNG buffer. + * + * @param res_buf the output buffer with a single encoded PNG + * @param img 2D image or a single LidarScan field data + * @param px_offset pixel shift per row used to destagger img data + * @return false (0) if operation is successful, true (1) if error occured + */ +template +bool encode64bitImage(ScanChannelData& res_buf, + const Eigen::Ref>& img, + const std::vector& px_offset); + +template +bool encode64bitImage(ScanChannelData& res_buf, + const Eigen::Ref>& img); + +// =================== Save to File Functions ==================== + +/** + * Save PNG encoded scan channel buffer to the PNG file. + * + * @param channel_buf single PNG buffer to decode + * @param filename file name of output PNG image + * @return false (0) if operation is successful, true (1) if error occured + */ +bool saveScanChannel(const ScanChannelData& channel_buf, + const std::string& filename); + +} // namespace osf +} // namespace ouster diff --git a/ouster_osf/src/reader.cpp b/ouster_osf/src/reader.cpp new file mode 100644 index 00000000..c816b6e3 --- /dev/null +++ b/ouster_osf/src/reader.cpp @@ -0,0 +1,1129 @@ +/** + * Copyright(c) 2021, Ouster, Inc. + * All rights reserved. + */ + +#include "ouster/osf/reader.h" + +#include +#include + +#include "fb_utils.h" +#include "ouster/osf/basics.h" +#include "ouster/osf/crc32.h" +#include "ouster/osf/file.h" +#include "ouster/osf/meta_streaming_info.h" +#include "ouster/osf/metadata.h" +#include "ouster/types.h" + +namespace ouster { +namespace osf { + +namespace { + +inline const ouster::osf::v2::Chunk* get_chunk_from_buf(const uint8_t* buf) { + return ouster::osf::v2::GetSizePrefixedChunk(buf); +} + +} // namespace + +// ======================================================= +// =========== ChunksPile ================================ +// ======================================================= + +void ChunksPile::add(uint64_t offset, ts_t start_ts, ts_t end_ts) { + ChunkState cs{}; + cs.offset = offset; + cs.next_offset = std::numeric_limits::max(); + cs.start_ts = start_ts; + cs.end_ts = end_ts; + cs.status = ChunkValidity::UNKNOWN; + pile_[offset] = cs; +} + +void ChunksPile::add_info(uint64_t offset, uint32_t stream_id, + uint32_t message_count) { + auto chunk_state = get(offset); + if (chunk_state == nullptr) { + // allowing adding info on chunks that already present with + // a corresponding chunk states + return; + } + ChunkInfoNode ci{}; + ci.offset = chunk_state->offset; + ci.next_offset = std::numeric_limits::max(); + ci.stream_id = stream_id; + ci.message_count = message_count; + pile_info_[offset] = ci; +} + +ChunkState* ChunksPile::get(uint64_t offset) { + auto cit = pile_.find(offset); + if (cit == pile_.end()) { + return nullptr; + } + return &cit->second; +} + +ChunkInfoNode* ChunksPile::get_info(uint64_t offset) { + auto cit = pile_info_.find(offset); + if (cit == pile_info_.end()) { + return nullptr; + } + return &cit->second; +} + +ChunkInfoNode* ChunksPile::get_info_by_message_idx(uint32_t stream_id, + uint32_t message_idx) { + if (!has_message_idx()) return nullptr; + + auto schunks = stream_chunks_.find(stream_id); + if (schunks == stream_chunks_.end()) return nullptr; + + // out of bounds + auto lci = get_info(schunks->second->back()); + if (message_idx >= lci->message_start_idx + lci->message_count) + return nullptr; + + auto lb = std::lower_bound( + schunks->second->begin(), schunks->second->end(), message_idx, + [&](uint64_t a, uint32_t m_idx) { + const auto* ci = get_info(a); + return ci->message_start_idx + ci->message_count - 1 < m_idx; + }); + + return get_info(*lb); +} + +ChunkState* ChunksPile::get_by_lower_bound_ts(uint32_t stream_id, + const ts_t ts) { + auto schunks = stream_chunks_.find(stream_id); + if (schunks == stream_chunks_.end()) return nullptr; + auto lb_offset = std::lower_bound( + schunks->second->begin(), schunks->second->end(), ts, + [&](uint64_t a, const ts_t t) { return get(a)->end_ts < t; }); + if (lb_offset == schunks->second->end()) return nullptr; + return get(*lb_offset); +} + +ChunkState* ChunksPile::next(uint64_t offset) { + auto chunk = get(offset); + if (!chunk) return nullptr; + return get(chunk->next_offset); +} + +ChunkState* ChunksPile::next_by_stream(uint64_t offset) { + auto chunk_info = get_info(offset); + if (!chunk_info) return nullptr; + return get(chunk_info->next_offset); +} + +ChunkState* ChunksPile::first() { return get(0); } + +ChunksPile::ChunkStateIter ChunksPile::begin() { + return pile_.begin(); +} + +ChunksPile::ChunkStateIter ChunksPile::end() { + return pile_.end(); +} + +size_t ChunksPile::size() const { return pile_.size(); } + +bool ChunksPile::has_info() const { + return !pile_info_.empty() && pile_info_.size() == pile_.size(); +} + +bool ChunksPile::has_message_idx() const { + // rely on the fact that message_count in the ChunkInfo, if present + // during Writing/Chunk building, can't be 0 (by construction in + // ChunkBuilder and StreamingLayoutCW) + // In other words we can't have Chunks with 0 messages written to OSF + // file + return has_info() && pile_info_.begin()->second.message_count > 0; +} + +void ChunksPile::link_stream_chunks() { + // This function does a couple of things: + // 1. Fills the stream_chunks_ map with offsets of chunks per stream id + // 2, Links ChunkInfoNode from pile_info_ to the linked list by offsets + // so the traverse laterally along the same stream_id is easy + // 3. Fills message_start_idx in ChunksInfoNode by counting previous chunks + // message_counts per stream + // Thus the resulting lateral links between ChunkInfoNode allows traversing + // between chunks per stream_id and quick search to the chunk by message_idx + + stream_chunks_.clear(); + + if (has_info()) { + // Do the next_offset links by streams + auto curr_chunk = first(); + while (curr_chunk != nullptr) { + auto ci = get_info(curr_chunk->offset); + if (ci == nullptr) { + throw std::logic_error("ERROR: Have a missing chunk info"); + } + if (stream_chunks_.count(ci->stream_id)) { + // verifying ts of prev and current chunks on non-decreasing + // invariant. + auto prev_chunk_offset = stream_chunks_[ci->stream_id]->back(); + auto prev_cs = get(prev_chunk_offset); + if (prev_cs->end_ts > curr_chunk->start_ts) { + throw std::logic_error( + "ERROR: Can't have decreasing by timestamp chunks " + "order in StreamingLayout"); + } + // get prev chunk info and update next_offset + auto prev_ci = get_info(prev_chunk_offset); + prev_ci->next_offset = curr_chunk->offset; + ci->message_start_idx = + prev_ci->message_start_idx + prev_ci->message_count; + stream_chunks_[ci->stream_id]->push_back(curr_chunk->offset); + } else { + stream_chunks_.insert( + {ci->stream_id, std::make_shared>( + 1, curr_chunk->offset)}); + } + curr_chunk = get(curr_chunk->next_offset); + } + } +} + +std::string to_string(const ChunkState& chunk_state) { + std::stringstream ss; + ss << "{offset = " << chunk_state.offset + << ", next_offset = " << chunk_state.next_offset + << ", start_ts = " << chunk_state.start_ts.count() + << ", end_ts = " << chunk_state.end_ts.count() + << ", status = " << (int)chunk_state.status << "}"; + return ss.str(); +} + +std::string to_string(const ChunkInfoNode& chunk_info) { + std::stringstream ss; + ss << "{offset = " << chunk_info.offset + << ", next_offset = " << chunk_info.next_offset + << ", stream_id = " << chunk_info.stream_id + << ", message_count = " << chunk_info.message_count + << ", message_start_idx = " << chunk_info.message_start_idx << "}"; + return ss.str(); +} + +// ========================================================== +// ========= Reader::ChunksIter ============================= +// ========================================================== + +ChunksIter::ChunksIter() + : current_addr_(0), end_addr_(0), reader_(nullptr) {} + +ChunksIter::ChunksIter(const ChunksIter& other) + : current_addr_(other.current_addr_), + end_addr_(other.end_addr_), + reader_(other.reader_) {} + +ChunksIter::ChunksIter(const uint64_t begin_addr, const uint64_t end_addr, + Reader* reader) + : current_addr_(begin_addr), end_addr_(end_addr), reader_(reader) { + if (current_addr_ != end_addr_ && !is_cleared()) next(); +} + +const ChunkRef ChunksIter::operator*() const { + if (current_addr_ == end_addr_) { + throw std::logic_error("ERROR: Can't dereference end iterator."); + } + return ChunkRef(current_addr_, reader_); +} + +const std::unique_ptr ChunksIter::operator->() const { + return std::make_unique(current_addr_, reader_); +} + +ChunksIter& ChunksIter::operator++() { + this->next(); + return *this; +} + +ChunksIter ChunksIter::operator++(int) { + auto res = *this; + this->next(); + return res; +} + +void ChunksIter::next() { + if (current_addr_ == end_addr_) return; + next_any(); + while (current_addr_ != end_addr_ && !is_cleared()) next_any(); +} + +void ChunksIter::next_any() { + if (current_addr_ == end_addr_) return; + auto next_chunk = reader_->chunks_.next(current_addr_); + if (next_chunk) { + current_addr_ = next_chunk->offset; + } else { + current_addr_ = end_addr_; + } +} + +bool ChunksIter::is_cleared() { + if (current_addr_ == end_addr_) return false; + return reader_->verify_chunk(current_addr_); +} + +bool ChunksIter::operator==(const ChunksIter& other) const { + return (current_addr_ == other.current_addr_ && + end_addr_ == other.end_addr_ && reader_ == other.reader_); +} + +bool ChunksIter::operator!=(const ChunksIter& other) const { + return !this->operator==(other); +} + +std::string ChunksIter::to_string() const { + std::stringstream ss; + ss << "ChunksIter: [ca = " << current_addr_ + << ", ea = " << end_addr_ << "]"; + return ss.str(); +} + +// ======================================================= +// ========= Reader::ChunksRange ========================= +// ======================================================= + +ChunksRange::ChunksRange(const uint64_t begin_addr, const uint64_t end_addr, + Reader* reader) + : begin_addr_(begin_addr), end_addr_(end_addr), reader_(reader) { +} + +ChunksIter ChunksRange::begin() const { + return ChunksIter(begin_addr_, end_addr_, reader_); +} + +ChunksIter ChunksRange::end() const { + return ChunksIter(end_addr_, end_addr_, reader_); +} + +std::string ChunksRange::to_string() const { + std::stringstream ss; + ss << "ChunksRange: [ba = " << begin_addr_ << ", ea = " << end_addr_ + << "]"; + return ss.str(); +} + +// ========================================================== +// ========= Reader::MessagesStandardIter =================== +// ========================================================== + +MessagesStandardIter::MessagesStandardIter() + : current_chunk_it_{}, end_chunk_it_{}, msg_idx_{0} {} + +MessagesStandardIter::MessagesStandardIter( + const MessagesStandardIter& other) + : current_chunk_it_(other.current_chunk_it_), + end_chunk_it_(other.end_chunk_it_), + msg_idx_(other.msg_idx_) {} + +MessagesStandardIter::MessagesStandardIter(const ChunksIter begin_it, + const ChunksIter end_it, + const size_t msg_idx) + : current_chunk_it_{begin_it}, end_chunk_it_{end_it}, msg_idx_{msg_idx} { + if (current_chunk_it_ != end_chunk_it_ && !is_cleared()) next(); +} + +const MessageRef MessagesStandardIter::operator*() const { + return current_chunk_it_->operator[](msg_idx_); +} + +std::unique_ptr MessagesStandardIter::operator->() const { + return current_chunk_it_->messages(msg_idx_); +} + +MessagesStandardIter& MessagesStandardIter::operator++() { + this->next(); + return *this; +} + +MessagesStandardIter MessagesStandardIter::operator++(int) { + auto res = *this; + this->next(); + return res; +} + +void MessagesStandardIter::next() { + if (current_chunk_it_ == end_chunk_it_) return; + next_any(); + while (current_chunk_it_ != end_chunk_it_ && !is_cleared()) next_any(); +} + +void MessagesStandardIter::next_any() { + if (current_chunk_it_ == end_chunk_it_) return; + auto chunk_ref = *current_chunk_it_; + ++msg_idx_; + if (msg_idx_ >= chunk_ref.size()) { + // Advance to the next chunk + ++current_chunk_it_; + msg_idx_ = 0; + } +} + +bool MessagesStandardIter::operator==( + const MessagesStandardIter& other) const { + return (current_chunk_it_ == other.current_chunk_it_ && + end_chunk_it_ == other.end_chunk_it_ && msg_idx_ == other.msg_idx_); +} + +bool MessagesStandardIter::operator!=( + const MessagesStandardIter& other) const { + return !this->operator==(other); +} + +bool MessagesStandardIter::is_cleared() { + if (current_chunk_it_ == end_chunk_it_) return false; + const auto chunk_ref = *current_chunk_it_; + if (!chunk_ref.valid()) return false; + return (msg_idx_ < chunk_ref.size()); +} + +std::string MessagesStandardIter::to_string() const { + std::stringstream ss; + ss << "MessagesStandardIter: [curr_chunk_it = " + << current_chunk_it_.to_string() << ", msg_idx = " << msg_idx_ + << ", end_chunk_it = " << end_chunk_it_.to_string() << "]"; + return ss.str(); +} + +// ========================================================= +// ========= Reader::MessagesStandardRange ========================= +// ========================================================= + +MessagesStandardRange::MessagesStandardRange(const ChunksIter begin_it, + const ChunksIter end_it) + : begin_chunk_it_(begin_it), end_chunk_it_(end_it) {} + +MessagesStandardIter MessagesStandardRange::begin() const { + return MessagesStandardIter(begin_chunk_it_, end_chunk_it_, 0); +} + +MessagesStandardIter MessagesStandardRange::end() const { + return MessagesStandardIter(end_chunk_it_, end_chunk_it_, 0); +} + +std::string MessagesStandardRange::to_string() const { + std::stringstream ss; + ss << "MessagesStandardRange: [bit = " << begin_chunk_it_.to_string() + << ", eit = " << end_chunk_it_.to_string() << "]"; + return ss.str(); +} + +// ========================================================== +// ========= Reader ========================================= +// ========================================================== + +MessagesStreamingRange Reader::messages() { + if (!has_stream_info()) { + throw std::logic_error( + "ERROR: Can't iterate by streams without StreamingInfo " + "available."); + } + return MessagesStreamingRange(start_ts(), end_ts(), {}, this); +} + +MessagesStreamingRange Reader::messages(const ts_t start_ts, + const ts_t end_ts) { + if (!has_stream_info()) { + throw std::logic_error( + "ERROR: Can't iterate by streams without StreamingInfo " + "available."); + } + return MessagesStreamingRange(start_ts, end_ts, {}, this); +} + +MessagesStreamingRange Reader::messages( + const std::vector& stream_ids) { + if (!has_stream_info()) { + throw std::logic_error( + "ERROR: Can't iterate by streams without StreamingInfo " + "available."); + } + return MessagesStreamingRange(start_ts(), end_ts(), stream_ids, this); +} + +MessagesStreamingRange Reader::messages(const std::vector& stream_ids, + const ts_t start_ts, + const ts_t end_ts) { + if (!has_stream_info()) { + throw std::logic_error( + "ERROR: Can't iterate by streams without StreamingInfo " + "available."); + } + return MessagesStreamingRange(start_ts, end_ts, stream_ids, this); +} + +nonstd::optional Reader::ts_by_message_idx(uint32_t stream_id, + uint32_t message_idx) { + if (!has_stream_info()) { + throw std::logic_error( + "ERROR: Can't iterate by streams without StreamingInfo " + "available."); + } + if (!chunks_.has_message_idx()) { + return nonstd::nullopt; + } + // TODO: Check for message_count existence + ChunkInfoNode* cin = chunks_.get_info_by_message_idx(stream_id, message_idx); + if (!cin) return nonstd::nullopt; + + if (!verify_chunk(cin->offset)) { + return nonstd::nullopt; + } + + auto chunk_msg_index = message_idx - cin->message_start_idx; + + // shortcuting and not reading the chunk content if it's very first message + // and we already checked validity + if (chunk_msg_index == 0) { + return {chunks_.get(cin->offset)->start_ts}; + } + + // reading chunk data to get message timestamp + ChunkRef cref(cin->offset, this); + if (chunk_msg_index < cref.size()) { + return {cref.messages(chunk_msg_index)->ts()}; + } + + return nonstd::nullopt; +} + +MessagesStandardRange Reader::messages_standard() { + return MessagesStandardRange(chunks().begin(), chunks().end()); +} + +ChunksRange Reader::chunks() { + return ChunksRange(0, file_.metadata_offset(), this); +} + +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; + throw std::logic_error("provided OSF file is not a valid OSF file."); + } + + chunks_base_offset_ = file_.chunks_offset(); + + read_metadata(); + + read_chunks_info(); + +} + +Reader::Reader(OsfFile& osf_file) : Reader(osf_file.filename()) {} + +void Reader::read_metadata() { + metadata_buf_.resize(FLATBUFFERS_PREFIX_LENGTH); + + file_.seek(file_.metadata_offset()); + file_.read(metadata_buf_.data(), FLATBUFFERS_PREFIX_LENGTH); + size_t meta_size = get_prefixed_size(metadata_buf_.data()); + + const size_t full_meta_size = + meta_size + FLATBUFFERS_PREFIX_LENGTH + CRC_BYTES_SIZE; + + metadata_buf_.resize(full_meta_size); + + // no-op here + file_.seek(file_.metadata_offset() + FLATBUFFERS_PREFIX_LENGTH); + + file_.read(metadata_buf_.data() + FLATBUFFERS_PREFIX_LENGTH, + meta_size + CRC_BYTES_SIZE); + + if (!check_prefixed_size_block_crc(metadata_buf_.data(), full_meta_size)) { + throw std::logic_error("ERROR: Invalid metadata block in OSF file."); + } + + auto metadata = + ouster::osf::gen::GetSizePrefixedMetadata(metadata_buf_.data()); + auto entries = metadata->entries(); + for (uint32_t i = 0; i < entries->size(); ++i) { + auto entry = entries->Get(i); + MetadataEntryRef meta_ref(reinterpret_cast(entry)); + // Option 1: Late reconstruction + // meta_store_.add(meta_ref); + + // Option 2: Early reconstruction (with dynamic_pointer_cast later) + + auto meta_obj = meta_ref.as_type(); + if (meta_obj) { + // Successfull reconstruction of the metadata here. + meta_store_.add(*meta_obj); + } else { + // Can't reconstruct, adding the MetadataEntryRef proxy object + // i.e. late reconstruction path + meta_store_.add(meta_ref); + } + } + + // Get chunks states + std::vector chunk_offsets{}; + if (metadata->chunks() && metadata->chunks()->size() > 0) { + for (uint32_t i = 0; i < metadata->chunks()->size(); ++i) { + auto co = metadata->chunks()->Get(i); + chunks_.add(co->offset(), ts_t{co->start_ts()}, ts_t{co->end_ts()}); + chunk_offsets.push_back(co->offset()); + } + } + + // Assign next_offsets links + if (!chunk_offsets.empty()) { + std::sort(chunk_offsets.begin(), chunk_offsets.end()); + for (size_t i = 0; i < chunk_offsets.size() - 1; ++i) { + chunks_.get(chunk_offsets[i])->next_offset = chunk_offsets[i + 1]; + } + } + + // NOTE: Left here for debugging + // print_metadata_entries(); +} + +void Reader::read_chunks_info() { + // Check that it has StreamingInfo and thus a valid StreamingLayout OSF + // see RFC0018 for details + auto streaming_info = meta_store_.get(); + if (!streaming_info) { + return; + } + + if (streaming_info->chunks_info().size() != chunks_.size()) { + throw std::logic_error( + "ERROR: StreamingInfo chunks info should equal chunks in the " + "Reader"); + } + + for (const auto& sci : streaming_info->chunks_info()) { + chunks_.add_info(sci.first, sci.second.stream_id, + sci.second.message_count); + } + + chunks_.link_stream_chunks(); +} + +// TODO[pb]: MetadataStore to_string() ? +void Reader::print_metadata_entries() { + std::cout << "Reader::print_metadata_entries:\n"; + int i = 0; + for (const auto& me : meta_store_.entries()) { + std::cout << " entry[" << i++ << "] = " << me.second->to_string() + << std::endl; + } +} + +std::string Reader::id() const { + if (auto metadata = get_osf_metadata_from_buf(metadata_buf_.data())) { + if (metadata->id()) { + return metadata->id()->str(); + } + } + return std::string{}; +} + +ts_t Reader::start_ts() const { + if (auto metadata = get_osf_metadata_from_buf(metadata_buf_.data())) { + return ts_t{metadata->start_ts()}; + } + return ts_t{}; +} + +ts_t Reader::end_ts() const { + if (auto metadata = get_osf_metadata_from_buf(metadata_buf_.data())) { + return ts_t{metadata->end_ts()}; + } + return ts_t{}; +} + +bool Reader::has_stream_info() const { return chunks_.has_info(); } + +bool Reader::verify_chunk(uint64_t chunk_offset) { + auto cs = chunks_.get(chunk_offset); + if (!cs) return false; + if (cs->status == ChunkValidity::UNKNOWN) { + auto chunk_buf = file_.read_chunk(chunks_base_offset_ + chunk_offset); + cs->status = osf::check_osf_chunk_buf(chunk_buf->data(), + chunk_buf->size()) + ? ChunkValidity::VALID + : ChunkValidity::INVALID; + } + return (cs->status == ChunkValidity::VALID); +} + +// ========================================================= +// ========= MessageRef ==================================== +// ========================================================= + +uint32_t MessageRef::id() const { + const ouster::osf::v2::StampedMessage* sm = + reinterpret_cast(buf_); + return sm->id(); +} + +MessageRef::ts_t MessageRef::ts() const { + const ouster::osf::v2::StampedMessage* sm = + reinterpret_cast(buf_); + return ts_t(sm->ts()); +} + +bool MessageRef::is(const std::string& type_str) const { + auto meta = meta_provider_.get(id()); + return (meta != nullptr) && (meta->type() == type_str); +} + +bool MessageRef::operator==(const MessageRef& other) const { + return (buf_ == other.buf_); +} + +bool MessageRef::operator!=(const MessageRef& other) const { + return !this->operator==(other); +} + +std::string MessageRef::to_string() const { + std::stringstream ss; + const ouster::osf::v2::StampedMessage* sm = + reinterpret_cast(buf_); + ss << "MessageRef: [id = " << id() << ", ts = " << ts().count() + << ", buffer = " + << osf::to_string(sm->buffer()->Data(), sm->buffer()->size(), 100) + << "]"; + return ss.str(); +} + +std::vector MessageRef::buffer() const { + const ouster::osf::gen::StampedMessage* sm = + reinterpret_cast(buf_); + + if (sm->buffer() == nullptr) { + return {}; + } + + return {sm->buffer()->data(), sm->buffer()->data() + sm->buffer()->size()}; +} + +// ======================================================= +// =========== ChunkRef ================================== +// ======================================================= + +ChunkRef::ChunkRef() + : chunk_offset_{std::numeric_limits::max()}, + reader_{nullptr}, + chunk_buf_{nullptr} {} + +ChunkRef::ChunkRef(const uint64_t offset, Reader* reader) + : chunk_offset_(offset), reader_(reader) { + if (!reader->file_.is_memory_mapped()) { + // NOTE[pb]: We just rely on OS file operations caching thus not + // trying to cache the read chunks in reader, however we might + // reconsider it if we will discover the our caching might give + // us better results (for now it's as is) + chunk_buf_ = reader->file_.read_chunk(reader_->chunks_base_offset_ + + chunk_offset_); + } + // Always expects "verified" chunk offset. See Reader::verify_chunk() + assert(reader_->chunks_.get(chunk_offset_)->status != + ChunkValidity::UNKNOWN); +} + +size_t ChunkRef::size() const { + if (!valid()) return 0; + const ouster::osf::v2::Chunk* chunk = get_chunk_from_buf(get_chunk_ptr()); + if (chunk->messages()) { + return chunk->messages()->size(); + } + return 0; +} + +bool ChunkRef::valid() const { + return (state()->status == ChunkValidity::VALID); +} + +std::unique_ptr ChunkRef::messages(size_t msg_idx) const { + if (!valid()) return nullptr; + const ouster::osf::v2::Chunk* chunk = get_chunk_from_buf(get_chunk_ptr()); + if (!chunk->messages() || msg_idx >= chunk->messages()->size()) + return nullptr; + const ouster::osf::v2::StampedMessage* m = chunk->messages()->Get(msg_idx); + return std::make_unique( + reinterpret_cast(m), reader_->meta_store_, chunk_buf_); +} + +const MessageRef ChunkRef::operator[](size_t msg_idx) const { + const ouster::osf::v2::Chunk* chunk = get_chunk_from_buf(get_chunk_ptr()); + const ouster::osf::v2::StampedMessage* m = chunk->messages()->Get(msg_idx); + return MessageRef(reinterpret_cast(m), reader_->meta_store_, + chunk_buf_); +} + +MessagesChunkIter ChunkRef::begin() const { + return MessagesChunkIter(*this, 0); +} + +MessagesChunkIter ChunkRef::end() const { + return MessagesChunkIter(*this, size()); +} + +bool ChunkRef::operator==(const ChunkRef& other) const { + return (chunk_offset_ == other.chunk_offset_ && reader_ == other.reader_); +} + +bool ChunkRef::operator!=(const ChunkRef& other) const { + return !this->operator==(other); +} + +std::string ChunkRef::to_string() const { + std::stringstream ss; + auto chunk_state = state(); + ss << "ChunkRef: [" + << "msgs_size = " << size() << ", state = (" + << (chunk_state ? osf::to_string(*chunk_state) : "no state") << ")" + << ", chunk_buf_ = " + << (chunk_buf_ ? "size=" + std::to_string(chunk_buf_->size()) : "nullptr") + << "]"; + return ss.str(); +} + +const uint8_t* ChunkRef::get_chunk_ptr() const { + if (reader_->file_.is_memory_mapped()) { + return reader_->file_.buf() + reader_->chunks_base_offset_ + + chunk_offset_; + } + if (chunk_buf_ && !chunk_buf_->empty()) { + return chunk_buf_->data(); + } + + return nullptr; +} + +// ========================================================== +// ========= MessagesChunkIter ============================== +// ========================================================== + +MessagesChunkIter::MessagesChunkIter() + : chunk_ref_{}, msg_idx_{0} {} + +MessagesChunkIter::MessagesChunkIter( + const MessagesChunkIter& other) + : chunk_ref_(other.chunk_ref_), msg_idx_(other.msg_idx_) {} + +MessagesChunkIter::MessagesChunkIter(const ChunkRef chunk_ref, + const size_t msg_idx) + : chunk_ref_(chunk_ref), msg_idx_(msg_idx) {} + +bool MessagesChunkIter::operator==(const MessagesChunkIter& other) const { + return (chunk_ref_ == other.chunk_ref_ && msg_idx_ == other.msg_idx_); +} + +bool MessagesChunkIter::operator!=(const MessagesChunkIter& other) const { + return !this->operator==(other); +} + +std::string MessagesChunkIter::to_string() const { + std::stringstream ss; + ss << "MessagesChunkIter: [chunk_ref = " << chunk_ref_.to_string() + << ", msg_idx = " << msg_idx_ << "]"; + return ss.str(); +} + +const MessageRef MessagesChunkIter::operator*() const { + return chunk_ref_[msg_idx_]; +} + +std::unique_ptr MessagesChunkIter::operator->() const { + return chunk_ref_.messages(msg_idx_); +} + +MessagesChunkIter& MessagesChunkIter::operator++() { + this->next(); + return *this; +} + +MessagesChunkIter MessagesChunkIter::operator++(int) { + auto res = *this; + this->next(); + return res; +} + +MessagesChunkIter& MessagesChunkIter::operator--() { + this->prev(); + return *this; +} + +MessagesChunkIter MessagesChunkIter::operator--(int) { + auto res = *this; + this->prev(); + return res; +} + +void MessagesChunkIter::next() { + if (msg_idx_ < chunk_ref_.size()) ++msg_idx_; +} + +void MessagesChunkIter::prev() { + if (msg_idx_ > 0) --msg_idx_; +} + +// ========================================================== +// ========= SreeamingReader::MessagesStreamingIter ========= +// ========================================================== + +// Simplest hash to better compare priority que states with stream ids +uint32_t calc_stream_ids_hash(const std::vector& stream_ids) { + uint32_t b = 378551; + uint32_t a = 63689; + uint32_t hash = 0; + std::vector tmp_stream_ids{stream_ids}; + std::sort(tmp_stream_ids.begin(), tmp_stream_ids.end()); + for (std::size_t i = 0; i < tmp_stream_ids.size(); ++i) { + hash = hash * a + tmp_stream_ids[i]; + a *= b; + } + return hash; +} + +MessagesStreamingIter::MessagesStreamingIter() + : curr_ts_{}, + end_ts_{}, + stream_ids_{}, + stream_ids_hash_{}, + reader_{nullptr}, + curr_chunks_{} {} + +MessagesStreamingIter::MessagesStreamingIter(const MessagesStreamingIter& other) + : curr_ts_{other.curr_ts_}, + end_ts_{other.end_ts_}, + stream_ids_{other.stream_ids_}, + stream_ids_hash_{other.stream_ids_hash_}, + reader_{other.reader_}, + curr_chunks_{other.curr_chunks_} {} + +MessagesStreamingIter::MessagesStreamingIter( + const ts_t start_ts, const ts_t end_ts, + const std::vector& stream_ids, Reader* reader) + : curr_ts_{start_ts}, + end_ts_{end_ts}, + stream_ids_{stream_ids}, + stream_ids_hash_{calc_stream_ids_hash(stream_ids_)}, + reader_{reader} { + if (curr_ts_ == end_ts_) return; + + if (stream_ids_.empty()) { + for (const auto& sm : reader_->chunks_.stream_chunks()) { + stream_ids_.push_back(sm.first); + } + } + + // Per every stream_id open the first valid chunk in [start_ts, end_ts) + // range. Steps: + // 1. find first chunk by start_ts (lower bound) + // 2. if chunk is valid open it, otherwise step 5 + // 3. find first message within chunk in [start_ts, end_ts) range + // 4. addd opened chunk (offset, msg_idx) to the queue of streams we are + // reading + // 5. move to the next chunk within stream, and continue from Step 2. + for (const auto stream_id : stream_ids_) { + // 1. find first chunk by start_ts (lower bound) + auto* cs = reader_->chunks_.get_by_lower_bound_ts(stream_id, start_ts); + bool filled = false; + while (cs != nullptr && cs->start_ts < end_ts && !filled) { + auto curr_offset = cs->offset; + if (reader_->verify_chunk(curr_offset)) { + // 2. if chunk is valid open it, otherwise step 5 + ChunkRef cref{curr_offset, reader_}; + for (size_t msg_idx = 0; msg_idx < cref.size(); ++msg_idx) { + // 3. find first message withing chunk in [start_ts, end_ts) + // range + if (cref[msg_idx].ts() >= start_ts && + cref[msg_idx].ts() < end_ts) { + // 4. addd opened chunk (offset, msg_idx) to the queue + // of streams we are reading + curr_chunks_.emplace(cref, msg_idx); + filled = true; + break; + } + } + } + // 5. move to the next chunk within stream, and continue from + // Step 2 + cs = reader_->chunks_.next_by_stream(curr_offset); + } + } + + if (!curr_chunks_.empty()) { + const auto& curr_item = curr_chunks_.top(); + curr_ts_ = curr_item.first[curr_item.second].ts(); + } else { + curr_ts_ = end_ts_; + } + +} + +const MessageRef MessagesStreamingIter::operator*() const { + const auto& curr_item = curr_chunks_.top(); + return curr_item.first[curr_item.second]; +} + +std::unique_ptr MessagesStreamingIter::operator->() const { + const auto& curr_item = curr_chunks_.top(); + return curr_item.first.messages(curr_item.second); +} + +// It's not a full equality because priority_queue requires either gnarly +// hacks to access internal collection or re-implementation. +// But for the purpose of checking the iterator position in a collection +// and checking boundaries it should be somewhat "correct" +// +// TODO[pb]: This should be revisited later with priority_queue +// re-implementation that will be easier to work with for our multi-streams +// case. +bool MessagesStreamingIter::operator==( + const MessagesStreamingIter& other) const { + return (curr_ts_ == other.curr_ts_ && end_ts_ == other.end_ts_ && + reader_ == other.reader_ && + stream_ids_hash_ == other.stream_ids_hash_ && + curr_chunks_.size() == other.curr_chunks_.size() && + (curr_chunks_.empty() || + curr_chunks_.top() == other.curr_chunks_.top())); +} + +bool MessagesStreamingIter::operator!=( + const MessagesStreamingIter& other) const { + return !this->operator==(other); +} + +MessagesStreamingIter& MessagesStreamingIter::operator++() { + this->next(); + return *this; +} + +MessagesStreamingIter MessagesStreamingIter::operator++(int) { + auto res = *this; + this->next(); + return res; +} + +void MessagesStreamingIter::next() { + if (curr_ts_ >= end_ts_) return; + + const auto curr_item = curr_chunks_.top(); + curr_chunks_.pop(); + + const ChunkRef& cref = curr_item.first; + size_t msg_idx = curr_item.second; + + if (msg_idx + 1 < cref.size()) { + // Traversing current chunk + ++msg_idx; + if (cref[msg_idx].ts() < end_ts_) { + curr_chunks_.emplace(cref, msg_idx); + } + } else { + // Looking for the next chunk of the current stream_id + // const auto curr_stream_id = cref[msg_idx].id(); + auto next_chunk_state = + reader_->chunks_.next_by_stream(curr_item.first.offset()); + if (next_chunk_state) { + auto next_chunk_info = + reader_->chunks_.get_info(next_chunk_state->offset); + if (next_chunk_info == nullptr) { + throw std::logic_error( + "ERROR: Can't iterate by streams without StreamingInfo " + "available."); + } + if (next_chunk_state->start_ts < end_ts_) { + if (reader_->verify_chunk(next_chunk_state->offset)) { + ChunkRef cref{next_chunk_state->offset, reader_}; + for (size_t msg_idx = 0; msg_idx < cref.size(); ++msg_idx) { + if (cref[msg_idx].ts() < curr_ts_) { + throw std::logic_error( + "ERROR: Can't have decreasing by timestamp " + "messages in StreamingLayout"); + } + if (cref[msg_idx].ts() < end_ts_) { + curr_chunks_.emplace(cref, msg_idx); + break; + } + } + } + } + } + } + + if (!curr_chunks_.empty()) { + const auto& curr_item = curr_chunks_.top(); + const auto next_ts = curr_item.first[curr_item.second].ts(); + if (next_ts < curr_ts_) { + throw std::logic_error( + "ERROR: Can't have decreasing by timestamp messages " + "in StreamingLayout"); + } + curr_ts_ = next_ts; + } else { + curr_ts_ = end_ts_; + } +} + +/// NOTE: Debug function, will be removed after some time ... +void MessagesStreamingIter::print_and_finish() { + while (!curr_chunks_.empty()) { + auto& top = curr_chunks_.top(); + std::cout << "(( ts = " << top.first[top.second].ts().count() + << ", id = " << top.first[top.second].id() + << ", msg_idx = " << top.second + << ", cref = " << top.first.to_string() << std::endl; + curr_chunks_.pop(); + } +} + +std::string MessagesStreamingIter::to_string() const { + std::stringstream ss; + ss << "MessagesStreamingIter: [curr_ts = " << curr_ts_.count() + << ", end_ts = " << end_ts_.count() + << ", curr_chunks_.size = " << curr_chunks_.size() + << ", stream_ids_hash_ = " << stream_ids_hash_; + if (!curr_chunks_.empty()) { + const auto& curr_item = curr_chunks_.top(); + ss << ", top = (ts = " << curr_item.first[curr_item.second].ts().count() + << ", id = " << curr_item.first[curr_item.second].id() << ")"; + } + ss << "]"; + return ss.str(); +} + +// ========================================================= +// ========= StreamingReader::MessagesStreamingRange ======= +// ========================================================= + +MessagesStreamingRange::MessagesStreamingRange( + const ts_t start_ts, const ts_t end_ts, + const std::vector& stream_ids, Reader* reader) + : start_ts_(start_ts), + end_ts_(end_ts), + stream_ids_{stream_ids}, + reader_{reader} {} + +MessagesStreamingIter MessagesStreamingRange::begin() const { + return MessagesStreamingIter(start_ts_, end_ts_ + ts_t{1}, stream_ids_, + reader_); +} + +MessagesStreamingIter MessagesStreamingRange::end() const { + return MessagesStreamingIter(end_ts_ + ts_t{1}, end_ts_ + ts_t{1}, + stream_ids_, reader_); +} + +std::string MessagesStreamingRange::to_string() const { + std::stringstream ss; + ss << "MessagesStreamingRange: [start_ts = " << start_ts_.count() + << ", end_ts = " << end_ts_.count() << "]"; + return ss.str(); +} + +} // namespace osf +} // namespace ouster \ No newline at end of file diff --git a/ouster_osf/src/stream_lidar_scan.cpp b/ouster_osf/src/stream_lidar_scan.cpp new file mode 100644 index 00000000..68e7f3f4 --- /dev/null +++ b/ouster_osf/src/stream_lidar_scan.cpp @@ -0,0 +1,381 @@ +/** + * Copyright(c) 2021, Ouster, Inc. + * All rights reserved. + */ + +#include "ouster/osf/stream_lidar_scan.h" + +#include + +#include "ouster/osf/basics.h" +#include "png_tools.h" + +#include "ouster/types.h" +#include "ouster/lidar_scan.h" + +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); +} + +sensor::ChanFieldType from_osf_enum(gen::CHAN_FIELD_TYPE ft) { + // TODO[pb]: When we start diverging add a better mapping. + return static_cast(ft); +} + +} + +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(); +} + +// TODO[pb]: Error if field_types is not subset of fields in ls? +LidarScan slice_with_cast(const LidarScan& ls_src, + const LidarScanFieldTypes& field_types) { + LidarScan ls_dest{static_cast(ls_src.w), + static_cast(ls_src.h), field_types.begin(), + field_types.end()}; + + ls_dest.frame_id = ls_src.frame_id; + + // Copy headers + 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(); + + // Copy fields + for (const auto& ft : field_types) { + if (ls_src.field_type(ft.first)) { + ouster::impl::visit_field(ls_dest, ft.first, + ouster::impl::copy_and_cast(), ls_src, + ft.first); + } else { + ouster::impl::visit_field(ls_dest, ft.first, zero_field()); + } + } + + return ls_dest; +} + +// === LidarScanStream support functions ==== + +// After Flatbuffers >= 22.9.24 the alignment bug was introduced in the #7520 +// https://github.com/google/flatbuffers/pull/7520 +// That is manifested in the additional 2 bytes written to the vector buffer +// before adding the vector size that are not properly handled by the reading +// the buffer back since vector is the 4 bytes length with the data starting +// right after the size. And with those additional 2 zero bytes it's just +// ruining the vector data. +// It started happening because of alignment rules changed in #7520 and it's +// triggering for our code because he have a small structs of just 2 bytes +// which can result in not 4 bytes aligned memory that is min requirement +// for storing the subsequent vector length in uoffset_t type) +// FIX[pb]: We are changing the original CreateVectorOfStructs implementation +// with CreateUninitializedVectorOfStructs impl that is different in +// a way how it's using StartVector underneath. +template +flatbuffers::Offset> CreateVectorOfStructs( + flatbuffers::FlatBufferBuilder& _fbb, const T* v, size_t len) { + T* buf_to_write; + auto res_off = _fbb.CreateUninitializedVectorOfStructs(len, &buf_to_write); + if (len > 0) { + memcpy(buf_to_write, reinterpret_cast(v), + sizeof(T) * len); + } + return res_off; +} + +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 (or extend if the field types is + // different) + // TODO[pb]: Consider to error instead of extending LidarScan, but be + // sure to check the consistence everywhere. That's why it's + // not done on the first pass here ... + ls = slice_with_cast(lidar_scan, meta_field_types); + } + + // Encode LidarScan to PNG buffers + ScanData scan_data = scanEncode(ls, info.format.pixel_shift_by_row); + + // 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( + fbb, field_types.data(), field_types.size()); + auto timestamp_off = fbb.CreateVector(ls.timestamp().data(), + ls.timestamp().size()); + auto measurement_id_off = + fbb.CreateVector(ls.measurement_id().data(), ls.w); + 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); + } + return gen::CreateLidarScanMsg(fbb, channels_off, field_types_off, + timestamp_off, measurement_id_off, + status_off, ls.frame_id, pose_off); +} + +std::unique_ptr restore_lidar_scan( + const std::vector buf, const ouster::sensor::sensor_info& info) { + auto ls_msg = + flatbuffers::GetSizePrefixedRoot( + buf.data()); + + uint32_t width = info.format.columns_per_frame; + uint32_t height = info.format.pixels_per_column; + + // read field_types + 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())); + }); + } + + // Init lidar scan with recovered fields + auto ls = std::make_unique(width, height, field_types.begin(), + field_types.end()); + + ls->frame_id = ls_msg->frame_id(); + + // Set timestamps per column + auto msg_ts_vec = ls_msg->header_timestamp(); + if (msg_ts_vec) { + if (static_cast(ls->w) == msg_ts_vec->size()) { + for (uint32_t i = 0; i < width; ++i) { + 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; + return nullptr; + } + } + + // Set measurement_id per column + auto msg_mid_vec = ls_msg->header_measurement_id(); + if (msg_mid_vec) { + if (static_cast(ls->w) == msg_mid_vec->size()) { + for (uint32_t i = 0; i < width; ++i) { + 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; + return nullptr; + } + } + + // Set status per column + auto msg_status_vec = ls_msg->header_status(); + if (msg_status_vec) { + if (static_cast(ls->w) == msg_status_vec->size()) { + for (uint32_t i = 0; i < width; ++i) { + 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; + return nullptr; + } + } + + // 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"; + return nullptr; + } + ScanData scan_data; + for (uint32_t i = 0; i < msg_scan_vec->size(); ++i) { + auto channel_buffer = msg_scan_vec->Get(i)->buffer(); + scan_data.emplace_back(channel_buffer->begin(), channel_buffer->end()); + } + + // 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); + } + } + } else if (pose_vec->size() != 0) { + std::cout << "ERROR: LidarScanMsg has pose of length: " + << pose_vec->size() + << ", expected: " << (ls->pose().size() * 16) + << std::endl; + return nullptr; + } + } + + // Decode PNGs data to LidarScan + if (scanDecode(*ls, scan_data, info.format.pixel_shift_by_row)) { + return nullptr; + } + + return ls; +} + + +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_types_off = osf::CreateVectorOfStructs( + fbb, field_types.data(), field_types.size()); + + auto lss_offset = ouster::osf::gen::CreateLidarScanStream( + fbb, sensor_meta_id_, field_types_off); + + fbb.FinishSizePrefixed(lss_offset); + + const uint8_t* buf = fbb.GetBufferPointer(); + const size_t size = fbb.GetSize(); + return {buf, buf + size}; +}; + +std::unique_ptr LidarScanStreamMeta::from_buffer( + const std::vector& buf) { + auto lidar_scan_stream = gen::GetSizePrefixedLidarScanStream(buf.data()); + auto sensor_meta_id = lidar_scan_stream->sensor_id(); + + auto field_types_vec = lidar_scan_stream->field_types(); + 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())); + }); + } + + // auto frame_mode = lidar_scan_stream->lidar_frame_mode(); + return std::make_unique( + sensor_meta_id, field_types); +}; + +std::string LidarScanStreamMeta::repr() const { + std::stringstream ss; + ss << "LidarScanStreamMeta: sensor_id = " << sensor_meta_id_ + << ", field_types = {"; + bool first = true; + for (const auto& f : field_types_) { + if (!first) ss << ", "; + ss << sensor::to_string(f.first) << ":" + << ouster::sensor::to_string(f.second); + first = false; + } + ss << "}"; + return ss.str(); +} + +// ============== LidarScan Stream ops =========================== + +LidarScanStream::LidarScanStream(Writer& writer, const uint32_t sensor_meta_id, + const LidarScanFieldTypes& field_types) + : writer_{writer}, + meta_(sensor_meta_id, field_types), + sensor_meta_id_(sensor_meta_id) { + // Check sensor and get sensor_info + auto sensor_meta_entry = writer.getMetadata(sensor_meta_id_); + if (sensor_meta_entry == nullptr) { + std::cerr << "ERROR: can't find sensor_meta_id = " << sensor_meta_id + << std::endl; + std::abort(); + } + + sensor_info_ = sensor_meta_entry->info(); + + stream_meta_id_ = writer_.addMetadata(meta_); + +} + +// TODO[pb]: Every save func in Streams is uniform, need to nicely extract +// it and remove close dependence on Writer? ... +void LidarScanStream::save(const ouster::osf::ts_t ts, + const LidarScan& lidar_scan) { + const auto& msg_buf = make_msg(lidar_scan); + writer_.saveMessage(meta_.id(), ts, msg_buf); +} + +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()); + fbb.FinishSizePrefixed(ls_msg_offset); + const uint8_t* buf = fbb.GetBufferPointer(); + const size_t size = fbb.GetSize(); + return {buf, buf + size}; +} + +std::unique_ptr LidarScanStream::decode_msg( + const std::vector& buf, const LidarScanStream::meta_type& meta, + const MetadataStore& meta_provider) { + + auto sensor = meta_provider.get(meta.sensor_meta_id()); + + auto info = sensor->info(); + + return restore_lidar_scan(buf, info); +} + + +} // namespace osf +} // namespace ouster diff --git a/ouster_osf/src/writer.cpp b/ouster_osf/src/writer.cpp new file mode 100644 index 00000000..6ff37855 --- /dev/null +++ b/ouster_osf/src/writer.cpp @@ -0,0 +1,234 @@ +/** + * Copyright(c) 2021, Ouster, Inc. + * All rights reserved. + */ + +#include "ouster/osf/writer.h" + +#include "ouster/osf/crc32.h" +#include "ouster/osf/basics.h" + +#include "ouster/osf/layout_standard.h" +#include "ouster/osf/layout_streaming.h" + +#include "fb_utils.h" + +constexpr size_t MAX_CHUNK_SIZE = 500 * 1024 * 1024; + +namespace ouster { +namespace osf { + +Writer::Writer(const std::string& filename) : Writer(filename, std::string{}) {} + +Writer::Writer(const std::string& filename, const std::string& metadata_id, + uint32_t chunk_size) + : file_name_(filename), + metadata_id_{metadata_id}, + chunks_layout_{ChunksLayout::LAYOUT_STREAMING} { + // chunks STREAMING_LAYOUT + chunks_writer_ = std::make_shared(*this, chunk_size); + + // or chunks STANDARD_LAYOUT (left for now to show the mechanisms of switching + // layout strategies) + // chunks_writer_ = std::make_shared(*this, chunk_size); + + // TODO[pb]: Check if file exists, add flag overwrite/not overwrite, etc + + header_size_ = start_osf_file(file_name_); + + if (header_size_ > 0) { + pos_ = static_cast(header_size_); + } else { + std::cerr << "ERROR: Can't write to file :(\n"; + std::abort(); + } +} + +uint64_t Writer::append(const uint8_t* buf, const uint64_t size) { + if (pos_ < 0) { + std::cerr << "ERROR: Writer is not ready (not started?)\n"; + std::abort(); + } + if (finished_) { + std::cerr << "ERROR: Hmm, Writer is finished. \n"; + std::abort(); + } + if (size == 0) { + std::cout << "nothing to append!!!\n"; + return 0; + } + uint64_t saved_bytes = buffer_to_file(buf, size, file_name_, true); + pos_ += static_cast(saved_bytes); + return saved_bytes; +} + +// > > > ===================== Chunk Emiter operations ====================== + +void Writer::saveMessage(const uint32_t stream_id, const ts_t ts, + const std::vector& msg_buf) { + + if (!meta_store_.get(stream_id)) { + std::cerr << "ERROR: Attempt to save the non existent stream: id = " + << stream_id << std::endl; + std::abort(); + return; + } + + chunks_writer_->saveMessage(stream_id, ts, msg_buf); +} + +uint64_t Writer::emit_chunk(const ts_t chunk_start_ts, const ts_t chunk_end_ts, + const std::vector& chunk_buf) { + uint64_t saved_bytes = append(chunk_buf.data(), chunk_buf.size()); + uint64_t res_chunk_offset{0}; + if (saved_bytes && saved_bytes == chunk_buf.size() + CRC_BYTES_SIZE) { + chunks_.emplace_back(chunk_start_ts.count(), chunk_end_ts.count(), + next_chunk_offset_); + res_chunk_offset = next_chunk_offset_; + if (start_ts_ > chunk_start_ts) start_ts_ = chunk_start_ts; + if (end_ts_ < chunk_end_ts) end_ts_ = chunk_end_ts; + next_chunk_offset_ += saved_bytes; + started_ = true; + } else { + std::cerr << "ERROR: Can't save to file. saved_bytes = " + << saved_bytes << std::endl; + std::abort(); + } + return res_chunk_offset; +} + +// < < < ================== Chunk Emiter operations ====================== + +std::vector Writer::make_metadata() const { + + auto metadata_fbb = flatbuffers::FlatBufferBuilder(32768); + + std::vector> entries = + meta_store_.make_entries(metadata_fbb); + + auto metadata = ouster::osf::gen::CreateMetadataDirect( + metadata_fbb, metadata_id_.c_str(), + !chunks_.empty() ? start_ts_.count() : 0, + !chunks_.empty() ? end_ts_.count() : 0, &chunks_, &entries); + + metadata_fbb.FinishSizePrefixed(metadata, + ouster::osf::gen::MetadataIdentifier()); + + const uint8_t* buf = metadata_fbb.GetBufferPointer(); + uint32_t size = metadata_fbb.GetSize(); + + return {buf, buf + size}; +} + +void Writer::close() { + if (finished_) { + // already did everything + return; + } + + // Finish all chunks in flight + chunks_writer_->finish(); + + // Encode chunks, metadata entries and other fields into final buffer + auto metadata_buf = make_metadata(); + + uint64_t metadata_offset = pos_; + uint64_t metadata_saved_size = + append(metadata_buf.data(), metadata_buf.size()); + if (metadata_saved_size && + metadata_saved_size == metadata_buf.size() + CRC_BYTES_SIZE) { + if (finish_osf_file(file_name_, metadata_offset, metadata_saved_size) == + header_size_) { + finished_ = true; + } else { + std::cerr << "ERROR: Can't finish OSF file! Recorded header of " + "different sizes ..." + << std::endl; + std::abort(); + } + } else { + std::cerr << "ERROR: Oh, why we are here and didn't finish correctly?" + << std::endl; + std::abort(); + } +} + +uint32_t Writer::chunk_size() const { + if (chunks_writer_) { + return static_cast(chunks_writer_->chunk_size()); + } + return 0; +} + +Writer::~Writer() { + close(); +} + +// ================================================================ + +void ChunkBuilder::saveMessage(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; + return; + } + + if (fbb_.GetSize() + msg_buf.size() > MAX_CHUNK_SIZE) { + std::cerr << "ERROR: reached max possible chunk size MAX_SIZE" + << std::endl; + std::abort(); + } + + update_start_end(ts); + + // wrap the buffer into StampedMessage + auto stamped_msg = gen::CreateStampedMessageDirect(fbb_, ts.count(), + stream_id, &msg_buf); + messages_.push_back(stamped_msg); +} + +void ChunkBuilder::reset() { + start_ts_ = ts_t::max(); + end_ts_ = ts_t::min(); + fbb_.Clear(); + messages_.clear(); + finished_ = false; +} + +uint32_t ChunkBuilder::size() const { + return fbb_.GetSize(); +} + +uint32_t ChunkBuilder::messages_count() const { + return static_cast(messages_.size()); +} + +void ChunkBuilder::update_start_end(const ts_t ts) { + if (start_ts_ > ts) start_ts_ = ts; + if (end_ts_ < ts) end_ts_ = ts; +} + +std::vector ChunkBuilder::finish() { + if (messages_.empty()) { + finished_ = true; + return {}; + } + + if (!finished_) { + auto chunk = gen::CreateChunkDirect(fbb_, &messages_); + fbb_.FinishSizePrefixed(chunk, gen::ChunkIdentifier()); + finished_ = true; + } + + const uint8_t* buf = fbb_.GetBufferPointer(); + uint32_t size = fbb_.GetSize(); + + return {buf, buf + size}; +} + +// ================================================================ + +} // namespace osf +} // namespace ouster \ No newline at end of file diff --git a/ouster_osf/tests/CMakeLists.txt b/ouster_osf/tests/CMakeLists.txt new file mode 100644 index 00000000..d6a470b4 --- /dev/null +++ b/ouster_osf/tests/CMakeLists.txt @@ -0,0 +1,37 @@ +cmake_minimum_required(VERSION 3.1.0) + +find_package(GTest REQUIRED) + +# Each test file should be in a format "_test.cpp" +set(OSF_TESTS_SOURCES png_tools_test.cpp + writer_test.cpp + writer_custom_test.cpp + file_test.cpp + crc_test.cpp + file_ops_test.cpp + reader_test.cpp + operations_test.cpp + pcap_source_test.cpp +) + +message(STATUS "OSF: adding testing .... ") + + +# Create "osf_" tests for every test +foreach(TEST_FULL_NAME ${OSF_TESTS_SOURCES}) + get_filename_component(TEST_FILENAME ${TEST_FULL_NAME} NAME_WE) + add_executable(osf_${TEST_FILENAME} ${TEST_FULL_NAME}) + set_target_properties(osf_${TEST_FILENAME} + PROPERTIES RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/tests") + target_include_directories(osf_${TEST_FILENAME} PRIVATE ${CMAKE_CURRENT_LIST_DIR}/../src) + target_link_libraries(osf_${TEST_FILENAME} ouster_osf + GTest::gtest GTest::gtest_main) + add_test(NAME osf_${TEST_FILENAME} + COMMAND osf_${TEST_FILENAME} --gtest_output=xml:osf_${TEST_FILENAME}.xml) + set_tests_properties( + osf_${TEST_FILENAME} + PROPERTIES + ENVIRONMENT + DATA_DIR=${CMAKE_CURRENT_LIST_DIR}/../../tests/ + ) +endforeach() \ No newline at end of file diff --git a/ouster_osf/tests/common.h b/ouster_osf/tests/common.h new file mode 100644 index 00000000..686de2c6 --- /dev/null +++ b/ouster_osf/tests/common.h @@ -0,0 +1,173 @@ +/** + * Copyright(c) 2021, Ouster, Inc. + * All rights reserved. + */ + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "compat_ops.h" +#include "ouster/lidar_scan.h" +#include "ouster/osf/basics.h" +#include "ouster/osf/stream_lidar_scan.h" + +namespace ouster { +namespace osf { + +constexpr double TEST_EPS = 1e-8; +constexpr char OSF_OUTPUT_DIR[] = "test_osf"; + +using idx = std::ptrdiff_t; + +inline bool get_test_data_dir(std::string& test_data_dir) { + std::string test_data_dir_var; + if (get_env_var("DATA_DIR", test_data_dir_var)) { + // printf("DATA_DIR: %s\n", test_data_dir_var); + if (!test_data_dir_var.empty()) { + if (!is_dir(test_data_dir_var)) { + printf("WARNING: DATA_DIR: %s doesn't exist\n", + test_data_dir_var.c_str()); + return false; + } + test_data_dir = test_data_dir_var; + return true; + } + } + printf("ERROR: DATA_DIR env var: is not set for OSF tests\n"); + return false; +} + +/// Get output dir for test files +bool get_output_dir(std::string& output_dir) { + std::string build_dir; + if (get_env_var("BUILD_DIR", build_dir)) { + if (!build_dir.empty()) { + if (!is_dir(build_dir)) { + printf("ERROR: BUILD_DIR: %s doesn't exist yet\n", + build_dir.c_str()); + return false; + } + output_dir = std::string{build_dir} + "/" + OSF_OUTPUT_DIR; + } + } else { + output_dir = OSF_OUTPUT_DIR; + } + if (!is_dir(output_dir)) { + if (!make_dir(output_dir)) { + printf("ERROR: Can't create output_dir: %s\n", output_dir.c_str()); + return false; + } + } + return true; +} + +inline double normal_d(const double m, const double s) { + std::random_device rd; + std::mt19937 gen{rd()}; + std::normal_distribution d{m, s}; + return d(gen); +} + +inline uint32_t normal_d_bounded(const double m, const double s, + const double b = 1 << 20) { + std::random_device rd; + std::mt19937 gen{rd()}; + std::normal_distribution d{m, s}; + double g = d(gen); + while (g < 0 || g >= b) g = d(gen); + return static_cast(g); +} + +// TODO: Extract all this to data generator with rd, gen etc cached +template +std::array normal_arr(const double& m, const double& s) { + std::array arr; + for (size_t i = 0; i < N; ++i) { + arr[i] = normal_d(m, s); + } + return arr; +} + +// set field to random values, with mask_bits specifienging 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)); + }); + } + } +}; + +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) { + // Closer to reality that RANGE is just 20bits and not all 32 + ouster::impl::visit_field(ls, f.first, set_to_random(), 20); + } else { + ouster::impl::visit_field(ls, f.first, set_to_random()); + } + } + + ls.frame_id = 5; + + const int64_t t_start = 100; + const int64_t dt = 100 * 1000 / (ls.w - 1); + for (ptrdiff_t i = 0; i < ls.w; ++i) { + if (i == 0) + ls.timestamp()[i] = t_start; + else + ls.timestamp()[i] = ls.timestamp()[i - 1] + dt; + ls.status()[i] = static_cast( + (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(); + } +} + +inline LidarScan get_random_lidar_scan( + const size_t w = 1024, const size_t h = 64, + sensor::UDPProfileLidar profile = + sensor::UDPProfileLidar::PROFILE_RNG19_RFL8_SIG16_NIR16_DUAL) { + LidarScan ls{w, h, profile}; + random_lidar_scan_data(ls); + return ls; +} + +inline LidarScan get_random_lidar_scan(const size_t w = 1024, + const size_t h = 64, + LidarScanFieldTypes field_types = {}) { + LidarScan ls{w, h, field_types.begin(), field_types.end()}; + random_lidar_scan_data(ls); + return ls; +} + +inline LidarScan get_random_lidar_scan(const sensor::sensor_info& si) { + return get_random_lidar_scan(si.format.columns_per_frame, + si.format.pixels_per_column, + si.format.udp_profile_lidar); +} + +} // namespace osf +} // namespace ouster diff --git a/ouster_osf/tests/crc_test.cpp b/ouster_osf/tests/crc_test.cpp new file mode 100644 index 00000000..9e904e6d --- /dev/null +++ b/ouster_osf/tests/crc_test.cpp @@ -0,0 +1,37 @@ +/** + * Copyright(c) 2021, Ouster, Inc. + * All rights reserved. + */ + +#include +#include + +#include + +#include + +#include +#include + +#include "ouster/osf/crc32.h" + + +namespace ouster { +namespace osf { +namespace { + +class CrcTest : public ::testing::Test {}; + +TEST_F(CrcTest, SmokeSanityCheck) { + const std::vector data = {0, 1, 2, 3, 4, 5, 6, 7}; + const uint32_t crc = osf::crc32(data.data(), data.size()); + EXPECT_EQ(0x88aa689f, crc); + + const std::vector data_rev(data.rbegin(), data.rend()); + const uint32_t crc_rev = osf::crc32(data_rev.data(), data_rev.size()); + EXPECT_EQ(0xa1509ef8, crc_rev); +} + +} // namespace +} // namespace osf +} // namespace ouster diff --git a/ouster_osf/tests/file_ops_test.cpp b/ouster_osf/tests/file_ops_test.cpp new file mode 100644 index 00000000..fb1eaca5 --- /dev/null +++ b/ouster_osf/tests/file_ops_test.cpp @@ -0,0 +1,138 @@ +/** + * Copyright(c) 2021, Ouster, Inc. + * All rights reserved. + */ + +#include + +#include +#include + +#include "common.h" +#include "osf_test.h" +#include "ouster/osf/meta_lidar_sensor.h" +#include "ouster/osf/reader.h" +#include "ouster/osf/stream_lidar_scan.h" +#include "ouster/osf/writer.h" + +namespace ouster { +namespace osf { +namespace { + +class FileOpsTest : public OsfTestWithDataAndFiles {}; + +TEST_F(FileOpsTest, TempDir) { + std::string tmp_dir; + EXPECT_TRUE(make_tmp_dir(tmp_dir)); + EXPECT_TRUE(path_exists(tmp_dir)); + EXPECT_TRUE(is_dir(tmp_dir)); + + EXPECT_TRUE(remove_dir(tmp_dir)); + + EXPECT_FALSE(path_exists(tmp_dir)); + EXPECT_FALSE(is_dir(tmp_dir)); + + EXPECT_FALSE(unlink_path(tmp_dir)); + EXPECT_FALSE(remove_dir(tmp_dir)); +} + +TEST_F(FileOpsTest, TempAndMakeDir) { + std::string tmp_dir; + EXPECT_TRUE(make_tmp_dir(tmp_dir)); + std::string tmp_dir_new = path_concat(tmp_dir, "new_dir"); + EXPECT_FALSE(path_exists(tmp_dir_new)); + EXPECT_FALSE(is_dir(tmp_dir_new)); + + EXPECT_TRUE(make_dir(tmp_dir_new)); + + EXPECT_TRUE(path_exists(tmp_dir_new)); + EXPECT_TRUE(is_dir(tmp_dir_new)); + + // Can't remove non-empty dir + EXPECT_FALSE(remove_dir(tmp_dir)); + EXPECT_FALSE(unlink_path(tmp_dir)); + + EXPECT_TRUE(remove_dir(tmp_dir_new)); + EXPECT_FALSE(path_exists(tmp_dir_new)); + EXPECT_FALSE(is_dir(tmp_dir_new)); + + EXPECT_TRUE(remove_dir(tmp_dir)); +} + +TEST_F(FileOpsTest, IsDirGeneral) { + EXPECT_FALSE(is_dir("")); + EXPECT_TRUE(is_dir(".")); +} + +TEST_F(FileOpsTest, PathConcats) { + EXPECT_EQ("", path_concat("", "")); + EXPECT_EQ("hello", path_concat("", "hello")); + EXPECT_EQ("hello", path_concat("hello", "")); +#ifdef _WIN32 + EXPECT_EQ("c:\\b", path_concat("c:", "b")); + EXPECT_EQ("/a/b\\f/g/", path_concat("/a/b//", "f/g/")); + EXPECT_EQ("/f/g/", path_concat("/a/b//", "/f/g/")); + EXPECT_EQ("f:/g/", path_concat("/a/b//", "f:/g/")); +#else + EXPECT_EQ("/", path_concat("/", "")); + EXPECT_EQ("////", path_concat("////", "")); + EXPECT_EQ("//", path_concat("//", "")); + EXPECT_EQ("/a", path_concat("//", "a")); + EXPECT_EQ("/a", path_concat("//", "a")); + EXPECT_EQ("/a", path_concat("//b", "/a")); + EXPECT_EQ("/", path_concat("", "/")); + EXPECT_EQ("//", path_concat("", "//")); + EXPECT_EQ("////", path_concat("/", "////")); + EXPECT_EQ("/a/b", path_concat("/a\\", "b")); + EXPECT_EQ("/a/b", path_concat("/a\\//", "b")); + EXPECT_EQ("c:/b", path_concat("c:", "b")); + EXPECT_EQ("/a/b/f", path_concat("/a/b/", "f")); + EXPECT_EQ("/a/b/f/g", path_concat("/a/b/", "f/g")); + EXPECT_EQ("/a/b/f/g/", path_concat("/a/b/", "f/g/")); + EXPECT_EQ("/a/b/f/g/", path_concat("/a/b//", "f/g/")); + EXPECT_EQ("/f/g/", path_concat("/a/b//", "/f/g/")); + // Yes its weird and this function just can't be ideal ... + EXPECT_EQ("/a/b/f:/g/", path_concat("/a/b//", "f:/g/")); +#endif +} + +TEST_F(FileOpsTest, TestDataDirCheck) { + EXPECT_TRUE(is_dir(test_data_dir())); +} + +TEST_F(FileOpsTest, TestFileSize) { + // TODO[pb]: Change to file creation later ... + const std::string test_file_name = + path_concat(test_data_dir(), "osfs/OS-1-128_v2.3.0_1024x10_lb_n3.osf"); + int64_t fsize = file_size(test_file_name); + EXPECT_EQ(1021684, fsize); + std::string not_a_file = path_concat(test_data_dir(), "not_a_file"); + EXPECT_TRUE(file_size(not_a_file) < 0); + EXPECT_TRUE(file_size(test_data_dir()) < 0); +} + +TEST_F(FileOpsTest, TestFileMapping) { + // TODO[pb]: Change to file creation later ... + const std::string test_file_name = + path_concat(test_data_dir(), "osfs/OS-1-128_v2.3.0_1024x10_lb_n3.osf"); + + uint8_t* file_buf = mmap_open(test_file_name); + EXPECT_TRUE(file_buf != nullptr); + + int64_t fsize = file_size(test_file_name); + EXPECT_EQ(1021684, fsize); + + if (file_buf != nullptr) { + std::cout << "bytes = " << to_string(file_buf, 64) << std::endl; + std::cout << "bytes = " << to_string(file_buf + 4, 64) << std::endl; + std::cout << "bytes = " << to_string(file_buf + 4 + 4, 64) << std::endl; + std::cout << "bytes = " << to_string(file_buf + fsize - 64, 64) + << std::endl; + } + + EXPECT_TRUE(mmap_close(file_buf, fsize)); +} + +} // namespace +} // namespace osf +} // namespace ouster diff --git a/ouster_osf/tests/file_test.cpp b/ouster_osf/tests/file_test.cpp new file mode 100644 index 00000000..fab98039 --- /dev/null +++ b/ouster_osf/tests/file_test.cpp @@ -0,0 +1,175 @@ +/** + * Copyright(c) 2021, Ouster, Inc. + * All rights reserved. + */ + +#include "ouster/osf/file.h" + +#include + +#include "ouster/osf/basics.h" +#include "osf_test.h" + +#include "fb_utils.h" + +namespace ouster { +namespace osf { +namespace { + +class OsfFileTest : public OsfTestWithData {}; + +TEST_F(OsfFileTest, OpensOsfFileDefaultAsBadState) { + // This opens nothing and produces the file in a !good() state + OsfFile osf_file; + + EXPECT_FALSE(osf_file.good()); + + // Check operator! + if (!osf_file) { + SUCCEED(); + } + + // Check bool operator + bool ok = osf_file.good(); + if (ok) FAIL(); +} + + +TEST_F(OsfFileTest, OpenOsfFileNominally) { + OsfFile osf_file( + path_concat(test_data_dir(), "osfs/OS-1-128_v2.3.0_1024x10_lb_n3.osf")); + EXPECT_TRUE(osf_file); + EXPECT_EQ(osf_file.version(), OSF_VERSION::V_2_0); + EXPECT_EQ(osf_file.size(), 1021684); + EXPECT_EQ(osf_file.offset(), 0); + + EXPECT_EQ(osf_file.metadata_offset(), 1013976); + std::cout << "file = " << osf_file.to_string() << std::endl; + + + EXPECT_EQ(osf_file.seek(100).offset(), 100); + + // Out of range seek should throw + EXPECT_THROW(osf_file.seek(50000000), std::out_of_range); + + // OSF v2 OsfFile should be OK + EXPECT_TRUE(osf_file.valid()); + + + // Test move semantics + OsfFile osff(std::move(osf_file)); + EXPECT_FALSE(osf_file.good()); + EXPECT_FALSE(osf_file.valid()); + + EXPECT_EQ(osff.offset(), 100); + EXPECT_TRUE(osff.good()); + + OsfFile osf_new; + EXPECT_FALSE(osf_new.good()); + osf_new = std::move(osff); + EXPECT_TRUE(osf_new.good()); + EXPECT_FALSE(osff); + + EXPECT_EQ(osf_new.seek(1001).offset(), 1001); + EXPECT_TRUE(osf_new.valid()); + + if (osf_new.is_memory_mapped()) { + const uint8_t* b = osf_new.buf(); + EXPECT_TRUE(b != nullptr); + } + + // Read header size from the beginning of the file + uint8_t size_buf[4]; + osf_new.seek(0).read(size_buf, 4); + EXPECT_EQ(osf_new.offset(), 4); + + size_t header_size = get_prefixed_size(size_buf); + + // Header length is always 52 bytes (0x34) + EXPECT_EQ(header_size, 52); + + // Close copied out + osff.close(); + EXPECT_FALSE(osff.good()); + EXPECT_FALSE(osff); + + // Close osf dest istance + osf_new.close(); + EXPECT_FALSE(osf_new.good()); + EXPECT_FALSE(osf_new); +} + + +TEST_F(OsfFileTest, OpenOsfFileWithStandardRead) { + OsfFile osf_file( + path_concat(test_data_dir(), "osfs/OS-1-128_v2.3.0_1024x10_lb_n3.osf")); + EXPECT_TRUE(osf_file); + EXPECT_EQ(osf_file.version(), OSF_VERSION::V_2_0); + EXPECT_EQ(osf_file.size(), 1021684); + EXPECT_EQ(osf_file.offset(), 0); + + EXPECT_EQ(osf_file.metadata_offset(), 1013976); + std::cout << "file = " << osf_file.to_string() << std::endl; + + EXPECT_TRUE(osf_file.valid()); +} + +TEST_F(OsfFileTest, OpenOsfFileHandleNonExistent) { + const std::string test_file_name = + path_concat(test_data_dir(), "non-file-thing"); + + OsfFile osf_file(test_file_name); + EXPECT_FALSE(osf_file); + + EXPECT_EQ(0, osf_file.size()); + EXPECT_EQ(test_file_name, osf_file.filename()); + EXPECT_EQ(OSF_VERSION::V_INVALID, osf_file.version()); + EXPECT_EQ(0, osf_file.offset()); + + // Access to a bad file is an error + ASSERT_THROW(osf_file.buf(), std::logic_error); + ASSERT_THROW(osf_file.buf(1), std::logic_error); + ASSERT_THROW(osf_file.seek(10), std::logic_error); + + uint8_t buf[10]; + ASSERT_THROW(osf_file.read(buf, 10), std::logic_error); +} + +TEST_F(OsfFileTest, OsfFileDontOpenDir) { + const std::string test_file_dir = test_data_dir(); + + OsfFile osf_file(test_file_dir); + EXPECT_FALSE(osf_file); + + ASSERT_THROW(osf_file.buf(), std::logic_error); +} + +TEST_F(OsfFileTest, OsfFileCheckOutOfRangeAccess) { + const std::string test_file_name = + path_concat(test_data_dir(), "osfs/OS-1-128_v2.3.0_1024x10_lb_n3.osf"); + + OsfFile osf_file(test_file_name); + EXPECT_TRUE(osf_file); + + // Read buffer for read() + constexpr int kBufSize = 10; + uint8_t buf[kBufSize]; + + // In range file access + if (osf_file.is_memory_mapped()) { + ASSERT_NO_THROW(osf_file.buf(100)); + } + ASSERT_NO_THROW(osf_file.seek(100).seek(0)); + ASSERT_NO_THROW(osf_file.read(buf, kBufSize)); + + // Out of range file access + if (osf_file.is_memory_mapped()) { + ASSERT_THROW(osf_file.buf(100000000), std::out_of_range); + } + ASSERT_THROW(osf_file.seek(100000000), std::out_of_range); + ASSERT_THROW(osf_file.read(buf, 100000000), std::out_of_range); +} + +} // namespace +} // namespace osf +} // namespace ouster \ No newline at end of file diff --git a/ouster_osf/tests/operations_test.cpp b/ouster_osf/tests/operations_test.cpp new file mode 100644 index 00000000..c88f9b83 --- /dev/null +++ b/ouster_osf/tests/operations_test.cpp @@ -0,0 +1,80 @@ +/** + * Copyright(c) 2021, Ouster, Inc. + * All rights reserved. + */ + +#include "ouster/osf/operations.h" + +#include + +#include "json_utils.h" +#include "osf_test.h" +#include "ouster/osf/basics.h" +#include "ouster/osf/file.h" +#include "ouster/osf/meta_lidar_sensor.h" +#include "ouster/osf/reader.h" +#include "ouster/osf/stream_lidar_scan.h" + +namespace ouster { +namespace osf { +namespace { + +class OperationsTest : public OsfTestWithDataAndFiles {}; + +TEST_F(OperationsTest, GetOsfDumpInfo) { + std::string osf_info_str = dump_metadata( + path_concat(test_data_dir(), "osfs/OS-1-128_v2.3.0_1024x10_lb_n3.osf"), + true); + + Json::Value osf_info_obj{}; + + EXPECT_TRUE(parse_json(osf_info_str, osf_info_obj)); + + ASSERT_TRUE(osf_info_obj.isMember("header")); + EXPECT_TRUE(osf_info_obj["header"].isMember("status")); + EXPECT_TRUE(osf_info_obj["header"].isMember("version")); + EXPECT_TRUE(osf_info_obj["header"].isMember("size")); + EXPECT_TRUE(osf_info_obj["header"].isMember("metadata_offset")); + EXPECT_TRUE(osf_info_obj["header"].isMember("chunks_offset")); + + ASSERT_TRUE(osf_info_obj.isMember("metadata")); + EXPECT_TRUE(osf_info_obj["metadata"].isMember("id")); + EXPECT_EQ("from_pcap pythonic", osf_info_obj["metadata"]["id"].asString()); + EXPECT_TRUE(osf_info_obj["metadata"].isMember("start_ts")); + EXPECT_TRUE(osf_info_obj["metadata"].isMember("end_ts")); + EXPECT_TRUE(osf_info_obj["metadata"].isMember("entries")); + EXPECT_EQ(3, osf_info_obj["metadata"]["entries"].size()); +} + +TEST_F(OperationsTest, ParseAndPrintSmoke) { + parse_and_print( + path_concat(test_data_dir(), "osfs/OS-1-128_v2.3.0_1024x10_lb_n3.osf")); +} + +// TODO[pb]: Remove this test and remove PcapRawSource since it's not mathing the python impl. +TEST_F(OperationsTest, PcapToOsf) { + 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"); + + std::string output_osf_filename = tmp_file("pcap_to_osf_test.osf"); + + bool res = pcap_to_osf(pcap_file, meta_file, 7502, output_osf_filename); + + EXPECT_TRUE(res); + + OsfFile output_osf_file{output_osf_filename}; + EXPECT_TRUE(output_osf_file.valid()); + + Reader reader{output_osf_file}; + + auto msgs_count = + std::distance(reader.messages().begin(), reader.messages().end()); + EXPECT_EQ(2, msgs_count); +} + +} // namespace + +} // namespace osf +} // namespace ouster diff --git a/ouster_osf/tests/osf_test.h b/ouster_osf/tests/osf_test.h new file mode 100644 index 00000000..ca200d2a --- /dev/null +++ b/ouster_osf/tests/osf_test.h @@ -0,0 +1,59 @@ +/** + * Copyright(c) 2021, Ouster, Inc. + * All rights reserved. + */ + +#pragma once + +#include + +#include "common.h" + +namespace ouster { +namespace osf { + +// Base class for all osf util tests +class OsfTest : public ::testing::Test {}; + +// Test fixture to get and check test data dir +// use it for tests that needed test data +class OsfTestWithData : public OsfTest { + protected: + virtual void SetUp() { + if (!get_test_data_dir(test_data_dir_)) { + FAIL() << "Can't get DATA_DIR"; + return; + } + } + + std::string test_data_dir() { return test_data_dir_; } + + private: + std::string test_data_dir_; +}; + +class OsfTestWithDataAndFiles : public osf::OsfTestWithData { + public: + static std::string output_dir; + static std::vector files; + std::string tmp_file(const std::string& basename) { + std::string res = path_concat(output_dir, basename); + // TODO[pb]: Switch to map? to avoid overlaps/double delete + files.push_back(res); + return res; + } + static void SetUpTestCase() { + if (!make_tmp_dir(output_dir)) FAIL(); + } + + // clean up temp files + static void TearDownTestCase() { + for (const auto& path : files) unlink_path(path); + remove_dir(output_dir); + } +}; +std::string OsfTestWithDataAndFiles::output_dir = {}; +std::vector OsfTestWithDataAndFiles::files = {}; + +} // namespace osf +} // namespace ouster diff --git a/ouster_osf/tests/pcap_source_test.cpp b/ouster_osf/tests/pcap_source_test.cpp new file mode 100644 index 00000000..53eae1c7 --- /dev/null +++ b/ouster_osf/tests/pcap_source_test.cpp @@ -0,0 +1,44 @@ +/** + * 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 new file mode 100644 index 00000000..c114ef20 --- /dev/null +++ b/ouster_osf/tests/png_tools_test.cpp @@ -0,0 +1,239 @@ +/** + * Copyright(c) 2021, Ouster, Inc. + * All rights reserved. + */ + +#include +#include +#include + +#include +#include +#include +#include + +#include "ouster/types.h" +#include "ouster/lidar_scan.h" + +#include "common.h" +#include "osf_test.h" + +#include "png_tools.h" +#include "ouster/osf/basics.h" + +namespace ouster { +namespace osf { +namespace { + +class OsfPngToolsTest : public OsfTestWithDataAndFiles {}; + +using ouster::sensor::sensor_info; +using ouster::sensor::lidar_mode; + +size_t field_size(LidarScan& ls, sensor::ChanField f) { + switch (ls.field_type(f)) { + case sensor::ChanFieldType::UINT8: + return ls.field(f).size(); + break; + case sensor::ChanFieldType::UINT16: + return ls.field(f).size(); + break; + case sensor::ChanFieldType::UINT32: + return ls.field(f).size(); + break; + case sensor::ChanFieldType::UINT64: + return ls.field(f).size(); + break; + default: + return 0; + break; + } +} + +// Check that we can make lidar scan and have fields with expected value inside +TEST_F(OsfPngToolsTest, MakesLidarScan) { + const sensor_info si = sensor::metadata_from_json( + path_concat(test_data_dir(), "pcaps/OS-1-128_v2.3.0_1024x10.json")); + + LidarScan ls = get_random_lidar_scan(si); + + const auto n = + si.format.columns_per_frame * si.format.pixels_per_column; + + 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); + } + EXPECT_EQ(ls.status().size(), si.format.columns_per_frame); +} + +#define ENCODE_IMAGE_TEST(TEST_NAME, ENCODE_FUNC, DECODE_FUNC) \ + template \ + struct TEST_NAME { \ + template \ + bool to(const LidarScan& ls, const std::vector& px_offset, \ + Ti mask_bits = 0) { \ + ScanChannelData encoded_channel; \ + img_t key_orig(ls.h, ls.w); \ + key_orig = key_orig.unaryExpr([=](Ti) { \ + double sr = static_cast(std::rand()) / RAND_MAX; \ + return static_cast( \ + sr * static_cast(std::numeric_limits::max())); \ + }); \ + if (mask_bits && sizeof(Ti) * 8 > mask_bits) { \ + key_orig = key_orig.unaryExpr([=](Ti a) { \ + return static_cast(a & ((1LL << mask_bits) - 1)); \ + }); \ + } \ + bool res_enc = \ + ENCODE_FUNC(encoded_channel, key_orig, px_offset); \ + EXPECT_FALSE(res_enc); \ + std::cout << #ENCODE_FUNC \ + << ": encoded bytes = " << encoded_channel.size() \ + << " ================= " << std::endl; \ + EXPECT_TRUE(!encoded_channel.empty()); \ + img_t decoded_img{ls.h, ls.w}; \ + bool res_dec = \ + DECODE_FUNC(decoded_img, encoded_channel, px_offset); \ + EXPECT_FALSE(res_dec); \ + bool round_trip = (key_orig.template cast() == \ + decoded_img.template cast()) \ + .all(); \ + auto round_trip_cnt = (key_orig.template cast() == \ + decoded_img.template cast()) \ + .count(); \ + std::cout << "cnt = " << round_trip_cnt << std::endl; \ + return round_trip; \ + } \ + }; + +ENCODE_IMAGE_TEST(test8bitImageCoders, encode8bitImage, decode8bitImage) +ENCODE_IMAGE_TEST(test16bitImageCoders, encode16bitImage, decode16bitImage) +ENCODE_IMAGE_TEST(test24bitImageCoders, encode24bitImage, decode24bitImage) +ENCODE_IMAGE_TEST(test32bitImageCoders, encode32bitImage, decode32bitImage) +ENCODE_IMAGE_TEST(test64bitImageCoders, encode64bitImage, decode64bitImage) + +// Check encodeXXbitImage functions on RANGE fields of LidarScan +// converted to various img_t of different unsigned int sizes. +TEST_F(OsfPngToolsTest, ImageCoders) { + const sensor_info si = sensor::metadata_from_json( + path_concat(test_data_dir(), "pcaps/OS-1-128_v2.3.0_1024x10.json")); + LidarScan ls = get_random_lidar_scan(si); + auto px_offset = si.format.pixel_shift_by_row; + + // ======== 8bit ========== + + EXPECT_TRUE(test8bitImageCoders().to(ls, px_offset, 8)); + EXPECT_TRUE(test8bitImageCoders().to(ls, px_offset, 8)); + EXPECT_TRUE(test8bitImageCoders().to(ls, px_offset, 8)); + EXPECT_TRUE(test8bitImageCoders().to(ls, px_offset, 8)); + + EXPECT_TRUE(test8bitImageCoders().to(ls, px_offset, 8)); + EXPECT_TRUE(test8bitImageCoders().to(ls, px_offset, 8)); + EXPECT_TRUE(test8bitImageCoders().to(ls, px_offset, 8)); + EXPECT_TRUE(test8bitImageCoders().to(ls, px_offset, 8)); + + EXPECT_TRUE(test8bitImageCoders().to(ls, px_offset, 8)); + EXPECT_TRUE(test8bitImageCoders().to(ls, px_offset, 8)); + EXPECT_TRUE(test8bitImageCoders().to(ls, px_offset, 8)); + EXPECT_TRUE(test8bitImageCoders().to(ls, px_offset, 8)); + + EXPECT_TRUE(test8bitImageCoders().to(ls, px_offset, 8)); + EXPECT_TRUE(test8bitImageCoders().to(ls, px_offset, 8)); + EXPECT_TRUE(test8bitImageCoders().to(ls, px_offset, 8)); + EXPECT_TRUE(test8bitImageCoders().to(ls, px_offset, 8)); + + // ======== 16bit ====== + + EXPECT_TRUE(test16bitImageCoders().to(ls, px_offset, 16)); + EXPECT_TRUE(test16bitImageCoders().to(ls, px_offset, 16)); + EXPECT_TRUE(test16bitImageCoders().to(ls, px_offset, 16)); + EXPECT_TRUE(test16bitImageCoders().to(ls, px_offset, 16)); + + EXPECT_FALSE(test16bitImageCoders().to(ls, px_offset, 16)); + EXPECT_TRUE(test16bitImageCoders().to(ls, px_offset, 16)); + EXPECT_TRUE(test16bitImageCoders().to(ls, px_offset, 16)); + EXPECT_TRUE(test16bitImageCoders().to(ls, px_offset, 16)); + + EXPECT_FALSE(test16bitImageCoders().to(ls, px_offset, 16)); + EXPECT_TRUE(test16bitImageCoders().to(ls, px_offset, 16)); + EXPECT_TRUE(test16bitImageCoders().to(ls, px_offset, 16)); + EXPECT_TRUE(test16bitImageCoders().to(ls, px_offset, 16)); + + EXPECT_FALSE(test16bitImageCoders().to(ls, px_offset, 16)); + EXPECT_TRUE(test16bitImageCoders().to(ls, px_offset, 16)); + EXPECT_TRUE(test16bitImageCoders().to(ls, px_offset, 16)); + EXPECT_TRUE(test16bitImageCoders().to(ls, px_offset, 16)); + + // ======== 24bit ====== + + EXPECT_TRUE(test24bitImageCoders().to(ls, px_offset, 24)); + EXPECT_TRUE(test24bitImageCoders().to(ls, px_offset, 24)); + EXPECT_TRUE(test24bitImageCoders().to(ls, px_offset, 24)); + EXPECT_TRUE(test24bitImageCoders().to(ls, px_offset, 24)); + + EXPECT_FALSE(test24bitImageCoders().to(ls, px_offset, 24)); + EXPECT_TRUE(test24bitImageCoders().to(ls, px_offset, 24)); + EXPECT_TRUE(test24bitImageCoders().to(ls, px_offset, 24)); + EXPECT_TRUE(test24bitImageCoders().to(ls, px_offset, 24)); + + EXPECT_FALSE(test24bitImageCoders().to(ls, px_offset, 24)); + EXPECT_FALSE(test24bitImageCoders().to(ls, px_offset, 24)); + EXPECT_TRUE(test24bitImageCoders().to(ls, px_offset, 24)); + EXPECT_TRUE(test24bitImageCoders().to(ls, px_offset, 24)); + + EXPECT_FALSE(test24bitImageCoders().to(ls, px_offset, 24)); + EXPECT_FALSE(test24bitImageCoders().to(ls, px_offset, 24)); + EXPECT_TRUE(test24bitImageCoders().to(ls, px_offset, 24)); + EXPECT_TRUE(test24bitImageCoders().to(ls, px_offset, 24)); + + // ======== 32bit ====== + + EXPECT_TRUE(test32bitImageCoders().to(ls, px_offset, 32)); + EXPECT_TRUE(test32bitImageCoders().to(ls, px_offset, 32)); + EXPECT_TRUE(test32bitImageCoders().to(ls, px_offset, 32)); + EXPECT_TRUE(test32bitImageCoders().to(ls, px_offset, 32)); + + EXPECT_FALSE(test32bitImageCoders().to(ls, px_offset, 32)); + EXPECT_TRUE(test32bitImageCoders().to(ls, px_offset, 32)); + EXPECT_TRUE(test32bitImageCoders().to(ls, px_offset, 32)); + EXPECT_TRUE(test32bitImageCoders().to(ls, px_offset, 32)); + + EXPECT_FALSE(test32bitImageCoders().to(ls, px_offset, 32)); + EXPECT_FALSE(test32bitImageCoders().to(ls, px_offset, 32)); + EXPECT_TRUE(test32bitImageCoders().to(ls, px_offset, 32)); + EXPECT_TRUE(test32bitImageCoders().to(ls, px_offset, 32)); + + EXPECT_FALSE(test32bitImageCoders().to(ls, px_offset, 32)); + EXPECT_FALSE(test32bitImageCoders().to(ls, px_offset, 32)); + EXPECT_TRUE(test32bitImageCoders().to(ls, px_offset, 32)); + EXPECT_TRUE(test32bitImageCoders().to(ls, px_offset, 32)); + + // ======== 64bit ====== + + EXPECT_TRUE(test64bitImageCoders().to(ls, px_offset, 64)); + EXPECT_TRUE(test64bitImageCoders().to(ls, px_offset, 64)); + EXPECT_TRUE(test64bitImageCoders().to(ls, px_offset, 64)); + EXPECT_TRUE(test64bitImageCoders().to(ls, px_offset, 64)); + + EXPECT_FALSE(test64bitImageCoders().to(ls, px_offset, 64)); + EXPECT_TRUE(test64bitImageCoders().to(ls, px_offset, 64)); + EXPECT_TRUE(test64bitImageCoders().to(ls, px_offset, 64)); + EXPECT_TRUE(test64bitImageCoders().to(ls, px_offset, 64)); + + EXPECT_FALSE(test64bitImageCoders().to(ls, px_offset, 64)); + EXPECT_FALSE(test64bitImageCoders().to(ls, px_offset, 64)); + EXPECT_TRUE(test64bitImageCoders().to(ls, px_offset, 64)); + EXPECT_TRUE(test64bitImageCoders().to(ls, px_offset, 64)); + + EXPECT_FALSE(test64bitImageCoders().to(ls, px_offset, 64)); + EXPECT_FALSE(test64bitImageCoders().to(ls, px_offset, 64)); + EXPECT_FALSE(test64bitImageCoders().to(ls, px_offset, 64)); + EXPECT_TRUE(test64bitImageCoders().to(ls, px_offset, 64)); + +} + +} // namespace +} // namespace OSF +} // namespace ouster diff --git a/ouster_osf/tests/reader_test.cpp b/ouster_osf/tests/reader_test.cpp new file mode 100644 index 00000000..ff43ab19 --- /dev/null +++ b/ouster_osf/tests/reader_test.cpp @@ -0,0 +1,171 @@ +/** + * Copyright(c) 2021, Ouster, Inc. + * All rights reserved. + */ + +#include "ouster/osf/reader.h" + +#include + +#include "common.h" +#include "osf_test.h" +#include "ouster/osf/meta_lidar_sensor.h" +#include "ouster/osf/stream_lidar_scan.h" + +namespace ouster { +namespace osf { +namespace { + +class ReaderTest : public osf::OsfTestWithData {}; + +TEST_F(ReaderTest, Basics) { + OsfFile osf_file( + path_concat(test_data_dir(), "osfs/OS-1-128_v2.3.0_1024x10_lb_n3.osf")); + + Reader reader(osf_file); + + EXPECT_EQ("from_pcap pythonic", reader.id()); + EXPECT_EQ(991587364520LL, reader.start_ts().count()); + EXPECT_EQ(991787323080LL, reader.end_ts().count()); + + // Get first sensor (it's the first by metadata_id) (i.e. first added) + auto sensor = reader.meta_store().get(); + EXPECT_TRUE(sensor); + + EXPECT_EQ(1, reader.meta_store().count()); + + EXPECT_EQ(3, std::distance(reader.messages_standard().begin(), + reader.messages_standard().end())); + + const MetadataStore& meta_store = reader.meta_store(); + EXPECT_EQ(3, meta_store.size()); +} + +TEST_F(ReaderTest, ChunksReading) { + OsfFile osf_file( + path_concat(test_data_dir(), "osfs/OS-1-128_v2.3.0_1024x10_lb_n3.osf")); + + Reader reader(osf_file); + + auto chunks = reader.chunks(); + + EXPECT_EQ(1, std::distance(chunks.begin(), chunks.end())); + + auto first_chunk_it = chunks.begin(); + EXPECT_EQ(3, first_chunk_it->size()); + + EXPECT_EQ(3, std::distance(first_chunk_it->begin(), first_chunk_it->end())); + + auto msg_it = first_chunk_it->begin(); + auto msg0 = *msg_it; + auto msg1 = *(++msg_it); + EXPECT_NE(msg0, msg1); + + auto msg00 = *(--msg_it); + EXPECT_EQ(msg0, msg00); + EXPECT_EQ(msg0, *(--msg_it)); +} + +TEST_F(ReaderTest, ChunksPileBasics) { + ChunkState st{}; + EXPECT_EQ(st.status, ChunkValidity::UNKNOWN); + + ChunksPile cp{}; + cp.add(0, ts_t{1}, ts_t{2}); + cp.add(10, ts_t{10}, ts_t{20}); + EXPECT_FALSE(cp.get(2)); + EXPECT_TRUE(cp.get(0)); + EXPECT_EQ(2, cp.get(0)->end_ts.count()); + EXPECT_EQ(ChunkValidity::UNKNOWN, cp.get(0)->status); + + cp.add(12, ts_t{12}, ts_t{22}); + EXPECT_EQ(3, cp.size()); +} + +TEST_F(ReaderTest, MessagesReadingStandard) { + OsfFile osf_file( + path_concat(test_data_dir(), "osfs/OS-1-128_v2.3.0_1024x10_lb_n3.osf")); + + Reader reader(osf_file); + + const auto msgs = reader.messages_standard(); + EXPECT_EQ(3, std::distance(msgs.begin(), msgs.end())); + + // Chunks Iterator + auto chunks = reader.chunks(); + EXPECT_EQ(1, std::distance(chunks.begin(), chunks.end())); + + // Get messages from first chunks + auto first_chunk_it = chunks.begin(); + EXPECT_EQ(3, first_chunk_it->size()); + EXPECT_EQ(3, std::distance(first_chunk_it->begin(), first_chunk_it->end())); +} + +TEST_F(ReaderTest, MessagesReadingStreaming) { + OsfFile osf_file( + path_concat(test_data_dir(), "osfs/OS-1-128_v2.3.0_1024x10_lb_n3.osf")); + + Reader reader(osf_file); + + // Reader iterator reads as messages from chunk as they appears on a disk + int it_cnt = 0; + ts_t it_prev{0}; + bool it_ordered = true; + for (const auto msg : reader.messages_standard()) { + it_ordered = it_ordered && (it_prev <= msg.ts()); + ++it_cnt; + it_prev = msg.ts(); + } + + EXPECT_EQ(3, it_cnt); + + // only for this test file, ordering by timestamp while reading by chunks + // order as they layout in the file is not guaranteed + EXPECT_TRUE(it_ordered); + + // StreammingReader reads StreamingLayout OSFs in timestamp order + int sit_cnt = 0; + ts_t sit_prev{0}; + bool sit_ordered = true; + + for (const auto msg : reader.messages()) { + sit_ordered = sit_ordered && (sit_prev <= msg.ts()); + ++sit_cnt; + sit_prev = msg.ts(); + } + EXPECT_EQ(3, sit_cnt); + EXPECT_TRUE(sit_ordered); + + EXPECT_EQ( + 3, std::distance(reader.messages().begin(), reader.messages().end())); + + // Read time based range of messages from StreamingLayout + ts_t begin_ts{991587364520LL}; + ts_t end_ts{991587364520LL + 1 * 100'000'000LL}; // start_ts + 1 * 0.1s + for (const auto msg : reader.messages(begin_ts, end_ts)) { + EXPECT_TRUE(begin_ts <= msg.ts()); + EXPECT_TRUE(msg.ts() <= end_ts); + } + auto some_msgs = reader.messages(begin_ts, end_ts); + EXPECT_EQ(2, std::distance(some_msgs.begin(), some_msgs.end())); + + // Get any first LidarScan stream from OSF + auto lidar_scan_stream = reader.meta_store().get(); + EXPECT_TRUE(lidar_scan_stream); + + // Get a stream of LidarScan messages only + auto scan_msgs = reader.messages({lidar_scan_stream->id()}); + for (const auto msg : scan_msgs) { + EXPECT_EQ(lidar_scan_stream->id(), msg.id()); + } + EXPECT_EQ(3, std::distance(scan_msgs.begin(), scan_msgs.end())); + + // Get a stream of LidarScan messages only with start/end_ts params + auto scan_msgs_full = reader.messages({lidar_scan_stream->id()}, + reader.start_ts(), reader.end_ts()); + EXPECT_EQ(3, std::distance(scan_msgs_full.begin(), scan_msgs_full.end())); +} + +} // namespace +} // namespace osf +} // namespace ouster diff --git a/ouster_osf/tests/writer_custom_test.cpp b/ouster_osf/tests/writer_custom_test.cpp new file mode 100644 index 00000000..62224535 --- /dev/null +++ b/ouster_osf/tests/writer_custom_test.cpp @@ -0,0 +1,172 @@ +/** + * Copyright(c) 2021, Ouster, Inc. + * All rights reserved. + */ + +#include + +#include + +#include "osf_test.h" +#include "common.h" +#include "ouster/osf/file.h" +#include "ouster/osf/writer.h" +#include "ouster/osf/reader.h" + +namespace ouster { +namespace osf { + +class WriterCustomTest : public osf::OsfTestWithDataAndFiles {}; + +class MyNewMetaInfo : public MetadataEntryHelper { + public: + explicit MyNewMetaInfo(const std::string& text) : text_(text) {} + + const std::string& text() const { return text_; } + + // Pack to byte array + std::vector buffer() const final { + return {text_.begin(), text_.end()}; + } + + // UnPack from byte array + static std::unique_ptr from_buffer( + const std::vector& buf) { + std::string s(buf.begin(), buf.end()); + return std::make_unique(s); + } + + // Custom view for nice to_string() output + std::string repr() const override { return "text: '" + text_ + "'"; } + + private: + std::string text_; +}; + +template <> +struct MetadataTraits { + static const std::string type() { + return "ouster/v1/MyNewSuperMetaInfo"; + } +}; + +// TODO[pb]: Define a StreamTagHelper for just dummy types/tags for use in +// custom stream definitions +class YoStreamMeta : public MetadataEntryHelper { + public: + YoStreamMeta() {} + std::vector buffer() const final { return {}; }; + static std::unique_ptr from_buffer( + const std::vector&) { + return std::make_unique(); + } + +}; + +template <> +struct MetadataTraits { + static const std::string type() { + return "ouster/v1/YoStream"; + } +}; + +// Message object of YoStream +struct yo { + uint8_t a; +}; + +// Define custom stream with the message type `yo` +class YoStream : public MessageStream { + public: + YoStream(Writer& writer) : writer_{writer}, meta_{} { + stream_meta_id_ = writer_.addMetadata(meta_); + }; + + // Boilerplate for writer + void save(const ouster::osf::ts_t ts, const obj_type& yo_obj) { + const auto& msg_buf = make_msg(yo_obj); + writer_.saveMessage(meta_.id(), ts, msg_buf); + } + + // Pack yo message into buffer + std::vector make_msg(const obj_type& yo_obj) { + return { yo_obj.a }; + } + + // UnPack yo message from buffer + static std::unique_ptr decode_msg(const std::vector& buf, + const meta_type&, + const MetadataStore&) { + auto y = std::make_unique(); + y->a = buf[0]; + return y; + } + + private: + Writer& writer_; + + meta_type meta_; + + uint32_t stream_meta_id_{0}; + +}; + +TEST_F(WriterCustomTest, WriteCustomMsgExample) { + + std::string output_osf_filename = tmp_file("writer_new_meta_info_msg.osf"); + + // Create OSF v2 Writer + osf::Writer writer(output_osf_filename, "Yo Example"); + + // Create LidarSensor record + writer.addMetadata("Happy New Year!"); + + // Create stream for `yo` objects + auto yo_stream = writer.createStream(); + + uint8_t yo_cnt = 0; + while (yo_cnt < 100) { + // `yo` object + yo y; + y.a = (uint8_t)yo_cnt; + + // Save `yo` object into stream + ts_t ts{yo_cnt * 10}; + yo_stream.save(ts, y); + + ++yo_cnt; + } + + writer.close(); + + OsfFile file(output_osf_filename); + osf::Reader reader(file); + + // Read all messages from OSF file + int msg_cnt = 0; + for (const auto m: reader.messages()) { + // Decoding messages + if (m.is()) { + auto y = *m.decode_msg(); + // Check `yo` msgs are the same and in the same order as written + EXPECT_EQ(msg_cnt, y.a); + // std::cout << "yo = " << (int)y.a << std::endl; + } + ++msg_cnt; + } + EXPECT_EQ(100, msg_cnt); + + auto my_metas = reader.meta_store().find(); + EXPECT_EQ(1, my_metas.size()); + + // Get MyNewMetaInfo metadata + auto my_meta = my_metas.begin()->second; + EXPECT_EQ(my_meta->text(), "Happy New Year!"); + + std::cout << "my_meta = " << my_meta->to_string() << std::endl; + // std::cout << "output = " << output_osf_filename << std::endl; + +} + +} // namespace osf +} // namespace ouster \ No newline at end of file diff --git a/ouster_osf/tests/writer_test.cpp b/ouster_osf/tests/writer_test.cpp new file mode 100644 index 00000000..4a470103 --- /dev/null +++ b/ouster_osf/tests/writer_test.cpp @@ -0,0 +1,547 @@ +/** + * Copyright(c) 2021, Ouster, Inc. + * All rights reserved. + */ + +#include "ouster/osf/writer.h" + +#include + +#include + +#include "common.h" +#include "osf_test.h" +#include "ouster/types.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/meta_streaming_info.h" +#include "ouster/osf/reader.h" +#include "ouster/osf/stream_lidar_scan.h" + +namespace ouster { +namespace osf { +namespace { + +using ouster::sensor::sensor_info; +using ouster::osf::get_random_lidar_scan; + +class WriterTest : public osf::OsfTestWithDataAndFiles {}; + +TEST_F(WriterTest, ChunksLayoutEnum) { + ChunksLayout cl = ChunksLayout::LAYOUT_STANDARD; + EXPECT_EQ(to_string(cl), "STANDARD"); + ChunksLayout cl1{}; + EXPECT_EQ(to_string(cl1), "STANDARD"); + ChunksLayout cl2{LAYOUT_STREAMING}; + EXPECT_EQ(to_string(cl2), "STREAMING"); + EXPECT_EQ(chunks_layout_of_string("STREAMING"), LAYOUT_STREAMING); + EXPECT_EQ(chunks_layout_of_string("STANDARD"), LAYOUT_STANDARD); + EXPECT_EQ(chunks_layout_of_string("RRR"), LAYOUT_STANDARD); +} + +TEST_F(WriterTest, WriteSingleLidarScan) { + 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); + + std::string output_osf_filename = tmp_file("writer_simple.osf"); + + std::string sinfo_str = sensor::to_string(sinfo); + + // Writing LidarScan + Writer writer(output_osf_filename, "test_session"); + EXPECT_EQ(writer.chunks_layout(), ChunksLayout::LAYOUT_STREAMING); + + auto sensor_meta_id = writer.addMetadata(sinfo_str); + + EXPECT_THROW({ writer.addMetadata(sinfo); }, + std::invalid_argument); + + auto ls_stream = writer.createStream( + sensor_meta_id, get_field_types(sinfo)); + ls_stream.save(ts_t{123}, ls); + writer.close(); + + OsfFile osf_file(output_osf_filename); + EXPECT_TRUE(osf_file.good()); + + Reader reader(osf_file); + EXPECT_EQ(reader.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); + + EXPECT_EQ(++msg_it, reader.messages().end()); + + // Map of all MetadataEntries of type LidarSensor + auto sensors = reader.meta_store().find(); + EXPECT_EQ(sensors.size(), 1); + + // Use first sensor and get its sensor_info + auto sinfo_recovered = sensors.begin()->second->info(); + EXPECT_EQ(sensor::to_string(sinfo_recovered), sinfo_str); + + auto metadata_recovered = sensors.begin()->second->metadata(); + EXPECT_EQ(metadata_recovered, sinfo_str); +} + +TEST_F(WriterTest, WriteLidarSensorWithExtrinsics) { + sensor_info sinfo = sensor::metadata_from_json( + path_concat(test_data_dir(), "pcaps/OS-1-128_v2.3.0_1024x10.json")); + + std::string output_osf_filename = + tmp_file("writer_lidar_sensor_extrinsics.osf"); + + std::string sinfo_str = sensor::to_string(sinfo); + + sinfo.extrinsic(0, 3) = 10.0; + sinfo.extrinsic(0, 1) = 0.756; + sinfo.extrinsic(1, 0) = 0.756; + sinfo.extrinsic(0, 0) = 0.0; + + // Writing LidarSensor + Writer writer(output_osf_filename, "test_session"); + + auto sensor_meta_id = writer.addMetadata(sinfo_str); + EXPECT_TRUE(sensor_meta_id != 0); + + writer.addMetadata(sinfo.extrinsic, sensor_meta_id); + + writer.close(); + + OsfFile osf_file(output_osf_filename); + EXPECT_TRUE(osf_file.good()); + + Reader reader(osf_file); + + auto sensors = reader.meta_store().find(); + EXPECT_EQ(sensors.size(), 1); + + // Use first sensor and get its sensor_info + auto sinfo_recovered = sensors.begin()->second->info(); + + auto metadata_recovered = sensors.begin()->second->metadata(); + EXPECT_EQ(metadata_recovered, sinfo_str); + + auto extrinsics = reader.meta_store().find(); + EXPECT_EQ(extrinsics.size(), 1); + + auto ext_mat_recovered = extrinsics.begin()->second->extrinsics(); + EXPECT_EQ(sinfo.extrinsic, ext_mat_recovered); + EXPECT_EQ(sensor_meta_id, extrinsics.begin()->second->ref_meta_id()); +} + +TEST_F(WriterTest, WriteSingleLidarScanStreamingLayout) { + 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); + + std::string output_osf_filename = tmp_file("writer_simple_streaming.osf"); + + std::string sinfo_str = sensor::to_string(sinfo); + + // Writing LidarScan + Writer writer(output_osf_filename, "test_session"); + EXPECT_EQ(writer.chunks_layout(), ChunksLayout::LAYOUT_STREAMING); + + auto sensor_meta_id = writer.addMetadata(sinfo_str); + auto ls_stream = writer.createStream( + sensor_meta_id, get_field_types(sinfo)); + ls_stream.save(ts_t{123}, ls); + writer.close(); + + OsfFile osf_file(output_osf_filename); + EXPECT_TRUE(osf_file.good()); + + Reader reader(osf_file); + EXPECT_EQ(reader.id(), "test_session"); + + // TODO[pb]: Add reader validation CRC + + 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); + + EXPECT_EQ(++msg_it, reader.messages().end()); + + // Map of all MetadataEntries of type LidarSensor + auto sensors = reader.meta_store().find(); + EXPECT_EQ(sensors.size(), 1); + + // Check that it's an OSF with StreamingLayout + EXPECT_EQ(1, reader.meta_store().count()); + auto streaming_info = reader.meta_store().get(); + + EXPECT_TRUE(streaming_info != nullptr); + + // One stream LidarScanStream + EXPECT_EQ(1, streaming_info->stream_stats().size()); + + // Use first sensor and get its sensor_info + auto sinfo_recovered = sensors.begin()->second->info(); + EXPECT_EQ(sensor::to_string(sinfo_recovered), sinfo_str); + + auto metadata_recovered = sensors.begin()->second->metadata(); + EXPECT_EQ(metadata_recovered, sinfo_str); +} + +TEST_F(WriterTest, WriteSlicedLidarScan) { + 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); + + // 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)); + + // Make a reduced field LidarScan + ls = slice_with_cast(ls, field_types); + + EXPECT_EQ(field_types.size(), std::distance(ls.begin(), ls.end())); + + std::string output_osf_filename = tmp_file("writer_sliced.osf"); + + std::string sinfo_str = sensor::to_string(sinfo); + + // Writing LidarScan + Writer writer(output_osf_filename, "test_session"); + auto sensor_meta_id = writer.addMetadata(sinfo_str); + auto ls_stream = + writer.createStream(sensor_meta_id, field_types); + ls_stream.save(ts_t{123}, ls); + writer.close(); + + OsfFile osf_file(output_osf_filename); + EXPECT_TRUE(osf_file.good()); + + Reader reader(osf_file); + EXPECT_EQ(reader.id(), "test_session"); + + auto msg_it = reader.messages().begin(); + EXPECT_NE(msg_it, reader.messages().end()); + + 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(*ls_recovered, ls); + + EXPECT_EQ(++msg_it, reader.messages().end()); + + // Map of all MetadataEntries of type LidarSensor + auto sensors = reader.meta_store().find(); + EXPECT_EQ(sensors.size(), 1); + + // Use first sensor and get its sensor_info + auto sinfo_recovered = sensors.begin()->second->info(); + EXPECT_EQ(sensor::to_string(sinfo_recovered), sinfo_str); + + auto metadata_recovered = sensors.begin()->second->metadata(); + EXPECT_EQ(metadata_recovered, sinfo_str); +} + +TEST_F(WriterTest, WriteSlicedLegacyLidarScan) { + const sensor_info sinfo = sensor::metadata_from_json(path_concat( + test_data_dir(), "metadata/2_5_0_os-992146000760-128_legacy.json")); + LidarScan ls_orig = get_random_lidar_scan(sinfo); + + // Subset of fields to leave in LidarScan (or extend ... ) during writing + LidarScanFieldTypes field_types; + field_types.emplace_back(sensor::ChanField::RANGE, + sensor::ChanFieldType::UINT32); + field_types.emplace_back(sensor::ChanField::SIGNAL, + sensor::ChanFieldType::UINT16); + field_types.emplace_back(sensor::ChanField::REFLECTIVITY, + sensor::ChanFieldType::UINT8); + field_types.emplace_back(sensor::ChanField::REFLECTIVITY2, + sensor::ChanFieldType::UINT16); + + std::cout << "LidarScan field_types: " << ouster::to_string(field_types) + << std::endl; + + // Make a reduced/extended fields LidarScan + // that will be compared with a recovered LidarScan from OSF + auto ls_reference = slice_with_cast(ls_orig, field_types); + + // Check that we have non existent REFLECTIVITY2 set as Zero + img_t refl2{ls_reference.h, ls_reference.w}; + impl::visit_field(ls_reference, sensor::ChanField::REFLECTIVITY2, + ouster::impl::read_and_cast(), refl2); + EXPECT_TRUE((refl2 == 0).all()); + + EXPECT_EQ(field_types.size(), + std::distance(ls_reference.begin(), ls_reference.end())); + + std::string output_osf_filename = tmp_file("writer_sliced_legacy.osf"); + + std::string sinfo_str = sensor::to_string(sinfo); + + // Writing LidarScan + Writer writer(output_osf_filename, "test_session"); + auto sensor_meta_id = writer.addMetadata(sinfo_str); + + // Creating LidarScanStream with custom field_types, that will be used to + // transform LidarScan during save() + auto ls_stream = + writer.createStream(sensor_meta_id, field_types); + ls_stream.save(ts_t{123}, ls_orig); + writer.close(); + + OsfFile osf_file(output_osf_filename); + EXPECT_TRUE(osf_file.good()); + + Reader reader(osf_file); + EXPECT_EQ(reader.id(), "test_session"); + + auto msg_it = reader.messages().begin(); + EXPECT_NE(msg_it, reader.messages().end()); + + 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(*ls_recovered, ls_reference); + + EXPECT_EQ(++msg_it, reader.messages().end()); + + // Map of all MetadataEntries of type LidarSensor + auto sensors = reader.meta_store().find(); + EXPECT_EQ(sensors.size(), 1); + + // Use first sensor and get its sensor_info + auto sinfo_recovered = sensors.begin()->second->info(); + EXPECT_EQ(sensor::to_string(sinfo_recovered), sinfo_str); + + auto metadata_recovered = sensors.begin()->second->metadata(); + EXPECT_EQ(metadata_recovered, sinfo_str); +} + +TEST_F(WriterTest, WriteCustomLidarScanWithFlags) { + const sensor_info sinfo = sensor::metadata_from_json(path_concat( + test_data_dir(), "metadata/3_0_1_os-122246000293-128_legacy.json")); + + LidarScanFieldTypes field_types_with_flags; + field_types_with_flags.emplace_back(sensor::ChanField::RANGE, + sensor::ChanFieldType::UINT32); + field_types_with_flags.emplace_back(sensor::ChanField::SIGNAL, + sensor::ChanFieldType::UINT16); + field_types_with_flags.emplace_back(sensor::ChanField::RANGE2, + sensor::ChanFieldType::UINT32); + field_types_with_flags.emplace_back(sensor::ChanField::SIGNAL2, + sensor::ChanFieldType::UINT16); + field_types_with_flags.emplace_back(sensor::ChanField::REFLECTIVITY, + sensor::ChanFieldType::UINT8); + field_types_with_flags.emplace_back(sensor::ChanField::NEAR_IR, + sensor::ChanFieldType::UINT16); + field_types_with_flags.emplace_back(sensor::ChanField::FLAGS, + sensor::ChanFieldType::UINT8); + field_types_with_flags.emplace_back(sensor::ChanField::FLAGS2, + sensor::ChanFieldType::UINT8); + field_types_with_flags.emplace_back(sensor::ChanField::CUSTOM0, + sensor::ChanFieldType::UINT64); + field_types_with_flags.emplace_back(sensor::ChanField::CUSTOM7, + sensor::ChanFieldType::UINT16); + + LidarScan ls = get_random_lidar_scan(sinfo.format.columns_per_frame, + sinfo.format.pixels_per_column, + field_types_with_flags); + + std::cout << "LidarScan field_types_with_flags: " + << ouster::to_string(field_types_with_flags) << std::endl; + + // Check that we have non zero FLAGS + img_t flags{ls.h, ls.w}; + impl::visit_field(ls, sensor::ChanField::FLAGS, + ouster::impl::read_and_cast(), flags); + EXPECT_FALSE((flags == 0).all()); + // and non zero FLAGS2 + impl::visit_field(ls, sensor::ChanField::FLAGS2, + ouster::impl::read_and_cast(), flags); + EXPECT_FALSE((flags == 0).all()); + + // 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); + EXPECT_FALSE((custom == 0).all()); + + EXPECT_EQ(field_types_with_flags.size(), + std::distance(ls.begin(), ls.end())); + + std::string output_osf_filename = tmp_file("writer_with_flags.osf"); + + std::string sinfo_str = sensor::to_string(sinfo); + + // Writing LidarScan + Writer writer(output_osf_filename, "test_session"); + auto sensor_meta_id = writer.addMetadata(sinfo_str); + + auto ls_stream = writer.createStream(sensor_meta_id, + get_field_types(ls)); + ls_stream.save(ts_t{123}, ls); + writer.close(); + + OsfFile osf_file(output_osf_filename); + EXPECT_TRUE(osf_file.good()); + + Reader reader(osf_file); + EXPECT_EQ(reader.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(field_types_with_flags.size(), + std::distance(ls_recovered->begin(), ls_recovered->end())); + + EXPECT_EQ(*ls_recovered, ls); + + EXPECT_EQ(++msg_it, reader.messages().end()); +} + +// Used in WriteExample test below +void ReadExample(const std::string filename) { + // Open output: OSF v2 + OsfFile file(filename); + Reader reader(file); + + // Read all messages from OSF file + for (const auto m : reader.messages()) { + auto ts = m.ts(); // << message timestamp + auto stream_meta_id = m.id(); // << link to the stream meta + + EXPECT_GT(ts.count(), 0); + EXPECT_GT(stream_meta_id, uint32_t{0}); + + // Decoding messages + if (m.is()) { + auto ls = m.decode_msg(); + EXPECT_TRUE(ls != nullptr); + // std::cout << "ls = " << *ls << std::endl; + } + } + + // Get meta objects by type map of (meta_id, meta_ptr) + auto sensors = reader.meta_store().find(); + EXPECT_EQ(1, sensors.size()); + + // Get LidarSensor metadata + auto lidar_sensor = reader.meta_store().get(); + EXPECT_TRUE(lidar_sensor); +} + +TEST_F(WriterTest, WriteExample) { + // Get sensor_info + const sensor_info sinfo = sensor::metadata_from_json( + path_concat(test_data_dir(), "pcaps/OS-1-128_v2.3.0_1024x10.json")); + std::string sensor_metadata = sensor::to_string(sinfo); + + std::string output_osf_filename = tmp_file("write_example.osf"); + + // Create OSF v2 Writer + osf::Writer writer(output_osf_filename, "Example Session 1234"); + EXPECT_EQ(writer.chunks_layout(), ChunksLayout::LAYOUT_STREAMING); + + // Create LidarSensor record + auto sensor_meta_id = writer.addMetadata(sensor_metadata); + + // Create stream for LidarScan objects + auto ls_stream = writer.createStream( + sensor_meta_id, get_field_types(sinfo)); + + const int LOOP_CNT = 7; + + int timestamp = 0; + while (timestamp++ < LOOP_CNT) { + LidarScan ls = get_random_lidar_scan(sinfo); + + // Save LidarScan + ls_stream.save(ts_t{timestamp}, ls); + } + + writer.close(); + + // Quick test the we have result file + OsfFile result(output_osf_filename); + EXPECT_TRUE(result.good()); + + // Quick test that number of messages in result file is what we wrote + Reader reader(result); + EXPECT_EQ(LOOP_CNT, std::distance(reader.messages().begin(), + reader.messages().end())); + + // Check that it's an OSF with StreamingLayout + auto streaming_info_entry = reader.meta_store().find(); + EXPECT_EQ(1, streaming_info_entry.size()); + auto streaming_info = streaming_info_entry.begin()->second; + + // std::cout << "streaming_info = " << streaming_info->to_string() << std::endl; + + // One stream: LidarScanStream + EXPECT_EQ(1, streaming_info->stream_stats().size()); + + EXPECT_TRUE(reader.has_message_idx()); + + auto lsm = reader.meta_store().get(); + + auto stream_msg_count = + streaming_info->stream_stats()[lsm->id()].message_count; + EXPECT_EQ(stream_msg_count, LOOP_CNT); + + for (size_t msg_idx = 0; msg_idx < stream_msg_count; ++msg_idx) { + auto msg_ts = reader.ts_by_message_idx(lsm->id(), msg_idx); + EXPECT_TRUE(msg_ts); + + EXPECT_TRUE(msg_ts >= reader.start_ts()); + EXPECT_TRUE(msg_ts <= reader.end_ts()); + + // by construction of the test + EXPECT_EQ(msg_idx + 1, msg_ts->count()); + + // if we start reading from that msg_ts using stream_id, there is indeed + // the first message returned with this timestamp + auto first_msg = + reader.messages({lsm->id()}, *msg_ts, reader.end_ts()).begin(); + EXPECT_EQ(first_msg->ts(), msg_ts); + } + + auto msg_ts100 = reader.ts_by_message_idx(lsm->id(), 100); + EXPECT_FALSE(msg_ts100); + + auto msg_ts_count = reader.ts_by_message_idx(lsm->id(), stream_msg_count); + EXPECT_FALSE(msg_ts_count); + + auto msg_ts_no_stream = reader.ts_by_message_idx(0, 0); + EXPECT_FALSE(msg_ts_no_stream); + + auto msg_ts_no_stream2 = reader.ts_by_message_idx(100, 0); + EXPECT_FALSE(msg_ts_no_stream2); + + ReadExample(output_osf_filename); +} + +} // namespace +} // namespace osf +} // namespace ouster diff --git a/ouster_pcap/CMakeLists.txt b/ouster_pcap/CMakeLists.txt index adb39eef..a78e57fb 100644 --- a/ouster_pcap/CMakeLists.txt +++ b/ouster_pcap/CMakeLists.txt @@ -17,7 +17,7 @@ endif() target_link_libraries(ouster_pcap PUBLIC OusterSDK::ouster_client - PRIVATE ${PCAP_LIBRARY} libtins) + PRIVATE libpcap::libpcap libtins::libtins) add_library(OusterSDK::ouster_pcap ALIAS ouster_pcap) # ==== Install ==== diff --git a/ouster_pcap/include/ouster/indexed_pcap_reader.h b/ouster_pcap/include/ouster/indexed_pcap_reader.h index 5b6016da..afc5b26b 100644 --- a/ouster_pcap/include/ouster/indexed_pcap_reader.h +++ b/ouster_pcap/include/ouster/indexed_pcap_reader.h @@ -11,15 +11,35 @@ namespace ouster { namespace sensor_utils { + +struct PcapIndex { + using frame_index = std::vector; ///< Maps a frame number to a file offset + std::vector frame_indices_; ///< frame index for each sensor + + PcapIndex(size_t num_sensors) + : frame_indices_(num_sensors) {} + + /** + * Returns the number of frames in the frame index for the given sensor index. + * + * @param sensor_index[in] The position of the sensor for which to retrieve the desired frame count. + * @return The number of frames in the sensor's frame index. + */ + size_t frame_count(size_t sensor_index) const; + + /** + * Seeks the given reader to the given frame number for the given sensor index + */ + void seek_to_frame(PcapReader& reader, size_t sensor_index, unsigned int frame_number); +}; + /** * A PcapReader that allows seeking to the start of a lidar frame. - * To do this, the constructor calls `get_stream_info`, which in turn calls - * IndexedPcapReader::update_index_for_current_packet for each packet. - * This allows us to compute the index and obtain the stream_info while reading - * the PCAP file only once. + * + * The index must be computed by iterating through all packets and calling + * `update_index_for_current_packet()` for each one. */ struct IndexedPcapReader : public PcapReader { - using frame_index = std::vector; ///< Maps a frame number to a file offset /** * @param pcap_filename[in] A file path of the pcap to read @@ -27,10 +47,11 @@ struct IndexedPcapReader : public PcapReader { */ IndexedPcapReader( const std::string& pcap_filename, - const std::vector& metadata_filenames, - std::function progress_callback + const std::vector& metadata_filenames ); + const PcapIndex& get_index() const; + /** * Attempts to match the current packet to one of the sensor info objects * and returns the appropriate packet format if there is one @@ -47,28 +68,9 @@ struct IndexedPcapReader : public PcapReader { /** * Updates the frame index for the current packet - * - * Important: this method is only meant to be invoked from `get_stream_info`! - */ - void update_index_for_current_packet(); - - /** - * @return The stream_info associated with this PcapReader - */ - std::shared_ptr get_stream_info() const; // TODO move to parent class - - /** - * Seeks to the given frame number for the given sensor index - */ - void seek_to_frame(size_t sensor_index, unsigned int frame_number); - - /** - * Returns the number of frames in the frame index for the given sensor index. - * - * @param sensor_index[in] The position of the sensor for which to retrieve the desired frame count. - * @return The number of frames in the sensor's frame index. + * @return the progress of indexing as an int from [0, 100] */ - size_t frame_count(size_t sensor_index) const; + int update_index_for_current_packet(); /** * Return true if the frame_id from the packet stream has rolled over, @@ -78,9 +80,9 @@ struct IndexedPcapReader : public PcapReader { */ static bool frame_id_rolled_over(uint16_t previous, uint16_t current); + std::vector sensor_infos_; ///< A vector of sensor_info that correspond to the provided metadata files - std::shared_ptr stream_info_; ///< TODO: move to parent class - std::vector frame_indices_; ///< frame index for each sensor + PcapIndex index_; std::vector> previous_frame_ids_; ///< previous frame id for each sensor }; diff --git a/ouster_pcap/include/ouster/os_pcap.h b/ouster_pcap/include/ouster/os_pcap.h index 62edef51..9ef6e02e 100644 --- a/ouster_pcap/include/ouster/os_pcap.h +++ b/ouster_pcap/include/ouster/os_pcap.h @@ -13,11 +13,36 @@ #include #include #include +#include #include #include #include "ouster/types.h" #include "ouster/pcap.h" +namespace ouster { +namespace sensor_utils { +/** + * Structure representing a hash key/sorting key for a udp stream + */ +struct stream_key { + std::string dst_ip; ///< The destination IP + std::string src_ip; ///< The source IP + int src_port; ///< The src port + int dst_port; ///< The destination port + + bool operator==(const struct stream_key &other) const; +}; +}} + +template<> +struct std::hash { + std::size_t operator()(const ouster::sensor_utils::stream_key& key) const noexcept { + return std::hash{}(key.src_ip) ^ + (std::hash{}(key.src_ip) << 1) ^ + (std::hash{}(key.src_port << 2)) ^ + (std::hash{}(key.dst_port << 3)); + } +}; namespace ouster { namespace sensor_utils { @@ -34,22 +59,7 @@ using ts = std::chrono::microseconds; ///< Microsecond timestamp */ std::ostream& operator<<(std::ostream& stream_in, const packet_info& data); -/** - * Structure representing a hash key/sorting key for a udp stream - */ -struct stream_key { - std::string dst_ip; ///< The destination IP - std::string src_ip; ///< The source IP - int src_port; ///< The src port - int dst_port; ///< The destination port - bool operator==(const struct stream_key &other) const; - bool operator!=(const struct stream_key &other) const; - bool operator<(const struct stream_key &other) const; - bool operator>(const struct stream_key &other) const; - bool operator<=(const struct stream_key &other) const; - bool operator>=(const struct stream_key &other) const; -}; /** * Structure representing a hash key/sorting key for a udp stream @@ -102,7 +112,7 @@ struct stream_info { ts timestamp_max; ///< The latest timestamp detected ts timestamp_min; ///< The earliest timestamp detected - std::map udp_streams; ///< Datastructure containing info on all of the different streams + std::unordered_map udp_streams; ///< Datastructure containing info on all of the different streams }; /** diff --git a/ouster_pcap/include/ouster/pcap.h b/ouster_pcap/include/ouster/pcap.h index 85fd2348..a7c71208 100644 --- a/ouster_pcap/include/ouster/pcap.h +++ b/ouster_pcap/include/ouster/pcap.h @@ -92,7 +92,7 @@ class PcapReader { /** * @return The size of the PCAP file in bytes */ - uint64_t file_size() const; + int64_t file_size() const; /** * Return the read position to the start of the PCAP file @@ -113,9 +113,11 @@ class PcapReader { */ void seek(uint64_t offset); + int64_t current_offset() const; + private: - uint64_t file_size_{}; - uint64_t file_start_{}; + int64_t file_size_{}; + int64_t file_start_{}; }; /** diff --git a/ouster_pcap/src/indexed_pcap_reader.cpp b/ouster_pcap/src/indexed_pcap_reader.cpp index 1c1eba65..13029754 100644 --- a/ouster_pcap/src/indexed_pcap_reader.cpp +++ b/ouster_pcap/src/indexed_pcap_reader.cpp @@ -5,10 +5,9 @@ namespace sensor_utils { IndexedPcapReader::IndexedPcapReader( const std::string& pcap_filename, - const std::vector& metadata_filenames, - std::function progress_callback) + const std::vector& metadata_filenames) : PcapReader(pcap_filename) - , frame_indices_(metadata_filenames.size()) + , index_(metadata_filenames.size()) , previous_frame_ids_(metadata_filenames.size()) { for(const std::string& metadata_filename : metadata_filenames) { @@ -16,21 +15,6 @@ IndexedPcapReader::IndexedPcapReader( ouster::sensor::metadata_from_json(metadata_filename) ); } - stream_info_ = ouster::sensor_utils::get_stream_info(*this, progress_callback, 256, -1); - if(sensor_infos_.size() == 1 && sensor_infos_[0].udp_port_lidar == 0) { - // guess lidar port for a single sensor if it is unspecified in sensor info - const ouster::sensor::packet_format& pf = ouster::sensor::packet_format(sensor_infos_[0]); - std::vector ports = guess_ports( - *stream_info_, - pf.lidar_packet_size, pf.imu_packet_size, - sensor_infos_[0].udp_port_lidar, sensor_infos_[0].udp_port_imu - ); - if(ports.empty()) { - throw std::runtime_error("IndexedPcapReader: unable to determine lidar UDP port"); - } - sensor_infos_[0].udp_port_lidar = ports[0].lidar; - sensor_infos_[0].udp_port_imu = ports[0].imu; - } } nonstd::optional IndexedPcapReader::sensor_idx_for_current_packet() const { @@ -60,31 +44,32 @@ bool IndexedPcapReader::frame_id_rolled_over(uint16_t previous, uint16_t current return previous > 0xff00 && current < 0x00ff; } -void IndexedPcapReader::update_index_for_current_packet() { +int IndexedPcapReader::update_index_for_current_packet() { if(nonstd::optional sensor_info_idx = sensor_idx_for_current_packet()) { if(nonstd::optional frame_id = current_frame_id()) { if(!previous_frame_ids_[*sensor_info_idx] || *previous_frame_ids_[*sensor_info_idx] < *frame_id // frame_id is greater than previous || frame_id_rolled_over(*previous_frame_ids_[*sensor_info_idx], *frame_id) ) { - frame_indices_[*sensor_info_idx].push_back(current_info().file_offset); + index_.frame_indices_[*sensor_info_idx].push_back(current_info().file_offset); previous_frame_ids_[*sensor_info_idx] = *frame_id; } } } + return static_cast(100 * static_cast(current_offset())/file_size()); } -void IndexedPcapReader::seek_to_frame(size_t sensor_index, unsigned int frame_number) { - seek(frame_indices_.at(sensor_index).at(frame_number)); +const PcapIndex& IndexedPcapReader::get_index() const { + return index_; } - -size_t IndexedPcapReader::frame_count(size_t sensor_index) const { - return frame_indices_.at(sensor_index).size(); +void PcapIndex::seek_to_frame(PcapReader& reader, size_t sensor_index, unsigned int frame_number) { + reader.seek(frame_indices_.at(sensor_index).at(frame_number)); } -std::shared_ptr IndexedPcapReader::get_stream_info() const { - return stream_info_; + +size_t PcapIndex::frame_count(size_t sensor_index) const { + return frame_indices_.at(sensor_index).size(); } } // namespace sensor_utils diff --git a/ouster_pcap/src/os_pcap.cpp b/ouster_pcap/src/os_pcap.cpp index 602abf68..b998c7b4 100644 --- a/ouster_pcap/src/os_pcap.cpp +++ b/ouster_pcap/src/os_pcap.cpp @@ -69,32 +69,6 @@ bool stream_key::operator==(const struct stream_key &other) const { dst_port == other.dst_port; } -bool stream_key::operator!=(const struct stream_key &other) const { - return !(*this == other); -} - -bool stream_key::operator<=(const struct stream_key &other) const { - return dst_ip <= other.dst_ip && - src_ip <= other.src_ip && - dst_port <= other.dst_port && - src_port <= other.src_port; -} - -bool stream_key::operator>=(const struct stream_key &other) const { - return dst_ip >= other.dst_ip && - src_ip >= other.src_ip && - dst_port >= other.dst_port && - src_port >= other.src_port; -} - -bool stream_key::operator<(const struct stream_key &other) const { - return *this <= other && *this != other; -} - -bool stream_key::operator>(const struct stream_key &other) const { - return *this >= other && *this != other; -} - std::ostream& operator<<(std::ostream& stream_in, const stream_key& data) { stream_in << "Source IP: \"" << data.src_ip << "\" " << std::endl; stream_in << "Destination IP: \"" << data.dst_ip << "\" " << std::endl; diff --git a/ouster_pcap/src/pcap.cpp b/ouster_pcap/src/pcap.cpp index 8f5130dd..e252dd18 100644 --- a/ouster_pcap/src/pcap.cpp +++ b/ouster_pcap/src/pcap.cpp @@ -9,13 +9,18 @@ * @TODO improve error reporting */ +#define _FILE_OFFSET_BITS 64 #include "ouster/pcap.h" #if defined _WIN32 #include +#define FTELL ftell +#define FSEEK fseek #else #include // inet_ntop #include // timeval +#define FTELL ftello +#define FSEEK fseeko #endif #include @@ -73,7 +78,7 @@ PcapReader::PcapReader(const std::string& file) : impl(new pcap_impl) { impl->encap_proto = impl->pcap_reader->link_type(); impl->pcap_reader_internals = pcap_file(impl->pcap_reader->get_pcap_handle()); - file_start_ = ftell(impl->pcap_reader_internals); + file_start_ = FTELL(impl->pcap_reader_internals); } PcapReader::~PcapReader() {} @@ -88,15 +93,25 @@ void PcapReader::seek(uint64_t offset) { if(offset < sizeof(struct pcap_file_header)) { offset = sizeof(struct pcap_file_header); } - if(fseek(impl->pcap_reader_internals, offset, SEEK_SET)) { + if(FSEEK(impl->pcap_reader_internals, offset, SEEK_SET)) { throw std::runtime_error("pcap seek failed"); } } -uint64_t PcapReader::file_size() const { +int64_t PcapReader::file_size() const { return file_size_; } +int64_t PcapReader::current_offset() const { + int64_t ret = FTELL(impl->pcap_reader_internals); + + if(ret == -1L) { + fclose(impl->pcap_reader_internals); + throw std::runtime_error("ftell error: errno " + std::to_string(errno)); + } + return ret; +} + void PcapReader::reset() { seek(file_start_); } @@ -108,7 +123,7 @@ size_t PcapReader::next_packet() { int reassm_packets = 0; while (!reassm) { reassm_packets++; - info.file_offset = ftell(impl->pcap_reader_internals); + info.file_offset = current_offset(); impl->packet_cache = impl->pcap_reader->next_packet(); if (impl->packet_cache) { auto pdu = impl->packet_cache.pdu(); diff --git a/ouster_viz/include/ouster/point_viz.h b/ouster_viz/include/ouster/point_viz.h index c998b4b8..508b58d9 100644 --- a/ouster_viz/include/ouster/point_viz.h +++ b/ouster_viz/include/ouster/point_viz.h @@ -751,6 +751,16 @@ class Cloud { */ void set_column_poses(const float* rotation, const float* translation); + /** + * Set the per-column poses, so that the point corresponding to the + * pixel at row u, column v in the staggered lidar scan is transformed + * by the vth pose, given as a homogeneous transformation matrix. + * + * @param[in] column_poses array of 4x4 pose elements and length w + * (i.e. [wx4x4]) column-storage + */ + void set_column_poses(const float* column_poses); + /** * Set the point cloud color palette. * @@ -968,6 +978,13 @@ class Label { */ void clear(); + /** + * Set all dirty flags. + * + * Re-sets everything so the object is always redrawn. + */ + void dirty(); + /** * Update label text. * diff --git a/ouster_viz/src/point_viz.cpp b/ouster_viz/src/point_viz.cpp index 7487a96e..c4744b5d 100644 --- a/ouster_viz/src/point_viz.cpp +++ b/ouster_viz/src/point_viz.cpp @@ -383,6 +383,7 @@ void PointViz::add(const std::shared_ptr& cuboid) { } void PointViz::add(const std::shared_ptr