diff --git a/.github/workflows/build_test.yaml b/.github/workflows/build_test.yaml index c11c7451..2f0fd06b 100644 --- a/.github/workflows/build_test.yaml +++ b/.github/workflows/build_test.yaml @@ -18,7 +18,7 @@ jobs: - name: install python-deps run: pip3 install mypy flake8 - name: python-lint - run: cd python && mypy ./src ./tests && flake8 + run: cd python && mypy ./src ./tests ../tests/hil ../tests/integration && flake8 linux-build: runs-on: ubuntu-latest steps: diff --git a/CHANGELOG.rst b/CHANGELOG.rst index b4007adf..d690be75 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,6 +2,100 @@ Changelog ========= +**Important: as of 0.13.0, the SDK is no longer compatible with firmware versions older than 2.2.0.** + +[20240702] [0.13.0] +====================== + +ouster_osf +------------------------ +* Add full index of both receive and sensor timestamps to metadata +* Speed up opening of OSF files with index + +* OSF now saves alert flags, thermal countdown and status, shot limiting countdown and status from ``LidarScan``. +* [BUGFIX] Fix OSF being unable to load LidarScans containing only custom fields +* [BUGFIX] Fix OSF not flushed when the user pressed CTRL-C more than once +* [BUGFIX] Fix improper timestamps when saving OSF on MacOS(m-series) and Windows +* [BUGFIX] Fix an issue with destaggering images after modifying ``SensorInfo`` in an ``OsfScanSource``. +* [BUGFIX] Fix an issue loading extrinsics from OSF metadata into a ``SensorInfo`` in ``OsfScanSource``. +* [BREAKING] Remove ``ChunksLayout`` and ``ChunkRef`` from Python API. + +ouster_client/Python SDK +------------------------ + +* Add support for reading and writing ROS1 and ROS2 bag files. +* Add new sensor client interface ``ouster::sensor::SensorClient`` which supports multiple sensors as well as multiple sensors and IMU data on the same port +* Add higher level sensor client interface ```ouster::sensor::SensorScanSource`` which produces ``LidarScan`` s from multiple sensors +* Add ``ouster.sdk.client.SensorPacketSource`` which receives packets from multiple sensors +* Add support for multiple sensors to ``ouster.sdk.sensor.SensorScanSource`` +* Greatly reduced redundant HTTP API calls to the sensor during initialization +* Deserialize FLAGS fields in each profile by default +* Add support for IPv6 multicast +* Add ``field_names`` argument to each scan source and to ``open_source`` to specify which fields to decode +* Add metadata validation functionality +* Add vendored json library +* Improved multi sensor pcap reading +* Improve ``ScanBatcher`` to release ``LidarScan`` as soon as they are completed +* ``ScanBatcher`` now adds alert flags, thermal countdown, and shot limiting countdown to ``LidarScan``. +* Use index to speed up ``ouster-cli source .osf info`` +* Use index to speed up slicing of indexed OSF sources when sliced immediately after the ``source`` command +* Add ``LidarScan.get_first_valid_column_timestamp()`` +* Add ``crc`` and ``calculate_crc`` methods to ``ouster::sensor::packet_format`` for obtaining or calculating (respectively) the CRC64 of a packet. +* ``scan_to_packets`` now creates packets with alert flags, thermal countdown and status, shot limiting countdown and status, and CRC64. +* Add ``ouster::pose_util::dewarp`` C++ function to de-warp a ``LidarScan`` (similar to ``ouster.sdk.pose_util`` in the Python API.) +* Add a constructor ``LidarScan(const ouster::sensor::sensor_info&)``. +* Always use ``nonstd::optional`` instead of drop-in ``std::optional`` from https://github.com/martinmoene/optional-lite.git to reduce issues associated with mixing C++14 and C++17. +* Add ``w()`` and ``h()`` methods to ``sensor_info`` in C++ and ``w`` and ``h`` properties to ``SensorInfo`` in Python. +* [BUGFIX] fix automatic UDP dest for FW 2.3 sensors. + + +* [BREAKING] Remove ``ouster::make_xyz_lut(const ouster::sensor::sensor_info&)``. (Use ``make_xyz_lut(const sensor::sensor_info& sensor, bool use_extrinsics)`` instead.) +* [BREAKING] changed REFLECTIVITY channel field size to 8 bits. (Important - this makes the SDK incompatible with FW 2.0 and 2.1.) +* [BREAKING] Removed ``UDPPacketSource`` and ``BufferedUDPSource``. +* [BREAKING] Removed ``ouster.sdk.util.firmware_version(hostname)`` please use ``ouster.sdk.client.SensorHttp.create(hostname).firmware_version()`` instead +* [BREAKING] ``open_source`` no longer automatically finds and applies extrinsics from ``sensor_extrinsics.json`` files. Use the ``extrinsics`` argument instead to specify the path to the relevant extrinsics file instead. +* [BREAKING] Deprecated ``osf.Scans(...)`` for ``osf.OsfScanSource(...).single_source(0)```. +* [BREAKING] Deprecated ``client.Sensor(...)`` for ``client.SensorPacketSource(...).single_source(0)```. +* [BREAKING] Deprecated ``pcap.Pcap(...)`` for ``pcap.PcapMultiPacketReader(...).single_source(0)```. +* [BREAKING] Deprecated ``ScanBatcher::ScanBatcher(size_t, const packet_format&)`` for ``ScanBatcher::ScanBatcher(const sensor_info&)``. +* [FUTURE BREAKING] Removing all instances of jsoncpp's ``Json::Value`` from the public C++ API methods in favor of ``std::string``. + +ouster_viz +---------- + +* ``LidarScanViz`` now supports multi-sensor datasets. +* Add Python callback registration methods for mouse button and scroll events from ``PointViz``. +* Add Python and C++ callback registration methods for frame buffer resize events. +* Add ``MouseButton``, ``MouseButtonEvent``, and ``EventModifierKeys`` enums. +* Add methods ``aspect_ratio``, ``normalized_coordinates``, and ``window_coordinates`` to ``viz::WindowCtx``. +* Add method ``window_coordinates_to_image_pixel`` to ``viz::Image``. (See ``viz_events_example.cpp`` for an example.) +* Add ``current_camera()`` method to ``PointViz``. +* [BREAKING] ``SimpleViz`` no longer accepts a ``ScansAccumulator`` instance and now accepts scan/map accumulation parameters as keyword args in its constructor. +* [BREAKING] ``ScansAccumulator`` is split into several different classes: ``ScansAccumulator``, ``MapAccumulator``, ``TracksAccumulator``, and ``LidarScanVizAccumulators``. +* [BREAKING] changed ``PointViz`` mouse button callback to fire for both mouse button press and release events. +* [BREAKING] changed ``PointViz`` mouse button callback signature to use the new enums. +* [BREAKING] removed ``bool update_on_input()`` and ``update_on_input(bool)`` methods from ``PointViz``. +* [BUGFIX] SimpleViz throws a 'generator already executing' exception. + +ouster_cli +---------- + +* Add support for reading and writing ROS1 and ROS2 bag files. +* Add support for working with multi scan sources. +* Add ``--fields`` argument to ``ouster-cli source`` to specify which fields to decode. +* Add metadata validation utility. +* [BUGFIX] Program doesn't terminate immediately when pressing CTRL-C the first time when streaming from a live sensor. +* [BUGFIX] Fix some errors that appeared when running ``ouster-cli util benchmark`` +* [BREAKING] ``source`` no longer automatically finds and applies extrinsics from ``sensor_extrinsics.json`` files. Use the ``-E`` argument instead to specify the path to the relevant extrinsics file instead. +* [BREAKING] Moved raw recording functionality for BAG and PCAP to ``ouster-cli source ... record_raw`` command. +* [BREAKING] CLI plugins now need to handle a list of Optional[LidarScan] instead of a single LidarScan to support multi sources. + +mapping +------- + +* Update KissICP version from 0.4.0 to 1.0.0. +* Add multi-sensor support. + [20240702] [0.12.0] =================== @@ -64,7 +158,9 @@ ouster-cli * Add chainable ``ouster-cli source ... stats`` command * Add chainable ``ouster-cli source ... clip`` command to discard points outside a provided range * Add ``--rate max`` option to ``ouster-cli source ... viz``` -* Improve argument naming and descriptions for ``ouster-cli source ... viz`` map and accum options +* Improve argument naming and descriptions for ``ouster-cli source ... viz`` map and accum options: + ``--accum-map`` is now called ``--map`` and ``--accum-map-ratio`` is now called ``--map-ratio``. +* New ``--map-size`` argument to set the maximum number of points used when ``--map`` is specified. * [BUGFIX] Prevent dropped frames from live sensors by consuming scans as fast as they come in rather than sleeping @@ -137,20 +233,18 @@ Python SDK * Add support for python 3.12, including wheels on pypi * Updated VCPKG libraries to 2023.10.19 * New ``ScanSource`` API: - * Added new ``MultiScanSource`` that supports streaming and manipulating LidarScan frames - from multiple concurrent LidarScan sources - * For non-live sources the ``MultiScanSource`` have the option to choose LidarScan(s) by index - or choose a subset of scans using slicing operation - * The ``MultiScanSource`` interface has the ability to fallback to ``ScanSource`` using the - ``single_source(sensor_idx)``, ``ScanSource`` interface yield a single LidarScan on iteration - rather than a List - * The ``ScanSource`` interface obtained via ``single_source`` method supports same indexing and - and slicing operations as the ``MultiScanSource`` - * Added a generic ``open_source`` that accepts sensor urls, or a path to a pcap recording - or an osf file + + * Added new ``MultiScanSource`` that supports streaming and manipulating LidarScan frames from multiple concurrent LidarScan sources + + * For non-live sources the ``MultiScanSource`` has the option to choose LidarScan(s) by index or choose a subset of scans using slicing operation + * The ``MultiScanSource`` interface has the ability to fallback to ``ScanSource`` using the ``single_source(sensor_idx)``, ``ScanSource`` interface yield a single LidarScan on iteration rather than a List + * The ``ScanSource`` interface obtained via ``single_source`` method supports same indexing and and slicing operations as the ``MultiScanSource`` + + * Added a generic ``open_source`` that accepts sensor urls, or a path to a pcap recording or an osf file * Add explicit flag ``index`` to index unindexed osf files, if flag is set to ``True`` the osf file will be indexed and the index will be saved to the file on first attempt * Display a progress bar during index of pcap file or osf (if unindexed) + * Improved the robustness of the ``resolve_metadata`` method used to automatically identify the sensor metadata associated with a PCAP source. * [bugfix] SimpleViz complains about missing fields @@ -240,6 +334,7 @@ Known issues * ouster-cli when combining ``slice`` command with ``viz`` the program will exit once iterate over the selected range of scans even when the ``--on-eof`` option is set to ``loop``. + - workaround: to have ``viz`` loop over the selected range, first perform a ``slice`` with ``save``, then playback the generated file. diff --git a/CMakeLists.txt b/CMakeLists.txt index 99945315..05591159 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -13,10 +13,10 @@ include(DefaultBuildType) include(VcpkgEnv) # ==== Project Name ==== -project(ouster_example VERSION 20231031) +project(ouster_sdk VERSION 0.13.0) # generate version header -set(OusterSDK_VERSION_STRING 0.12.0) +set(OusterSDK_VERSION_STRING 0.13.0) include(VersionGen) # ==== Options ==== diff --git a/LICENSE b/LICENSE index 2d09cf12..d154c0f5 100644 --- a/LICENSE +++ b/LICENSE @@ -1,7 +1,7 @@ Format: https://www.debian.org/doc/packaging-manuals/copyright-format/1.0/ Upstream-Name: OusterSDK Upstream-Contact: Ouster Sensor SDK Developers -Source: https://github.com/ouster-lidar/ouster_example +Source: https://github.com/ouster-lidar/ouster_sdk Files: * Copyright: 2018-2022 Ouster, Inc @@ -11,6 +11,10 @@ Files: include/optional-lite/* Copyright: 2014-2021 Martin Moene License: BSL-1.0 +Files: include/jsoncons* +Copyright Daniel Parker 2013 - 2020 +License: BSL-1.0 + License: BSD-3-Clause Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: diff --git a/LICENSE-bin b/LICENSE-bin index a64b4b6f..6a0d9f8b 100644 --- a/LICENSE-bin +++ b/LICENSE-bin @@ -71,6 +71,35 @@ License: MIT CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. +Name: jsoncons +Description: statically linked vendored header only library +Availability: https://github.com/danielaparker/jsoncons/ +Copyright Daniel Parker 2013 - 2020 +License: BSL-1.0 +Boost Software License - Version 1.0 - August 17th, 2003 + +Permission is hereby granted, free of charge, to any person or organization +obtaining a copy of the software and accompanying documentation covered by +this license (the "Software") to use, reproduce, display, distribute, +execute, and transmit the Software, and to prepare derivative works of the +Software, and to permit third-parties to whom the Software is furnished to +do so, all subject to the following: + +The copyright notices in the Software and this entire statement, including +the above license grant, this restriction and the following disclaimer, +must be included in all copies of the Software, in whole or in part, and +all derivative works of the Software, unless such copies or derivative +works are solely in the form of machine-executable object code generated by +a source language processor. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT +SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE +FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE, +ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER +DEALINGS IN THE SOFTWARE. + Name: libpcap Description: statically linked Availability: https://github.com/the-tcpdump-group/libpcap diff --git a/README.rst b/README.rst index b8c30032..53f16eea 100644 --- a/README.rst +++ b/README.rst @@ -1,4 +1,4 @@ -.. figure:: https://github.com/ouster-lidar/ouster_example/raw/master/docs/images/Ouster_Logo_TM_Horiz_Black_RGB_600px.png +.. figure:: https://github.com/ouster-lidar/ouster_sdk/raw/master/docs/images/Ouster_Logo_TM_Horiz_Black_RGB_600px.png ------------------------------------------------------ @@ -22,6 +22,8 @@ reading and visualizing data. * `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) +* `sdk-extenstions` is a submodule of the ouster-sdk repository which is currently for internal use only. +The submodule cannot be cloned or updated. .. note:: Ouster ROS driver code has been moved out to a separate GitHub repository. To get started using the @@ -31,7 +33,7 @@ reading and visualizing data. Contact ======= -For support of the Ouster SDK, please use `Github Issues `_ in this repo. +For support of the Ouster SDK, please use `Github Issues `_ in this repo. For support of Ouster products outside of the SDK, please use `Ouster customer support `_. diff --git a/clang-linting.sh b/clang-linting.sh index 1aed0e97..349c2126 100755 --- a/clang-linting.sh +++ b/clang-linting.sh @@ -1,5 +1,7 @@ #!/bin/bash +set -euo pipefail + help='false' apply='false' check='true' @@ -28,15 +30,23 @@ if [[ $apply == 'true' ]]; then SUBCOMMAND='' fi - # Ignore the ./ouster_client/include/optional-lite/nonstd/optional.hpp file while formatting files -CMD="find . -regex '.*\.\(cpp\|hpp\|cu\|c\|h\)' -not -path ./ouster_client/include/optional-lite/nonstd/optional.hpp \ --exec clang-format ${SUBCOMMAND} -style=file -i {} \;" -OUTPUT=$(eval "$CMD" 2>&1 | grep error) +run_command() { + find . -regex '.*\.\(cpp\|hpp\|cu\|c\|h\)' -not -path './ouster_client/include/optional-lite/nonstd/optional.hpp' \ + -and -not -path './thirdparty/*' \ + -exec clang-format ${SUBCOMMAND} -style=file -i {} \; + echo "Exit code: $?" +} +# grep returns an exit code of 1 if it finds no matches. +set +e +OUTPUT=$(run_command 2>&1 | grep error) +set -e +echo "OUTPUT $OUTPUT" + if [ -z "$OUTPUT" ]; then echo "clang-format check passed! No errors" exit 0; else - eval "$CMD" + run_command exit 1; -fi +fi \ No newline at end of file diff --git a/cmake/FindOusterSDK.cmake b/cmake/FindOusterSDK.cmake index 10257caa..3bcd2b1f 100644 --- a/cmake/FindOusterSDK.cmake +++ b/cmake/FindOusterSDK.cmake @@ -1,5 +1,5 @@ # Allow downstream code to depend on source transparently if(NOT TARGET EXAMPLE_INCLUDED) add_custom_target(EXAMPLE_INCLUDED) - add_subdirectory(${CMAKE_CURRENT_LIST_DIR}/.. ouster_example EXCLUDE_FROM_ALL) + add_subdirectory(${CMAKE_CURRENT_LIST_DIR}/.. ouster_sdk EXCLUDE_FROM_ALL) endif() diff --git a/conanfile.py b/conanfile.py index c2e53082..e521b07b 100644 --- a/conanfile.py +++ b/conanfile.py @@ -11,7 +11,7 @@ class ousterSdkRecipe(ConanFile): package_type = "library" license = "BSD 3-Clause License" author = "Ouster, Inc." - url = "https://github.com/ouster-lidar/ouster_example" + url = "https://github.com/ouster-lidar/ouster_sdk" description = "Ouster SDK - tools for working with Ouster Lidars" topics = ("lidar", "driver", "hardware", "point cloud", "3d", "robotics", "automotive") settings = "os", "compiler", "build_type", "arch" @@ -43,6 +43,7 @@ class ousterSdkRecipe(ConanFile): "ouster_osf/*", "ouster_viz/*", "tests/*", + "thirdparty/*", "CMakeLists.txt", "CMakeSettings.json", "LICENSE", diff --git a/docs/Doxyfile b/docs/Doxyfile index beebfba9..6f301465 100644 --- a/docs/Doxyfile +++ b/docs/Doxyfile @@ -481,7 +481,7 @@ EXTRACT_PACKAGE = NO # included in the documentation. # The default value is: NO. -EXTRACT_STATIC = NO +EXTRACT_STATIC = YES # If the EXTRACT_LOCAL_CLASSES tag is set to YES, classes (and structs) defined # locally in source files will be included in the documentation. If set to NO, @@ -489,7 +489,7 @@ EXTRACT_STATIC = NO # for Java sources. # The default value is: YES. -EXTRACT_LOCAL_CLASSES = YES +EXTRACT_LOCAL_CLASSES = NO # This flag is only useful for Objective-C code. If set to YES, local methods, # which are defined in the implementation section but not in the interface are @@ -804,7 +804,7 @@ WARN_IF_DOC_ERROR = YES # parameters have no documentation without warning. # The default value is: YES. -#WARN_IF_INCOMPLETE_DOC = YES +WARN_IF_INCOMPLETE_DOC = "$enhanced_warnings" # This WARN_NO_PARAMDOC option can be enabled to get warnings for functions that # are documented, but have no documentation for their parameters or return @@ -814,7 +814,7 @@ WARN_IF_DOC_ERROR = YES # WARN_IF_INCOMPLETE_DOC # The default value is: NO. -WARN_NO_PARAMDOC = NO +WARN_NO_PARAMDOC = "$enhanced_warnings" # If the WARN_AS_ERROR tag is set to YES then doxygen will immediately stop when # a warning is encountered. If the WARN_AS_ERROR tag is set to FAIL_ON_WARNINGS @@ -952,7 +952,11 @@ EXCLUDE_SYMLINKS = NO # Note that the wildcards are matched against the file with absolute path, so to # exclude all test directories for example use the pattern */test/* -EXCLUDE_PATTERNS = +EXCLUDE_PATTERNS = */impl/* \ + *.cpp \ + */src/* \ + */test/* \ + */tests/* # The EXCLUDE_SYMBOLS tag can be used to specify one or more symbol names # (namespaces, classes, functions, etc.) that should be excluded from the @@ -1159,7 +1163,7 @@ IGNORE_PREFIX = # If the GENERATE_HTML tag is set to YES, doxygen will generate HTML output # The default value is: YES. -GENERATE_HTML = NO +GENERATE_HTML = YES # The HTML_OUTPUT tag is used to specify where the HTML docs will be put. If a # relative path is entered the value of OUTPUT_DIRECTORY will be put in front of diff --git a/docs/cli/clip-sessions.rst b/docs/cli/clip-sessions.rst index 7a4f4b31..43346054 100644 --- a/docs/cli/clip-sessions.rst +++ b/docs/cli/clip-sessions.rst @@ -1,4 +1,4 @@ -Clip PointCloud in the Ouster-CLI +Clip Point clouds in the Ouster-CLI ================================= .. _ouster-cli-clip: @@ -54,7 +54,7 @@ in the slam command. However, you can explicitly pass in the range settings to t apply different ranges to the clip operation. Note that the range settings in the ``slam`` command only affect the point cloud within the SLAM algorithm. -The slam range settings will not modify the lidarscan and will not affect the other following commands. +The slam range settings will not modify the lidar scan and will not affect the other following commands. Example Usage diff --git a/docs/cli/common-use-cases.rst b/docs/cli/common-use-cases.rst index b35f61d1..e7ae5a73 100644 --- a/docs/cli/common-use-cases.rst +++ b/docs/cli/common-use-cases.rst @@ -45,7 +45,7 @@ using: .. code:: bash - $ ouster-cli source info > .json + $ ouster-cli source metadata > .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 @@ -101,7 +101,7 @@ the metadata to a json file with the same name, simply use: .. code:: bash - $ ouster-cli source record + $ ouster-cli source save_raw .pcap 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 diff --git a/docs/cli/getting-started.rst b/docs/cli/getting-started.rst index 8a2bc49d..02fddb61 100644 --- a/docs/cli/getting-started.rst +++ b/docs/cli/getting-started.rst @@ -77,7 +77,7 @@ auto-configuration, respectively. - 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 + - CLI command ``ouster-cli source {info,config}`` are working properly - Viz ``ouster-cli source viz`` from the ``tcpdump`` recorded pcap can be played and visualized @@ -106,23 +106,31 @@ commands also have subcommands that further extend or specify what * ``discover`` - uses mDNS to locate Ouster sensors on you local networks. * ``source`` - Read lidar data from a sensor or file and use it as input to one or more commands. Most subcommands can be "chained", meaning the output of a subcommand will become the input of the next subcommand. - * Sensors and files: + + * Sensors and files + * ``viz`` - visualizes data in a 3D point cloud viewer. * ``slam`` - computes trajectories by determining the change in pose between lidar frames. - * ``slice`` - reads a subset of lidar frames from the source using counts or time durations. + * ``slice`` - reads a subset of lidar frames from the source using counts or time duration. * ``clip`` - restrict the minimum or maximum range of lidar measurements in the source data. * ``stats`` - calculates statistics from the source data. * ``metadata`` - displays the metadata (e.g. sensor information) associated with the source data. * ``save`` - saves the source data, optionally converting to a new format. + * Pcap and OSF files only + * ``info`` - prints information about a pcap or OSF file. * Sensors only + * ``config`` - configures a sensor. * ``userdata`` - displays the userdata from a sensor. * OSF files only + * ``dump`` - prints metadata from an OSF file. * ``parse`` - prints message types from an OSF file. + * ``util`` - Miscellaneous utilities. + * ``benchmark`` - runs a performance benchmark for ouster-sdk. * ``benchmark-sensor`` - runs a performance benchmark for ouster-sdk using a sensor. * ``system-info`` - generates system diagnostic information as a JSON string, useful to Ouster support staff when providing customer support. diff --git a/docs/cli/mapping-sessions.rst b/docs/cli/mapping-sessions.rst index fc772949..ec156971 100644 --- a/docs/cli/mapping-sessions.rst +++ b/docs/cli/mapping-sessions.rst @@ -82,7 +82,7 @@ Then execute the following command: Save Command ------------ -The ``save`` command stores the lidarscan and the lidar movement trajectory into a OSF file by +The ``save`` command stores the lidar data and the lidar movement trajectory into a OSF file by specifying a filename with a .osf extension. This OSF file will be used for accumulated point cloud generation and the other post-process tools we offer in the future. @@ -109,7 +109,7 @@ SLAM performance degradation. ouster-cli source slam save output.ply -The accumulated point cloud data is automatically split and downsampleed into multiple files to +The accumulated point cloud data is automatically split and downsampled into multiple files to prevent exporting a huge size file. The terminal will display details, and you will see the following printout for each output file: @@ -126,8 +126,8 @@ following printout for each output file: Use the ``--help`` flag for more information such as selecting different fields as values, and changing the point cloud downsampling scale etc. -To filter out the point cloud, you can using the ``clip`` command. Coverting the SLAM output OSF -file to a PLY file and keep only the point within 20 to 80 meteter range you can run: +To filter out the point cloud, you can using the ``clip`` command. Converting the SLAM output OSF +file to a PLY file and keep only the point within 20 to 80 meters range you can run: .. code:: bash @@ -143,7 +143,7 @@ Viz Command ----------- The ``viz`` command enables visualizing the accumulated point cloud generation during the -SLAM process. By default, the viz operates in looping mode, meaning the visualiation will +SLAM process. By default, the viz operates in looping mode, meaning the visualization will continuously replay the source file. .. code:: bash @@ -161,26 +161,27 @@ complete iteration. ouster-cli source / slam viz -e exit save sample.osf -**Scans Accumulation**: The viz command allows the user to customize ... +**Accumulation**: The viz command supports several options for creating visually-pleasing maps by accumulating data from +lidar scans that contain pose information. The following sections describe the options and provide usage examples. Available view modes ~~~~~~~~~~~~~~~~~~~~~ -There are three view modes of **ScansAccumulator**, that may be enabled/disabled depending on -it's params and the data that is passed throught it: +There are three view modes of accumulation implemented in the default +visualizer that may be enabled/disabled depending on its parameters and the data +that is passed through it: - * **poses** (or **TRACK**), key ``8`` - all scan poses in a trajectory/path view (available only - if poses data is present in scans) - * **scan map** (or **MAP**), key ``7`` - overall map view with select ratio of random points - from every scan (available for scans with/without poses) - * **scan accum** (or **ACCUM**), key ``6`` - accumulated *N* scans (key frames) that is picked - according to params (available for scans with/without poses) + * **poses** mode, key ``8`` - all scan poses in a trajectory/path view (if poses data is present in scans.) + * **map accumulation** mode, key ``7`` - overall map view with select ratio of random points + from every scan (available for scans with or without poses.) + * **scan accumulation** mode, key ``6`` - accumulated *N* scans (key frames) that is picked + according to parameters (available for scans with or without poses.) Key bindings ~~~~~~~~~~~~~ -Keyboard controls available with **ScansAccumulator**: +The following key shortcuts apply to accumulation options while running Ouster CLI's ``viz`` command. ============== ============================================================= Key What it does @@ -193,22 +194,36 @@ Keyboard controls available with **ScansAccumulator**: ``j / J`` Increase/decrease point size of accumulated clouds or map ============== ============================================================= -Ouster CLI **ScansAccumulator** options: +Ouster CLI ``viz`` accumulation options: + + * **scan accumulation options** + * ``--accum-num INTEGER`` Accumulate up to this number of past scans for + visualization. Use <= 0 for unlimited. Defaults to 100 if ``--accum-every`` or + ``--accum-every-m`` is set. + * ``--accum-every INTEGER`` Add a new scan to the accumulator every this + number of scans. + * ``--accum-every-m FLOAT`` Add a new scan to the accumulator after this many + meters of travel. + * **map accumulation options** + * ``--map`` If set, add random points from every scan into an overall map for + visualization. Enabled if either ``--map-ratio`` or ``--map-size`` are set. + * ``--map-ratio FLOAT`` Fraction of random points in every scan to add to + overall map (0, 1]. [default: 0.01] + * ``--map-size INTEGER`` Maximum number of points in overall map before + discarding. [default: 1500000] - * ``--accum-num N`` - accumulate *N* scans (default: ``0``) - * ``--accum-every K`` - accumulate every *Kth* scan (default: ``1``) - * ``--accum-every-m M`` - accumulate a scan every *Mth* meters traveled (default: ``None``) - * ``--accum-map`` - enable the overall map accumulation, select some percentage of points from - every scan (default: disabled) - * ``--accum-map-ratio R`` - set *R* as a ratio of points to randomly select from every scan - (default: ``0.001`` (*0.1%*)) Dense accumulated clouds view (with every point of a scan) ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ -To obtain the densest view use the ``--accum-num N --accum-every 1`` params where ``N`` is the +To obtain the densest view use the ``--accum-num N --accum-every 1`` parameters where ``N`` is the number of clouds to accumulate (``N`` up to 100 is generally small enough to avoid slowing down -the viz interface):: +the viz interface.) + +The following example computes poses for each scan using the ``slam`` command and creates a dense +map using the ``viz --accum-num 20`` to accumulate the points from 20 scans. Finally, the ``save`` command writes the +scans with their computed trajectories to an OSF file. (Note - accumulation is a visualization feature only. The +accumulated data is not saved to the file.):: ouster-cli source / slam viz --accum-num 20 save sample.osf @@ -218,7 +233,6 @@ and the dense accumulated clouds result: Dense view of 20 accumulated scans during the ``slam viz`` run - Overall map view (with poses) ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ @@ -229,7 +243,7 @@ viz with scan accumulator feature without appending the ``slam`` option. :: ouster-cli source sample.osf viz --accum-num 20 \ - --accum-every 0 --accum-every-m 10.5 --accum-map -r 3 -e stop + --accum-every-m 10.5 --map -r 3 -e stop Here is a preview example of the overall map generated from the accumulated scan results. @@ -238,7 +252,7 @@ displaying the preview of the lidar trajectory: .. figure:: /images/scans_accum_map_all_scan.png - Data fully replayed with map and accum enabled (last current scan is displayed here in grey + Data fully replayed with map and accum enabled (last current scan is displayed here in gray palette) diff --git a/docs/cli/sample-sessions.rst b/docs/cli/sample-sessions.rst index 4409c771..71e63969 100644 --- a/docs/cli/sample-sessions.rst +++ b/docs/cli/sample-sessions.rst @@ -65,7 +65,7 @@ That should produce screen output that looks something like: Go ahead and look in the current directory for the named pcap file and associated metadata file. -The ``slice`` command also allows recording for fixed time durations. For example, the following will record 30 seconds of pcap data. +The ``slice`` command also allows recording for fixed time duration. For example, the following will record 30 seconds of pcap data. .. code:: bash @@ -114,3 +114,21 @@ To visualize the pcap at 2x speed while looping back: 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 + + +Working with OSF files +---------------------- + +Most of the Ouster CLI commands mentioned above also apply to OSF files. Here are a few examples. + +To save 100 frames of lidar data from a sensor to an OSF file, run + +.. code:: bash + + $ ouster-cli source slice 0:100 save .osf + +To visualize the OSF at 2x speed while looping back: + +.. code:: bash + + $ ouster-cli source viz -r 2.0 -e loop diff --git a/docs/conf.py b/docs/conf.py index 652165be..e8b884e5 100644 --- a/docs/conf.py +++ b/docs/conf.py @@ -1,5 +1,4 @@ import sphinx_rtd_theme # noqa -import os import json # Configuration file for the Sphinx documentation builder. @@ -44,7 +43,6 @@ OUSTER_SDK_PATH = os.path.dirname(SRC_PATH) if not os.path.exists(os.path.join(OUSTER_SDK_PATH, "cmake")): raise RuntimeError("Could not guess OUSTER_SDK_PATH") -print(OUSTER_SDK_PATH) # https://packaging.python.org/en/latest/guides/single-sourcing-package-version/ @@ -120,7 +118,7 @@ def parse_version(): html_context = { 'display_github': True, 'github_user': 'ouster-lidar', - 'github_repo': 'ouster_example', + 'github_repo': 'ouster_sdk', # 'github_version': 'ouster/python-bindings', 'github_version': 'master', 'conf_py_path': '/docs/', @@ -175,6 +173,9 @@ def parse_version(): # tabs behavior sphinx_tabs_disable_tab_closing = True +enhanced_warnings = (os.getenv("DOXYGEN_ENHANCED_WARNINGS", + default="false").lower() == "true") + # -- Doxygen XML generation handlers ----------------------------------- def do_doxygen_generate_xml(app): # Only runs is breathe projects exists @@ -195,6 +196,7 @@ def do_doxygen_generate_xml(app): 'project': app.config.project, 'version': app.config.release, 'output_dir': doxygen_output_dir, + 'enhanced_warnings': "YES" if enhanced_warnings else "NO", 'warn_log_file': os.path.join( doxygen_output_dir, "warning_log.log") } diff --git a/docs/cpp/api.rst b/docs/cpp/api.rst index 81a41d6b..cc11f653 100644 --- a/docs/cpp/api.rst +++ b/docs/cpp/api.rst @@ -8,8 +8,4 @@ CPP API Documentation ouster_client ouster_pcap ouster_osf - -.. todo:: - Uncomment ``ouster_viz`` section below when we fix C++ PointViz doxygen comments in code - - .. ouster_viz + ouster_viz diff --git a/docs/cpp/building.rst b/docs/cpp/building.rst index 807d454e..59ef127c 100644 --- a/docs/cpp/building.rst +++ b/docs/cpp/building.rst @@ -9,7 +9,7 @@ jsoncpp, Eigen3, and tins libraries with headers installed on the system. The sa requires the GLFW3 and GLEW libraries. The C++ example code is available `on the Ouster Github -`_. Follow the instructions for cloning the project. +`_. Follow the instructions for cloning the project. Building on Linux / macOS ========================= @@ -19,7 +19,8 @@ To install build dependencies on Ubuntu:20.04+, run: .. code:: console $ sudo apt install build-essential cmake libjsoncpp-dev libeigen3-dev libcurl4-openssl-dev \ - libtins-dev libpcap-dev libglfw3-dev libglew-dev libspdlog-dev + libtins-dev libpcap-dev libglfw3-dev libglew-dev libspdlog-dev \ + libpng-dev libflatbuffers-dev You may also install curl with a different ssl backend, for example libcurl4-gnutls-dev or libcurl4-nss-dev. @@ -28,7 +29,7 @@ On macOS, install XCode and `homebrew `_ and run: .. code:: console - $ brew install cmake pkg-config jsoncpp eigen curl libtins glfw glew spdlog + $ brew install cmake pkg-config jsoncpp eigen curl libtins glfw glew spdlog libpng flatbuffers To build run the following commands: @@ -36,10 +37,10 @@ To build run the following commands: $ mkdir build $ cd build - $ cmake -DCMAKE_BUILD_TYPE=Release -DBUILD_EXAMPLES=ON - $ cmake --build . + $ cmake -DCMAKE_BUILD_TYPE=Release -DBUILD_EXAMPLES=ON + $ cmake --build . -- -j$(nproc) -where ```` is the location of the ``ouster_example`` source directory. The +where ```` is the location of the ``ouster_sdk`` source directory. The CMake build script supports several optional flags. Add any of the following to override the defaults: @@ -52,19 +53,6 @@ defaults: -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 =================== @@ -96,17 +84,19 @@ You should be able to install dependencies with 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`` +After these steps are complete, you should be able to open, build and run the ``ouster_sdk`` project using Visual Studio: 1. Start Visual Studio. 2. When the prompt opens asking you what type of project to open click **Open a local folder** and - navigate to the ``ouster_example`` source directory. + navigate to the ``ouster_sdk`` source directory. 3. After opening the project for the first time, wait for CMake configuration to complete. -4. Make sure Visual Studio is `building in release mode`_. You may experience performance issues and +4. Make sure the ``Desktop development with C++`` is installed. If not, install it using the Search bar + on the top of the screen. +5. Make sure Visual Studio is `building in release mode`_. You may experience performance issues and missing data in the visualizer otherwise. -5. In the menu bar at the top of the screen, select **Build > Build All**. -6. To use the resulting binaries, go to **View > Terminal** and run, for example: +6. In the menu bar at the top of the screen, select **Build > Build All**. +7. To use the resulting binaries, go to **View > Terminal** and run, for example: .. code:: powershell @@ -130,7 +120,7 @@ write point clouds out to CSV files: $ ./client_example where ```` can be the hostname (os-99xxxxxxxxxx) or IP of the sensor and ```` is the hostname or IP to which the sensor should send lidar data. You can also +data destination>`` is the hostname or IP to which the sensor should send lidar data. You can also supply ``""``, an empty string, to utilize automatic detection. On Windows, you may need to allow the client/visualizer through the Windows firewall to receive diff --git a/docs/cpp/examples/index.rst b/docs/cpp/examples/index.rst index 1405be1f..1423a9cb 100644 --- a/docs/cpp/examples/index.rst +++ b/docs/cpp/examples/index.rst @@ -6,4 +6,3 @@ CPP Examples :caption: CPP Examples Simple Examples - OSF Examples diff --git a/docs/cpp/examples/simple_examples.rst b/docs/cpp/examples/simple_examples.rst index 77fdc056..ce67ab5d 100644 --- a/docs/cpp/examples/simple_examples.rst +++ b/docs/cpp/examples/simple_examples.rst @@ -3,7 +3,7 @@ C++ Examples ============ To facilitate working with the Ouster C++ SDK, we provide these examples of common operations. The -examples explained below are compiled into executables which print to screen to demonstratre +examples explained below are compiled into executables which print to screen to demonstrate behavior. Build with ``BUILD_EXAMPLES`` and print to screen to demonstrate behavior. Sensor Configuration @@ -31,7 +31,7 @@ Let's look at the first step, where we get the configuration of the sensor as it The function ``get_config`` takes a ``sensor_config`` so we must declare it first. It returns true -if it succesfully retrieves the config, and false if an error occurs in connecting to the sensor and +if it successfully retrieves the config, and false if an error occurs in connecting to the sensor and setting the config. The function ``set_config`` works similarly, returning true if it successfully sets the config, and false otherwise. @@ -99,7 +99,7 @@ To see this in action, you can run the example executable ``lidar_scan_example`` $ lidar_scan_example $SAMPLE_DUAL_RETURNS_PCAP $SAMPLE_DUAL_RETURNS_JSON -The source code of ``lidar_scan_example`` is available `here `_. +The source code of ``lidar_scan_example`` is available `here `_. 2D Representations and 3D representations @@ -121,17 +121,17 @@ To run this example: representations_example $SAMPLE_DUAL_RETURNS_PCAP $SAMPLE_DUAL_RETURNS_JSON -The source code of ``representations_example`` is available `on the github `_. +The source code of ``representations_example`` is available `on the github `_. Reshaping XYZ to 2D +++++++++++++++++++ Users may find that they wish to access the ``x``, ``y``, and ``z`` coordinates of a single return -in a similar way. As the conversiton to cartesian coordinates returns an ``Eigen::Array`` ``n x 3``, +in a similar way. As the conversion to Cartesian coordinates returns an ``Eigen::Array`` ``n x 3``, with ``n = w * h``, reshaping the resulting array is necessary. -We can combine our knowledge in projecting into cartesian coordinates and destaggering using the +We can combine our knowledge in projecting into Cartesian coordinates and destaggering using the following function: .. literalinclude:: /../examples/representations_example.cpp @@ -147,7 +147,7 @@ This demonstrates the functionality with ``x``, but it can be easily expanded to Adjusting XYZLut With External Matrix +++++++++++++++++++++++++++++++++++++ -Users may find that they wish to apply an extra transform while projecting to cartesian coordinates. +Users may find that they wish to apply an extra transform while projecting to Cartesian coordinates. Such a transform, likely an extrinsics matrix of some sort, can be baked into the :cpp:struct:`ouster::XYZLut` created with :cpp:func:`ouster::make_xyz_lut`. diff --git a/docs/cpp/ouster_client/client.rst b/docs/cpp/ouster_client/client.rst index 1a786dfa..7f6cb718 100644 --- a/docs/cpp/ouster_client/client.rst +++ b/docs/cpp/ouster_client/client.rst @@ -31,7 +31,12 @@ Config And Metadata .. doxygenenum:: ouster::sensor::config_flags +.. doxygenfunction:: parse_and_validate_metadata(const std::string &json_data, ValidatorIssues &issues) +.. doxygenfunction:: parse_and_validate_metadata(const std::string &json_data, nonstd::optional &sensor_info, ValidatorIssues &issues) + +.. doxygenstruct:: ouster::ValidatorIssues + :members: Network Operations ================== diff --git a/docs/cpp/ouster_client/lidar_scan.rst b/docs/cpp/ouster_client/lidar_scan.rst index a046ae12..1819a0be 100644 --- a/docs/cpp/ouster_client/lidar_scan.rst +++ b/docs/cpp/ouster_client/lidar_scan.rst @@ -31,7 +31,7 @@ XYZLut .. doxygenfunction:: ouster::make_xyz_lut(size_t w, size_t h, double range_unit, const mat4d& beam_to_lidar_transform, const mat4d& transform, const std::vector& azimuth_angles_deg, const std::vector& altitude_angles_deg) -.. doxygenfunction:: ouster::make_xyz_lut(const sensor::sensor_info& sensor) +.. doxygenfunction:: ouster::make_xyz_lut(const sensor::sensor_info& sensor, bool use_extrinsics) ScanBatcher =========== diff --git a/docs/cpp/ouster_client/types.rst b/docs/cpp/ouster_client/types.rst index 6aa9aa38..ac521c31 100644 --- a/docs/cpp/ouster_client/types.rst +++ b/docs/cpp/ouster_client/types.rst @@ -99,13 +99,22 @@ UDP Profile IMU .. doxygenfunction:: ouster::sensor::udp_profile_imu_of_string Chan Field ---------------- - +---------- .. doxygenenum:: ouster::sensor::ChanFieldType -.. doxygenenum:: ouster::sensor::ChanField +.. doxygennamespace:: ouster::sensor::ChanField + +.. doxygenfunction:: ouster::sensor::to_string(ChanFieldType ft) + +Product Info +------------ +.. doxygenclass:: ouster::sensor::product_info -.. doxygenfunction:: ouster::sensor::to_string(ChanField field) +.. doxygenfunction:: ouster::sensor::operator==(const product_info& lhs, const product_info& rhs) + +.. doxygenfunction:: ouster::sensor::operator!=(const product_info& lhs, const product_info& rhs) + +.. doxygenfunction:: ouster::sensor::to_string(const sensor_info& info) Sensor Info =========== @@ -117,8 +126,6 @@ Sensor Info .. doxygenfunction:: ouster::sensor::metadata_from_json -.. doxygenfunction:: ouster::sensor::convert_to_legacy - .. doxygenfunction:: ouster::sensor::operator==(const sensor_info& lhs, const sensor_info& rhs) .. doxygenfunction:: ouster::sensor::operator!=(const sensor_info& lhs, const sensor_info& rhs) diff --git a/docs/cpp/ouster_client/version.rst b/docs/cpp/ouster_client/version.rst index aa37bcd6..33b26e29 100644 --- a/docs/cpp/ouster_client/version.rst +++ b/docs/cpp/ouster_client/version.rst @@ -11,9 +11,7 @@ Version .. doxygenstruct:: ouster::util::version :members: -.. doxygenfunction:: ouster::util::to_string - -.. doxygenfunction:: ouster::util::version_of_string +.. doxygenfunction:: ouster::util::version_from_string .. doxygengroup:: ouster_client_version_operators :content-only: diff --git a/docs/cpp/ouster_osf/index.rst b/docs/cpp/ouster_osf/index.rst index 36b3f3e1..552a81ef 100644 --- a/docs/cpp/ouster_osf/index.rst +++ b/docs/cpp/ouster_osf/index.rst @@ -14,7 +14,6 @@ Ouster OSF API meta_streaming_info.h metadata.h operations.h - pcap_source.h reader.h stream_lidar_scan.h writer.h diff --git a/docs/cpp/ouster_osf/operations.rst b/docs/cpp/ouster_osf/operations.rst index e42bb830..5a404722 100644 --- a/docs/cpp/ouster_osf/operations.rst +++ b/docs/cpp/ouster_osf/operations.rst @@ -11,5 +11,3 @@ operations.h .. doxygenfunction:: ouster::osf::restore_osf_file_metablob .. doxygenfunction:: ouster::osf::osf_file_modify_metadata - -.. doxygenfunction:: ouster::osf::pcap_to_osf diff --git a/docs/cpp/ouster_osf/pcap_source.rst b/docs/cpp/ouster_osf/pcap_source.rst deleted file mode 100644 index f4ff67c3..00000000 --- a/docs/cpp/ouster_osf/pcap_source.rst +++ /dev/null @@ -1,6 +0,0 @@ -============= -pcap_source.h -============= - -.. doxygenclass:: ouster::osf::PcapRawSource - :members: diff --git a/docs/cpp/ouster_osf/reader.rst b/docs/cpp/ouster_osf/reader.rst index 88a93cf4..b6750dd8 100644 --- a/docs/cpp/ouster_osf/reader.rst +++ b/docs/cpp/ouster_osf/reader.rst @@ -45,11 +45,6 @@ MessageRef .. doxygenclass:: ouster::osf::MessageRef :members: -ChunkRef --------- -.. doxygenclass:: ouster::osf::ChunkRef - :members: - MessagesChunkIter ----------------- .. doxygenstruct:: ouster::osf::MessagesChunkIter diff --git a/docs/cpp/ouster_osf/stream_lidar_scan.rst b/docs/cpp/ouster_osf/stream_lidar_scan.rst index 78b261b2..a1e21ca7 100644 --- a/docs/cpp/ouster_osf/stream_lidar_scan.rst +++ b/docs/cpp/ouster_osf/stream_lidar_scan.rst @@ -2,9 +2,6 @@ stream_lidar_scan.h =================== -.. doxygenstruct:: ouster::osf::zero_field - :members: - .. doxygenclass:: ouster::osf::LidarScanStreamMeta :members: diff --git a/docs/index.rst b/docs/index.rst index 476bca60..7333ca16 100644 --- a/docs/index.rst +++ b/docs/index.rst @@ -65,8 +65,8 @@ :hidden: :caption: External Links - Source Code - Issue Tracker + Source Code + Issue Tracker Sensor Documentation More Sample Data diff --git a/docs/migration/migration-20220927-20230114.rst b/docs/migration/migration-20220927-20230114.rst index 64183d3a..b5e3abec 100644 --- a/docs/migration/migration-20220927-20230114.rst +++ b/docs/migration/migration-20220927-20230114.rst @@ -95,7 +95,7 @@ migration guide (whichever is relevant). - ``encoder``: Encoder counts were part of the LEGACY UDP profile, representing the azimuth angle as a raw encoder count starting from 0 to a max value of 90111. It thus incremented 44 ticks per azimuth angle in 2048 mode, 88 ticks in 1024 mode and 176 in 512 mode. To recover encoder - infcount, you can multiply the ``measurement_id`` by the number dictated by your lidar mode. We + count, you can multiply the ``measurement_id`` by the number dictated by your lidar mode. We would suggest, however, migrating to use simply ``measurement_id`` and multiplying by ``360 degrees/ N`` where ``N`` is the number of columns in your mode (512, 1024, 2048) when you need the azimuth, thus untying any sense of azimuth from the internal mechanicals of the Ouster sensor. diff --git a/docs/migration/migration-20230114-20230403.rst b/docs/migration/migration-20230114-20230403.rst index a7faec48..4cb49c95 100644 --- a/docs/migration/migration-20230114-20230403.rst +++ b/docs/migration/migration-20230114-20230403.rst @@ -54,7 +54,7 @@ A number of long-deprecated members of the LidarScan interface have been removed - ``encoder``: Encoder counts were part of the LEGACY UDP profile, representing the azimuth angle as a raw encoder count starting from 0 to a max value of 90111. It thus incremented 44 ticks per azimuth angle in 2048 mode, 88 ticks in 1024 mode and 176 in 512 mode. To recover encoder - infcount, you can multiply the ``measurement_id`` by the number dictated by your lidar mode. We + count, you can multiply the ``measurement_id`` by the number dictated by your lidar mode. We suggest, however, migrating to use simply ``measurement_id`` and multiplying by ``360 degrees/ N`` where ``N`` is the number of columns in your mode (512, 1024, 2048) when you need the azimuth, thus untying any sense of azimuth from the internal mechanicals of the Ouster sensor. @@ -77,14 +77,14 @@ Python 3.7 reaches its end of life on June 27th, 2023. We will likely stop produ FW versions ~~~~~~~~~~~ FW 2.0 will be 3 years old in November 2023, with numerous upgrades introduces in subsequent FWs -2.1-2.5, and FW 3.0. New versions of the Python SDK and new commits of ouster_example will likely +2.1-2.5, and FW 3.0. New versions of the Python SDK and new commits of ouster_sdk will likely stop supporting direct communication with sensors running FW 2.0 by November 2023, although recorded data in the pcap+json format will continue to be read and supported. Ubuntu 18.04 ~~~~~~~~~~~~ Ubuntu 18.04 reaches its end of standard support in May 2023. We will likely stop supporting the C++ -and Python builds of ``ouster_example`` on 18.04 by November 2023. You will still be able to run +and Python builds of ``ouster_sdk`` on 18.04 by November 2023. You will still be able to run available Python wheels on your system if you so choose. @@ -120,7 +120,7 @@ We will discuss future FW removals and deprecations and their impact if you are TCP API ~~~~~~~ -The planned removal of the TCP API in future firmwares should not affect SDK users who communciate +The planned removal of the TCP API in future firmwares should not affect SDK users who communicate with the sensor only through SDK APIs, as we have already updated our code to use the HTTP API where it is available. diff --git a/docs/overview.rst b/docs/overview.rst index 24160832..912d5fe4 100644 --- a/docs/overview.rst +++ b/docs/overview.rst @@ -24,7 +24,7 @@ Quick links Supported Devices ----------------- -The sdk supports the following Ouster sensors: +The SDK supports the following Ouster sensors: * OS0_ * OS1_ @@ -51,6 +51,9 @@ The following table indicates the compatibility of each released SDK version and ===================================== ======= ======= ======= ======= ======= ======= ======= ======= SDK Tag (Release) / Python SDK FW 2.0 FW 2.1 FW 2.2 FW 2.3 FW 2.4 FW 2.5 FW 3.0 FW 3.1 ===================================== ======= ======= ======= ======= ======= ======= ======= ======= +C++ SDK 20240703 / Python SDK 0.13.0 no **yes** **yes** **yes** **yes** **yes** **yes** **yes** +C++ SDK 20240703 / Python SDK 0.12.0 **yes** **yes** **yes** **yes** **yes** **yes** **yes** **yes** +C++ SDK 20240423 / Python SDK 0.11.1 **yes** **yes** **yes** **yes** **yes** **yes** **yes** **yes** C++ SDK 20240423 / Python SDK 0.11.0 **yes** **yes** **yes** **yes** **yes** **yes** **yes** **yes** C++ SDK 20231031 / Python SDK 0.10.0 **yes** **yes** **yes** **yes** **yes** **yes** **yes** **yes** C++ SDK 20230710 / Python SDK 0.9.0 **yes** **yes** **yes** **yes** **yes** **yes** **yes** **yes** @@ -90,11 +93,11 @@ announcements`_ .. _Ouster sensor documentation: https://static.ouster.dev/sensor-docs/index.html .. _Ouster support: https://ouster.atlassian.net/servicedesk/customer/portal/8 -.. _Github issue tracker: https://github.com/ouster-lidar/ouster_example/issues -.. _Ouster Github announcements: https://github.com/ouster-lidar/ouster_example/discussions/categories/announcements -.. _Changelog: https://github.com/ouster-lidar/ouster_example/blob/master/CHANGELOG.rst +.. _Github issue tracker: https://github.com/ouster-lidar/ouster_sdk/issues +.. _Ouster Github announcements: https://github.com/ouster-lidar/ouster_sdk/discussions/categories/announcements +.. _Changelog: https://github.com/ouster-lidar/ouster_sdk/blob/master/CHANGELOG.rst .. _Ouster ROS 1 driver: https://github.com/ouster-lidar/ouster-ros -.. _Lifecycle Policies: https://github.com/ouster-lidar/ouster_example/discussions/532 +.. _Lifecycle Policies: https://github.com/ouster-lidar/ouster_sdk/discussions/532 .. _OS0: https://ouster.com/products/hardware/os0-lidar-sensor .. _OS1: https://ouster.com/products/hardware/os1-lidar-sensor .. _OS2: https://ouster.com/products/hardware/os2-lidar-sensor diff --git a/docs/python/api/client.rst b/docs/python/api/client.rst index ef25f315..aa5b489f 100644 --- a/docs/python/api/client.rst +++ b/docs/python/api/client.rst @@ -85,6 +85,20 @@ Metadata :members: :undoc-members: +.. autofunction:: parse_and_validate_metadata + +.. autoclass:: ValidatorIssues + :members: + :undoc-members: + +.. autoclass:: ValidatorEntry + :members: + :undoc-members: + +.. autoclass:: ProductInfo + :members: + :undoc-members: + ---- diff --git a/docs/python/api/examples.rst b/docs/python/api/examples.rst index 166326ae..0db0d127 100644 --- a/docs/python/api/examples.rst +++ b/docs/python/api/examples.rst @@ -33,7 +33,7 @@ PCAP Examples :mod:`ouster.sdk.examples.pcap` Open3D Examples :mod:`ouster.sdk.examples.open3d` ================================================= -.. automodule:: ouster.sdk.examples.open3d +.. automodule:: ouster.sdk.examples.open3d_example :members: ---- diff --git a/docs/python/api/osf.rst b/docs/python/api/osf.rst index db721397..2daf9b05 100644 --- a/docs/python/api/osf.rst +++ b/docs/python/api/osf.rst @@ -31,14 +31,6 @@ Reading :members: :undoc-members: -``ChunkRef`` wrapper for a `chunk` -~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ - -.. autoclass:: ChunkRef - :members: - :undoc-members: - :special-members: __len__ - ``MetadataStore`` for `metadata entries` ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ @@ -46,7 +38,7 @@ Reading :members: :undoc-members: -``MetadataEntry`` base class for all metadatas +``MetadataEntry`` base class for all metadata ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. autoclass:: MetadataEntry @@ -64,10 +56,6 @@ Writing OSF files :members: :undoc-members: -.. autoclass:: ChunksLayout - :members: - :undoc-members: - Common `metadata entries` ^^^^^^^^^^^^^^^^^^^^^^^^^ @@ -86,7 +74,7 @@ Common `metadata entries` :members: :undoc-members: -``StreamingInfo`` stream statisitcs +``StreamingInfo`` stream statistics ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. autoclass:: StreamingInfo @@ -113,4 +101,4 @@ High-Level API .. autoclass:: Scans :members: - :special-members: __iter__ \ No newline at end of file + :special-members: __iter__ diff --git a/docs/python/api/viz.rst b/docs/python/api/viz.rst index 735d55a3..aa283f3c 100644 --- a/docs/python/api/viz.rst +++ b/docs/python/api/viz.rst @@ -28,9 +28,6 @@ Core .. autoclass:: SimpleViz :members: -.. autoclass:: ScansAccumulator - :members: - .. autoattribute:: ouster.sdk.viz.spezia_palette :annotation: = spezia colors diff --git a/docs/python/devel.rst b/docs/python/devel.rst index d498eeeb..e03f510b 100644 --- a/docs/python/devel.rst +++ b/docs/python/devel.rst @@ -24,12 +24,12 @@ Building the Python SDK from source requires several dependencies: - `python `_ >= 3.8 (with headers and development libraries) - `pybind11 `_ >= 2.0 -The Python SDK source is available `on the Ouster Github `_. You should clone the whole project. +The Python SDK source is available `on the Ouster Github `_. You should clone the whole project. -Linux and macos +Linux and macOS --------------- -On supported Debian-based linux systems, you can install all build dependencies by running: +On supported Debian-based Linux systems, you can install all build dependencies by running: .. code:: console @@ -39,7 +39,7 @@ On supported Debian-based linux systems, you can install all build dependencies libglfw3-dev libglew-dev libspdlog-dev \ libpng-dev libflatbuffers-dev -On macos >= 11, using homebrew, you should be able to run: +On macOS >= 11, using Homebrew, you should be able to run: .. code:: console @@ -49,8 +49,8 @@ After you have the system dependencies, you can build the SDK with: .. code:: console - # first, specify the path to the ouster_example repository - $ export OUSTER_SDK_PATH= + # first, specify the path to the ouster_sdk repository + $ export OUSTER_SDK_PATH= # make sure you have an up-to-date version of pip and setuptools installed $ python3 -m pip install --user --upgrade pip setuptools @@ -86,8 +86,8 @@ The currently tested vcpkg tag is ``2024.04.26``. After that, using a developer .. code:: powershell - # first, specify the path to the ouster_example repository - PS > $env:OUSTER_SDK_PATH="" + # first, specify the path to the ouster_sdk repository + PS > $env:OUSTER_SDK_PATH="" # point cmake to the location of vcpkg (make sure to use an absolute path) PS > $env:CMAKE_TOOLCHAIN_FILE="\scripts\buildsystems\vcpkg.cmake" @@ -112,7 +112,7 @@ See the top-level README in the `Ouster Example repository`_ for more details on development environment on Windows. .. _vcpkg: https://github.com/microsoft/vcpkg/blob/master/README.md -.. _Ouster Example repository: https://github.com/ouster-lidar/ouster_example +.. _Ouster Example repository: https://github.com/ouster-lidar/ouster_sdk Developing diff --git a/docs/python/examples/conversion.rst b/docs/python/examples/conversion.rst index 735d00cd..256b6737 100644 --- a/docs/python/examples/conversion.rst +++ b/docs/python/examples/conversion.rst @@ -22,16 +22,16 @@ To convert the first ``5`` scans of our sample data from a pcap file, you can tr .. code-tab:: console Linux/macOS - $ python3 -m ouster.sdk.examples.pcap $SAMPLE_DATA_PCAP_PATH $SAMPLE_DATA_JSON_PATH pcap-to-csv --scan-num 5 + $ ouster-cli source --meta $SAMPLE_DATA_JSON_PATH $SAMPLE_DATA_PCAP_PATH slice 0:5 save output.csv .. code-tab:: powershell Windows x64 - PS > py -3 -m ouster.sdk.examples.pcap $SAMPLE_DATA_PCAP_PATH $SAMPLE_DATA_JSON_PATH pcap-to-csv --scan-num 5 + PS > ouster-cli.exe source --meta $SAMPLE_DATA_JSON_PATH $SAMPLE_DATA_PCAP_PATH slice 0:5 save output.csv -The source code of an example below: +The following function implements the pcap to csv conversion above. -.. literalinclude:: /../python/src/ouster/cli/core/pcap.py +.. literalinclude:: /../python/src/ouster/cli/plugins/source_save.py :start-after: [doc-stag-pcap-to-csv] :end-before: [doc-etag-pcap-to-csv] :linenos: diff --git a/docs/python/examples/lidar-scan.rst b/docs/python/examples/lidar-scan.rst index 7b7c25ae..75b68f1e 100644 --- a/docs/python/examples/lidar-scan.rst +++ b/docs/python/examples/lidar-scan.rst @@ -74,7 +74,7 @@ To generate staggered and destaggered images yourself, you can try the following plt.imshow(ranges_destaggered, cmap='gray', resample=False) .. todo:: - (Kai) Might be nice to cover either here or in the reference how to + Might be nice to cover either here or in the reference how to duplicate timestamps into an 'img', "destagger" it, and then use for for association of XYZ points with their timestamps diff --git a/docs/python/examples/osf-examples.rst b/docs/python/examples/osf-examples.rst index 1f0087cf..db59e78a 100644 --- a/docs/python/examples/osf-examples.rst +++ b/docs/python/examples/osf-examples.rst @@ -138,8 +138,8 @@ fields, we can keep only ``3`` and save the disk space and bandwidth during repl A general scheme of writing scans to the OSF with Writer: 0. Create ``osf.Writer`` with the output file name, lidar metadata(s) (``ouster.sdk.client.SensorInfo``) and optionally the desired output scan fields. -1. Use the writers's ``save`` function ``writer.save(index, scan)`` to encode the LidarScan ``scan`` into the - underlying message buffer for lidar ``index`` and finally push it to disk. If you have multiple lidars you can +1. Use the writer's ``save`` function ``writer.save(index, scan)`` to encode the LidarScan ``scan`` into the + underlying message buffer for lidar ``index`` and finally push it to disk. If you have multiple lidar sensors you can save the scans simultaneously by providing them in an array to ``writer.save``. .. literalinclude:: /../python/src/ouster/sdk/examples/osf.py diff --git a/docs/python/examples/record-stream.rst b/docs/python/examples/record-stream.rst index 444897e8..a0b3f17e 100644 --- a/docs/python/examples/record-stream.rst +++ b/docs/python/examples/record-stream.rst @@ -12,7 +12,7 @@ Recording, Streaming, and Conversion Recording Sensor Data ====================== -It's easy to record data to a pcap file from a sensor programatically. Let's try it on a +It's easy to record data to a pcap file from a sensor programmatically. Let's try it on a :ref:`configured` sensor: .. tabs:: diff --git a/docs/python/examples/udp-packets.rst b/docs/python/examples/udp-packets.rst index 9a489a41..54853769 100644 --- a/docs/python/examples/udp-packets.rst +++ b/docs/python/examples/udp-packets.rst @@ -5,18 +5,19 @@ Lidar and IMU Packets ===================== -The :py:class:`.PacketSource` is the basic interface for sensor packets. It can be advantageous to +The :py:class:`.PacketMultiSource` is the basic interface for sensor packets. It can be advantageous to work with packets directly when latency is a concern, or when you wish to examine the data packet by packet, e.g., if you wish to examine timestamps of packets. -Let's make a :py:class:`.PacketSource` from our sample data using :py:class:`.pcap.Pcap`: +Let's make a :py:class:`.PacketMultiSource` from our sample data using :py:class:`.pcap.PcapMultiPacketReader`: .. code:: python + from ouster.sdk import pcap with open(metadata_path, 'r') as f: metadata = client.SensorInfo(f.read()) - source = pcap.Pcap(pcap_path, metadata) + source = pcap.PcapMultiPacketReader(pcap_path, metadatas=[metadata]) Now we can read packets from ``source`` with the following code: diff --git a/docs/python/examples/visualizations.rst b/docs/python/examples/visualizations.rst index d5e353df..9b956727 100644 --- a/docs/python/examples/visualizations.rst +++ b/docs/python/examples/visualizations.rst @@ -84,13 +84,13 @@ As an example, you can view frame ``84`` from the sample data by running the fol .. code-tab:: console Linux/macOS - $ python3 -m ouster.sdk.examples.open3d \ - --pcap $SAMPLE_DATA_PCAP_PATH --meta $SAMPLE_DATA_JSON_PATH --start 84 --pause + $ python3 -m ouster.sdk.examples.open3d_example \ + --pcap $SAMPLE_DATA_PCAP_PATH --start 84 --pause .. code-tab:: powershell Windows x64 - PS > py -3 -m ouster.sdk.examples.open3d ^ - --pcap $SAMPLE_DATA_PCAP_PATH --meta $SAMPLE_DATA_JSON_PATH --start 84 --pause + PS > py -3 -m ouster.sdk.examples.open3d_example ^ + --pcap $SAMPLE_DATA_PCAP_PATH --start 84 --pause You may also want to try the ``--sensor`` option to display the output of a running sensor. Use the ``-h`` flag to see a full list of command line options and flags. @@ -107,7 +107,7 @@ You should be able to click and drag the mouse to look around. You can zoom in a mouse wheel, and hold control or shift while dragging to pan and roll, respectively. Hitting the spacebar will start playing back the rest of the pcap in real time. Note that reasonable -performance for realtime playback requires relatively fast hardware, since Open3d runs all rendering +performance for real-time playback requires relatively fast hardware, since Open3d runs all rendering and processing in a single thread. All of the visualizer controls are listed in the table below: @@ -174,7 +174,7 @@ datatype and plot a range image where each column corresponds to a single azimut range_field = scan.field(client.ChanField.RANGE) range_img = client.destagger(info, range_field) -We can plot the results using standard Python tools that work with numpy datatypes. Here, we extract +We can plot the results using standard Python tools that work with numpy data types. Here, we extract a column segment of the range data and display the result: .. code:: python diff --git a/docs/python/quickstart.rst b/docs/python/quickstart.rst index eb37c49a..e14ad9c8 100644 --- a/docs/python/quickstart.rst +++ b/docs/python/quickstart.rst @@ -92,10 +92,11 @@ regardless of whether the source data comes from a sensor, pcap, or osf file. Notice here that rather than we try to load and parse the metadata ourselves we only need to pass to metadata to the method through ``meta`` parameter and the method will take care of loading it and associating it with the source object. The ``meta`` parameter however is optional and can be omitted. When the ``meta`` parameter is not -set explicity the ``open_source`` method will attempt to locate the metadata automatically for us and we can reduce +set explicitly the ``open_source`` method will attempt to locate the metadata automatically for us and we can reduce the call to: .. code:: python + >>> source = open_source(pcap_path) However if metadata file is not in the same folder as the pcap and don't have a shared name prefix the method will @@ -108,7 +109,7 @@ fail. storage) contains scans from more than one sensor, in this case, users can set the ``sensor_idx`` to a any value between zero and ``sensors_count -1`` to access and manipulate scans from a specific sensor by the order they appear in the file. Alternatively, if users set the value of ``sensor_idx`` to ``-1`` then ``open_source`` will return a - slightly differnt interface from ``ScanSource`` which is the ``MultiScanSource`` this interface and as the name + slightly different interface from ``ScanSource`` which is the ``MultiScanSource`` this interface and as the name suggests allows users to work with sensor data collected from multiple sensors at the same time. The main different between the ``MultiScanSource`` and the ``ScanSource`` is the expected return of some of the object @@ -118,7 +119,7 @@ fail. is being used. This is true even when the pcap file contains data for a single sensor. -On the other hand, if the user wants to open an osf file or access the a live sensor, all that changes is url +On the other hand, if the user wants to open an OSF file or access a live sensor, all that changes is URL of the source. For example, to interact with a live sensor the user can execute the following snippet: .. code:: python @@ -132,7 +133,7 @@ Obtaining sensor metadata ------------------------- Every ScanSource holds a reference to the sensor metadata, which has crucial information that is important when -when processing the invidivual scans. Continuing the example, a user this can access the metadata through the +when processing the individual scans. Continuing the example, a user this can access the metadata through the ``metadata`` property of a ``ScanSource`` object: .. code:: python @@ -161,9 +162,9 @@ As we noted earlier, if we set ``sensor_idx=-1`` when invoking ``open_source`` m ``MultiScanSource``, which always addresses a group of sensors. Thus, when iterating over the ``source`` the user receives a collated set of scans from the addressed sensors per iteration. The ``MultiScanSource`` examines the timestamp of every scan from every sensor and returns a list of scans that fit within the same time window as single -batch. The size of the batch is fixed corresponding to how many sensors contained in the pcap or osf file. However, +batch. The size of the batch is fixed corresponding to how many sensors contained in the pcap or OSF file. However, the collation could yield a null value if one or more of the sensors didn't produce a ``LidarScan`` object that fits -within the time frame of current batch or iteration. Thus, depending on the operation at hand it is crticial to check +within the time frame of current batch or iteration. Thus, depending on the operation at hand it is critical to check if we got a valid ``LidarScan`` object when examining the iteration output of a ``MultiScanSource``. If we are to perform the same example as above when ``source`` is a handle to ``MultiScanSource`` and display the frame_id of ``LidarScan`` objects the belongs to the same batch on the same line the code needs to updated to the following: @@ -174,7 +175,7 @@ perform the same example as above when ``source`` is a handle to ``MultiScanSour >>> source_iter = iter(source) >>> for scans in source_iter: ... for scan in scans: # source_iter here returns a list of scans - ... if scan: # check if invidiual scan object is valid + ... if scan: # check if individual scan object is valid ... print(scan.frame_id, end=', ') ... print() # new line for next batch ... ctr += 1 @@ -192,8 +193,8 @@ Using indexing and slicing capabilities of a ScanSource One of the most prominent new features of the ScanSource API, (besides being able to address multi sensors), is the ability to use indexing and slicing when accessing the stored scans within the ``LidarScan`` source. Currently, this capability is only supported for indexable sources. That is to say, the functionality we are discussing can only be used -when accessing a pcap or an osf file with indexing turned on. To turn on indexing simply add the ``index`` flag and set -it ``True`` when opening a pcap or osf file: +when accessing a pcap or an OSF file with indexing turned on. To turn on indexing simply add the ``index`` flag and set +it ``True`` when opening a pcap or OSF file: .. code:: python @@ -243,15 +244,15 @@ To print frame_id of the last 10 LidarScans we do: ... print(scan.frame_id) -Finally, as you would expect from a typical slice operation, you can also using negative step and also use a reversed -iteration as shown in the following example: +Finally, as you would expect from a typical slice operation you can also use a step value, though reversed +iteration is not supported. .. code:: python >>> for scan in source[0:10:2]: # prints the frame_id of every second scan of the first 10 scans ... print(scan.frame_id) - >>> for scan in source[10:0:-1]: # prints the frame_id of every scan of the first 10 scans in reverse + >>> for scan in source[10:0:-1]: # unsupported ... print(scan.frame_id) @@ -284,7 +285,7 @@ following snippet shows few examples to demonstrate this capability: Using the client API ==================== -The client API provides :py:class:`.PacketSource` implementations, as well as access to methods for configuring a sensor or reading metadata. +The client API provides :py:class:`.PacketMultiSource` implementations, as well as access to methods for configuring a sensor or reading metadata. As such, you can use it instead of the ``ScanSource`` API if you prefer to work with individual packets rather than lidar scans. @@ -301,13 +302,28 @@ information from ``metadata_path`` first, using the client module: ... info = client.SensorInfo(f.read()) Now that we've parsed the metadata file into a :py:class:`.SensorInfo`, we can use it to read our -captured UDP data by instantiating :py:class:`.pcap.Pcap`. This class acts as a -:py:class:`.PacketSource` and can be used in many of the same contexts as a real sensor. +captured UDP data by instantiating :py:class:`.pcap.PcapMultiPacketReader`. This class acts as a +:py:class:`.PacketMultiSource` and can be used in many of the same contexts as a real sensor. .. code:: python >>> from ouster.sdk import pcap - >>> source = pcap.Pcap(pcap_path, info) + >>> source = pcap.PcapMultiPacketReader(pcap_path, metadatas=[info]) + >>> for sensor_idx, packet in source: + >>> ... + +The ``source`` object returned is an ``Iterable`` of tuples each containing the sensor index and a packet that +originates from that sensor. (The index corresponds to the positions of the ``SensorInfo`` instances provided to the +``metadatas`` keyword argument.) + +To limit the output to an ``Iterable`` of packets (without the sensor index) use the ``single_source`` method, providing +the index of the sensor you want to read from (which will be ``0`` if you've only provided a single ``SensorInfo`` +instance.) + +.. code:: python + + >>> for packet in source.single_source(0): + >>> ... To visualize data from this pcap file, proceed to :doc:`/python/examples/visualizations` examples. @@ -363,20 +379,37 @@ Now configure the client: >>> from ouster.sdk import client >>> config = client.SensorConfig() + >>> config.udp_profile_lidar = client.UDPProfileLidar.PROFILE_LIDAR_RNG19_RFL8_SIG16_NIR16_DUAL >>> config.udp_port_lidar = 7502 >>> config.udp_port_imu = 7503 >>> config.operating_mode = client.OperatingMode.OPERATING_NORMAL >>> client.set_config(hostname, config, persist=True, udp_dest_auto = True) -Just like with the sample data, you can create a :py:class:`.PacketSource` from the sensor: +When using a :py:class:`.SensorPacketSource`, provide a list of hostname/``SensorConfig`` pairs: .. code:: python - >>> source = client.Sensor(hostname, 7502, 7503) + >>> source = client.SensorPacketSource([(hostname, config)]) >>> info = source.metadata -Now we have a ``source`` from our sensor! You're ready to record, visualize to visualize data from your sensor, proceed to -:doc:`/python/examples/visualizations` examples. Or you can check out other things you can do with a +Now we have a ``source`` from our sensor! As before, since ``SensorPacketSource`` supports multiple sensors, you have +the option to read it as an ``Iterable[List[Optional[LidarScan]]]`` of each containing the sensor index and a scan, or to limit the output +to a single sensor (an ``Iterable[LidarScan]``.) + +.. code:: python + + >>> for scans in source: + >>> ... # a list of scans, one per sensor + +Or, + +.. code:: python + + >>> for scan in source.single_source(0): + >>> ... # a single scan for sensor index 0 + +At this point, you're ready visualize to visualize data from your sensor. Proceed to +:doc:`/python/examples/visualizations` for examples. Or you can check out other things you can do with a ``source`` in the Python :doc:`/python/examples/index`. diff --git a/docs/python/slam-api-example.rst b/docs/python/slam-api-example.rst index 59909ff1..f1c8de3b 100644 --- a/docs/python/slam-api-example.rst +++ b/docs/python/slam-api-example.rst @@ -24,31 +24,37 @@ between consecutive scans from ouster.sdk.mapping.slam import KissBackend import numpy as np import ouster.sdk.client as client - data_source = open_source(pcap_path) + source_file_path = "/PATH_TO_THE_FILE" + data_source = open_source(source_file_path, sensor_idx=-1) slam = KissBackend(data_source.metadata, max_range=75, min_range=1, voxel_size=1.0) last_scan_pose = np.eye(4) - for idx, scan in enumerate(data_source): - scan_w_poses = slam.update(scan) - col = client.first_valid_column(scan_w_poses) - # scan_w_poses.pose is a list where each pose represents a column points' pose. - # use the first valid scan's column pose as the scan pose - scan_pose = scan_w_poses.pose[col] - scan_ts = scan.timestamp[col] - print(f"idx = {idx} at timestamp {scan_ts} has the pose {scan_pose}") - - # calculate the inverse transformation of the last scan pose - inverse_last = np.linalg.inv(last_scan_pose) - # calculate the pose difference by matrix multiplication - pose_diff = np.dot(inverse_last, scan_pose) - # extract rotation and translation - rotation_diff = pose_diff[:3, :3] - translation_diff = pose_diff[:3, 3] - print(f"idx = {idx} and Rotation Difference: {rotation_diff}, " - f"Translation Difference: {translation_diff}") - - -SLAM with Visulizer and Accumulated Scans + for idx, scans in enumerate(data_source): + # slam.update takes a list of scans as input and returns a list of scans, + # supporting potential multi-sensor configurations + scans_w_poses = slam.update(scans) + if not scans_w_poses: + continue + # use the first scan for calculation + col = client.first_valid_column(scans_w_poses[0]) + # scans_w_poses[0].pose is a list of poses where each pose represents a column + # points' pose. Use the first valid scan's column pose as the scan pose + scan_pose = scans_w_poses[0].pose[col] + scan_ts = scans[0].timestamp[col] + print(f"idx = {idx} at timestamp {scan_ts} has the pose {scan_pose}") + + # calculate the inverse transformation of the last scan pose + inverse_last = np.linalg.inv(last_scan_pose) + # calculate the pose difference by matrix multiplication + pose_diff = np.dot(inverse_last, scan_pose) + # extract rotation and translation + rotation_diff = pose_diff[:3, :3] + translation_diff = pose_diff[:3, 3] + print(f"idx = {idx} and Rotation Difference: {rotation_diff}, " + f"Translation Difference: {translation_diff}") + + +SLAM with Visualizer and Accumulated Scans ========================================= Visualizers and Accumulated Scans are also available for monitoring the performance of the algorithm, as well as for demonstration and feedback purposes. @@ -56,20 +62,17 @@ as well as for demonstration and feedback purposes. .. code:: python from ouster.sdk import open_source - from functools import partial - from ouster.sdk.viz import SimpleViz, ScansAccumulator + from ouster.sdk.viz import SimpleViz from ouster.sdk.mapping.slam import KissBackend - data_source = open_source(pcap_path) + + source_file_path = "/PATH_TO_THE_FILE" + data_source = open_source(source_file_path, sensor_idx=-1) slam = KissBackend(data_source.metadata, max_range=75, min_range=1, voxel_size=1.0) - scans_w_poses = map(partial(slam.update), data_source) - scans_acc = ScansAccumulator(data_source.metadata, - accum_max_num=10, - accum_min_dist_num=1, - map_enabled=True, - map_select_ratio=0.01) + scans_w_poses = map(lambda x: slam.update(x)[0], data_source) + + SimpleViz(data_source.metadata, accum_max_num=10).run(scans_w_poses) - SimpleViz(data_source.metadata, scans_accum=scans_acc, rate=1.0).run(scans_w_poses) More details about the visualizer and accumulated scans can be found at the :ref:`Ouster Visualizer ` and :ref:`Scans Accumulator ` diff --git a/docs/python/viz/index.rst b/docs/python/viz/index.rst index e6ea8ec3..f2d4e6e8 100644 --- a/docs/python/viz/index.rst +++ b/docs/python/viz/index.rst @@ -44,3 +44,7 @@ below: viz-api-tutorial +.. toctree:: + + viz-scans-accum + diff --git a/docs/python/viz/viz-api-tutorial.rst b/docs/python/viz/viz-api-tutorial.rst index 5dccc26d..79e126dc 100644 --- a/docs/python/viz/viz-api-tutorial.rst +++ b/docs/python/viz/viz-api-tutorial.rst @@ -72,8 +72,8 @@ grayscale images are supported, though you can use :meth:`.viz.Image.set_mask`, masks, to get color. The :class:`.viz.Image` screen coordinate system is *height-normalized* and goes from bottom to top -(``[-1, +1]``) for the ``y`` cordinate, and from left to right (``[-aspect, +aspect]``) for the -``x`` coordidate, where:: +(``[-1, +1]``) for the ``y`` coordinate, and from left to right (``[-aspect, +aspect]``) for the +``x`` coordinate, where:: aspect = viewport width in Pixels / viewport height in Pixels @@ -233,7 +233,7 @@ from :class:`.client.SensorInfo` (metadata object) and 2D ``RANGE`` image as an .. note:: - Please refer to :doc:`/reference/lidar-scan` for details about interal ``LidarScan`` + Please refer to :doc:`/reference/lidar-scan` for details about internal ``LidarScan`` representations and its basic operations. @@ -304,7 +304,7 @@ higher-order visual component :class:`.viz.LidarScanViz` that enables: - 3D point cloud and two 2D fields images in one view - color palettes for 3D point cloud coloration - point cloud point size increase/decrease -- toggles for view of different fields images and increasing/decreaseing their size +- toggles for view of different fields images and increasing/decreasing their size - dual return point clouds and fields images support - key handlers for all of the above @@ -382,7 +382,7 @@ Keyboard handlers We haven't yet covered the :class:`.viz.Camera` object and ways to control it. So here's a quick example of how we can map ``R`` key to move the camera closer or farther from the target. -Random :meth:`.viz.Camera.dolly` change on keypress: +Random :meth:`.viz.Camera.dolly` change on key press: .. literalinclude:: /../python/src/ouster/sdk/examples/viz.py :start-after: [doc-stag-key-handlers] diff --git a/docs/python/viz/viz-run.rst b/docs/python/viz/viz-run.rst index 115f4d8f..55129f41 100644 --- a/docs/python/viz/viz-run.rst +++ b/docs/python/viz/viz-run.rst @@ -13,7 +13,7 @@ where ```` is the hostname (os-99xxxxxxxxxx) or IP of the senso Alternately, to replay the existing data from ``pcap`` and ``json`` files call the visualizer as:: - $ ouster-cli source [--meta ] viz + $ ouster-cli source [--meta ] viz .. figure:: /images/ouster-viz.png :align: center @@ -41,10 +41,10 @@ Keyboard Controls Key What it does ================ =============================================== ``shift`` Camera Translation with mouse drag - ``w`` Camera pitch up - ``s`` Camera pitch down - ``a`` Camera yaw left - ``d`` Camera yaw right + ``w`` Camera pitch down + ``s`` Camera pitch up + ``a`` Camera yaw right + ``d`` Camera yaw left ``R`` Reset camera ``ctr-r`` Set camera to the birds-eye view ``u`` Toggle camera mode FOLLOW/FIXED @@ -78,7 +78,9 @@ Keyboard Controls ``p / P`` Increase/decrease point size ``m / M`` Cycle point cloud coloring mode ``f / F`` Cycle point cloud color palette + ``ctrl + [N]`` Enable/disable the Nth sensor cloud where N is `1` to `9` ``1`` Toggle first return point cloud visibility + ``2`` Toggle second return point cloud visibility ``6`` Toggle scans accumulation view mode (ACCUM) ``7`` Toggle overall map view mode (MAP) ``8`` Toggle poses/trajectory view mode (TRACK) @@ -110,9 +112,9 @@ The visualizer also includes an option to control the orientation of the point c loaded. If you possess, say, an OS-DOME mounted an upside down, you can start the visualizer with the option ``--extrinsics``:: - $ ouster-cli source 10.0.0.13 viz --extrinsics -1 0 0 0 0 1 0 0 0 0 -1 0 0 0 0 1 + $ ouster-cli source --extrinsics -1 0 0 0 0 1 0 0 0 0 -1 0 0 0 0 1 10.0.0.13 viz -The input is a row-major homogenous matrix. +The input is a row-major homogeneous matrix. For other options, run ``ouster-cli source viz -h`` @@ -126,9 +128,9 @@ For other options, run ``ouster-cli source viz -h`` Advanced usage with sensor -------------------------- -The Ouster visualizer automatically configures connected sensors to send data to the appropriate udp +The Ouster visualizer automatically configures connected sensors to send data to the appropriate UDP destination address. If your sensor is already configured appropriately, you may find it useful to -use the argument ``--no-auto-dest`` to save time by skipping the roundtrip to reconfigure the +use the argument ``--no-auto-udp-dest`` to save time by skipping the round trip to reconfigure the sensor. diff --git a/docs/python/viz/viz-scans-accum.rst b/docs/python/viz/viz-scans-accum.rst index 108dde55..358982b5 100644 --- a/docs/python/viz/viz-scans-accum.rst +++ b/docs/python/viz/viz-scans-accum.rst @@ -1,6 +1,6 @@ .. _viz-scans-accum: -Visualize SLAM Poses using ``ScansAccumulator`` - accumulates track, point clouds and map views +Visualize SLAM Poses using ``SimpleViz`` - accumulates track, point clouds and map views ----------------------------------------------------------------------------------------------- .. contents:: @@ -11,30 +11,34 @@ Visualize SLAM Poses using ``ScansAccumulator`` - accumulates track, point cloud Overview ^^^^^^^^^ -**ScansAccumulator** is the continuation of the efforts to view the lidar data with poses that may -come within the ``LidarScan.pose`` property. When poses are not present in the ``LidarScan`` -**ScansAccumulator** may still be useful to view the accumulated *N* scans from the live +Beginning in ouster-sdk 0.13.0, it's easier than ever to use "map" or "scan" +accumulation to visualize lidar scans that contain pose information. The +default visualizer, ``SimpleViz``, present in both ``ouster-cli`` and Ouster +SDK's Python API support these accumulation modes out of the box. + +Furthermore, when poses are not present in the ``LidarScan`` accumulation may +still be useful to view the accumulated *N* scans from the live sensor/recording and reveal the accuracy/repeatability of the data. Available view modes ~~~~~~~~~~~~~~~~~~~~~ -There are three view modes of **ScansAccumulator**, that may be enabled/disabled depending on -it's params and the data that is passed throught it: +There are three view modes of accumulation implemented in the default +visualizer that may be enabled/disabled depending on its parameters and the data +that is passed through it: - * **poses** (or **TRACK**), key ``8`` - all scan poses in a trajectory/path view (available only - if poses data is present in scans) - * **scan map** (or **MAP**), key ``7`` - overall map view with select ratio of random points - from every scan (available for scans with/without poses) - * **scan accum** (or **ACCUM**), key ``6`` - accumulated *N* scans (key frames) that is picked - according to params (available for scans with/without poses) + * **poses** mode, key ``8`` - all scan poses in a trajectory/path view (if poses data is present in scans.) + * **map accumulation** mode, key ``7`` - overall map view with select ratio of random points + from every scan (available for scans with or without poses.) + * **scan accumulation** mode, key ``6`` - accumulated *N* scans (key frames) that is picked + according to parameters (available for scans with or without poses.) Key bindings ~~~~~~~~~~~~~ -Keyboard controls available with **ScansAccumulator**: +Keyboard controls available with **SimpleViz**: ============== ============================================================= Key What it does @@ -52,7 +56,7 @@ Keyboard controls available with **ScansAccumulator**: Use in Ouster SDK CLI ^^^^^^^^^^^^^^^^^^^^^^ -**ScansAccumulator** is accessible when visualizing data with ``ouster-cli`` using the ``viz`` command. +**SimpleViz** is accessible when visualizing data with ``ouster-cli`` using the ``viz`` command. Here are the ``viz`` command options that affect it: @@ -71,7 +75,7 @@ Examples of the CLI commands: Dense accumulated clouds view (with every point of a scan) ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ -To obtain the densest view use the ``--accum-num N --accum-every 1`` params where ``N`` is the +To obtain the densest view use the ``--accum-num N --accum-every 1`` parameters where ``N`` is the number of clouds to accumulate (``N`` up to 100 is generally small enough to avoid slowing down the viz interface):: @@ -98,7 +102,7 @@ And here is the final result when viz is done and stopped (``-e stop``) after pl .. figure:: /images/scans_accum_map_all_scan.png - Data fully replayed with map and accum enabled (last current scan is displayed here in grey + Data fully replayed with map and accum enabled (last current scan is displayed here in gray palette) @@ -113,81 +117,70 @@ And here is the final result when viz is done and stopped (``-e stop``) after pl positions) -Programmatic use with (and without) PointViz -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -With ``point_viz: PointViz`` object the ``ScansAccumulator`` can be used as a regular -``LidarScanViz`` and passed directly to ``SimpleViz``:: - - from ouster.sdk.viz import PointViz, add_default_controls, ScansAccumulator, SimpleViz - - point_viz = PointViz("SimpleViz usecase") - add_default_controls(point_viz) - - # ... get scans_w_poses Scans source ... - - scans_acc = ScansAccumulator(meta, - point_viz=point_viz, - accum_max_num=10, - accum_min_dist_num=1, - map_enabled=True, - map_select_ratio=0.5) - - SimpleViz(scans_acc, rate=1.0).run(scans_w_poses) - +Programmatic use +^^^^^^^^^^^^^^^^ -Alternatively with a ``PointViz`` it can be used as a canvas to draw the final state only:: +To use any of these accumulation modes, provide their configuration directly to ``SimpleViz`` via keyword arguments. The following snippet will play back scans from the source ``scans_w_poses`` and the sensor configuration provided by ``meta``:: - from ouster.sdk.viz import ScansAccumulator, add_default_controls, PointViz + import sys + from ouster.sdk import open_source + from ouster.sdk.viz import SimpleViz + from ouster.sdk.mapping import KissBackend - point_viz = PointViz("Overall map case") - add_default_controls(point_viz) + source_uri = sys.argv[1] + source = open_source(source_uri) - # ... get scans_w_poses Scans source ... + kiss_icp = KissBackend([source.metadata]) - scans_acc = ScansAccumulator(meta, - point_viz=point_viz, - accum_max_num=10, - accum_min_dist_num=1, - map_enabled=True, - map_select_ratio=0.5) - for scan in scans_w_poses: - scans_acc.update(scan) + def scans_w_poses(): + for scan in source: + yield kiss_icp.update([scan]) - scans_acc.draw(update=True) - point_viz.update() - point_viz.run() + viz = SimpleViz( + source.metadata, + accum_max_num=100, + accum_min_dist_num=0, + accum_min_dist_meters=4, + rate=1, + on_eof='stop' + ) -Without ``PointViz`` it can be used as in the following snippet to accumulate all data and use the -data later to draw anywhere (here we still use the ``PointViz`` and ``viz.Cloud()`` as a main -graphing tool, but it can be ``matplotlib`` instead):: + viz.run(scans_w_poses()) - from ouster.sdk.viz import grey_palette, ScansAccumulator, Cloud, add_default_controls, PointViz +Alternatively, ``LidarScanViz`` (which is a lower-level visualizer that implements ``SimpleViz``) can display a static map +from scans that have poses computed in a preprocessing step:: - # ... get scans_w_poses Scans source ... + import sys + from tqdm import tqdm # for progress bar + from ouster.sdk import open_source + from ouster.sdk.viz import LidarScanViz + from ouster.sdk.viz.accumulators_config import LidarScanVizAccumulatorsConfig + from ouster.sdk.mapping import KissBackend - # create scans accum without PointViz - scans_acc = ScansAccumulator(meta, - map_enabled=True, - map_select_ratio=0.5) + source_uri = sys.argv[1] + source = open_source(source_uri) - # processing doesn't require viz presence in scans accum - for scan in scans_w_poses: - scans_acc.update(scan) + kiss_icp = KissBackend([source.metadata]) - point_viz = PointViz("Standalone case") - add_default_controls(point_viz) + num_scans_to_map = 200 + scans_w_poses = [ + kiss_icp.update([scan]) for _, scan in + zip(tqdm(range(num_scans_to_map), desc="Computing map"), source) + ] - # draw the cloud manually to the viz using ScansAccumulator MAP data - cloud_map = Cloud(scans_acc._map_xyz.shape[0]) - cloud_map.set_xyz(scans_acc._map_xyz) - cloud_map.set_key(scans_acc._map_keys["NEAR_IR"]) - cloud_map.set_palette(grey_palette) - cloud_map.set_point_size(1) - point_viz.add(cloud_map) + viz = LidarScanViz( + [source.metadata], + accumulators_config = LidarScanVizAccumulatorsConfig( + accum_max_num=100, + accum_min_dist_num=0, + accum_min_dist_meters=4 + ) + ) -In the example above one might use ``matplotlib`` with some modifications to use palette for picking -the key color. + for scan in scans_w_poses: + viz.update(scan) + viz.draw(update=True) + viz.run() diff --git a/docs/reference/lidar-scan.rst b/docs/reference/lidar-scan.rst index 3fa38633..381a524c 100644 --- a/docs/reference/lidar-scan.rst +++ b/docs/reference/lidar-scan.rst @@ -6,7 +6,7 @@ Lidar Scan API In this reference document, we explain a core concept in both C++ and Python, and will often link function names in the order (``C++ class/function``, ``Python class/function``). For -langauge-specific usage and running example code, please see the :ref:`Python LidarScan examples` +language-specific usage and running example code, please see the :ref:`Python LidarScan examples` or the :ref:`C++ LidarScan examples`. The ``LidarScan`` class (:cpp:class:`ouster::LidarScan`, :py:class:`.LidarScan`) batches lidar @@ -134,6 +134,33 @@ Running the above code on a sample ``LidarScan`` will give you output that looks Now that we know how to create the ``LidarScan`` and access its contents, let's see what we can do with it! +Adding custom fields to a LidarScan +=================================== + +Beginning in Ouster SDK 0.12.0, it's possible to add custom fields to a ``LidarScan``. This is especially useful if you +want to add custom data to an OSF file for visualization purposes. Like the standard fields normally found in a +``LidarScan``, custom fields also make use of ``Eigen::Array`` and ``numpy.ndarray`` in C++ and Python, respectively. + +.. tabs:: + + .. tab:: python + + .. literalinclude:: /../python/src/ouster/sdk/examples/lidar_scan.py + :language: python + :start-after: [doc-stag-python-scan-add-field] + :end-before: [doc-etag-python-scan-add-field] + :dedent: + + .. tab:: C++ + + .. literalinclude:: /../examples/lidar_scan_example.cpp + :language: cpp + :start-after: [doc-stag-cpp-scan-add-field] + :end-before: [doc-etag-cpp-scan-add-field] + :dedent: + +Note - fields can also be removed using the ``del_field`` method. This can be useful for removing unneeded data, thereby +saving space when saving scans to an OSF file. Projecting into Cartesian Coordinates ===================================== @@ -210,7 +237,7 @@ of timestamp. For this natural 2D image, we *destagger* the relevant field of th :dedent: The above code gives the scene below (see the long strip at the bottom). We've magnified two patches -for better visiblity atop. +for better visibility atop. .. figure:: /images/lidar_scan_destaggered.png :align: center @@ -232,5 +259,5 @@ both sampling, used in :ref:`ex-visualization-with-matplotlib`, and streaming, u Under the hood, this class batches packets into ``LidarScans``. C++ users must batch packets themselves using the :cpp:class:`ouster::ScanBatcher` class. To get a feel for how to use it, we recommend -reading `this example on Github -`_. +reading `this example on GitHub +`_. diff --git a/docs/reference/osf.rst b/docs/reference/osf.rst index 85aa8f57..24b369c6 100644 --- a/docs/reference/osf.rst +++ b/docs/reference/osf.rst @@ -33,7 +33,7 @@ Getting example OSF files ------------------------- The :doc:`sample data page <../sample-data>` has instructions for obtaining sample datasets. Sample -datasets are availble in OSF format in addition to pcap. +datasets are available in OSF format in addition to pcap. OSF format details ------------------ diff --git a/docs/sample-data.rst b/docs/sample-data.rst index 4caf64e4..f88d4fc5 100644 --- a/docs/sample-data.rst +++ b/docs/sample-data.rst @@ -39,7 +39,7 @@ Visualize It! If you've installed the ``ouster-sdk`` (see :ref:`Python Installation `) then you're all set to visualize with:: - $ ouster-cli source $SAMPLE_DATA_PCAP_PATH [--meta $SAMPLE_DATA_JSON_PATH] viz + $ ouster-cli source [--meta $SAMPLE_DATA_JSON_PATH] $SAMPLE_DATA_PCAP_PATH viz You should get a view similar to: diff --git a/docs/versions.json b/docs/versions.json index 2437b4c2..71d34b94 100644 --- a/docs/versions.json +++ b/docs/versions.json @@ -1,4 +1,11 @@ [ + { + "version": "0.12.0", + "tags": { + "sdkx": "681b7ffa7ac83a8b361fa68c9ae599cc409c46b6", + "sdk": "389f7a8e4f94a5a5956cfa536d68165ebd71b7e9" + } + }, { "version": "0.11.1", "tags": { diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 925019bb..ee256351 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -1,12 +1,12 @@ add_executable(client_example client_example.cpp) target_link_libraries(client_example PRIVATE OusterSDK::ouster_client) +add_executable(client_packet_example client_packet_example.cpp) +target_link_libraries(client_packet_example PRIVATE OusterSDK::ouster_client) + add_executable(async_client_example async_client_example.cpp) target_link_libraries(async_client_example PRIVATE OusterSDK::ouster_client) -add_executable(mtp_client_example mtp_client_example.cpp) -target_link_libraries(mtp_client_example PRIVATE OusterSDK::ouster_client) - add_executable(config_example config_example.cpp) target_link_libraries(config_example PRIVATE OusterSDK::ouster_client) @@ -33,6 +33,9 @@ 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) + + add_executable(viz_events_example viz_events_example.cpp) + target_link_libraries(viz_events_example PRIVATE OusterSDK::ouster_client OusterSDK::ouster_viz) else() message(STATUS "No ouster_viz library available; skipping examples") endif() diff --git a/examples/async_client_example.cpp b/examples/async_client_example.cpp index e3faa802..340f2124 100644 --- a/examples/async_client_example.cpp +++ b/examples/async_client_example.cpp @@ -15,12 +15,12 @@ #include "ouster/client.h" #include "ouster/impl/build.h" #include "ouster/lidar_scan.h" +#include "ouster/sensor_client.h" #include "ouster/types.h" using namespace ouster; const size_t N_SCANS = 5; -const size_t UDP_BUF_SIZE = 65536; void FATAL(const char* msg) { std::cerr << msg << std::endl; @@ -69,12 +69,14 @@ int main(int argc, char* argv[]) { * hostname/ip. */ const std::string sensor_hostname = argv[1]; - const std::string data_destination = (argc == 3) ? argv[2] : ""; + const std::string data_destination = (argc == 3) ? argv[2] : "@auto"; std::cerr << "Connecting to \"" << sensor_hostname << "\"...\n"; - auto handle = sensor::init_client(sensor_hostname, data_destination); - if (!handle) FATAL("Failed to connect"); + ouster::sensor::sensor_config config; + config.udp_dest = data_destination; + sensor::SensorClient client( + {ouster::sensor::Sensor(sensor_hostname, config)}); std::cerr << "Connection to sensor succeeded" << std::endl; /* @@ -83,10 +85,9 @@ int main(int argc, char* argv[]) { * accurate point clouds. */ std::cerr << "Gathering metadata..." << std::endl; - auto metadata = sensor::get_metadata(*handle); - // Raw metadata can be parsed into a `sensor_info` struct - sensor::sensor_info info(metadata); + // You can access the retrieved metadata from the SensorClient class + sensor::sensor_info info = client.get_sensor_info()[0]; size_t w = info.format.columns_per_frame; size_t h = info.format.pixels_per_column; @@ -101,17 +102,18 @@ int main(int argc, char* argv[]) { << column_window.second << "]" << std::endl; // A LidarScan holds lidar data for an entire rotation of the device - LidarScan scan{w, h, info.format.udp_profile_lidar}; + LidarScan scan{info}; // pre-compute a table for efficiently calculating point clouds from // range - XYZLut lut = ouster::make_xyz_lut(info); + // the second argument specifies if sensor extrinsics should be applied to + // the output point cloud + XYZLut lut = ouster::make_xyz_lut(info, true); // A an array of points to hold the projected representation of the scan LidarScan::Points cloud; // A ScanBatcher can be used to batch packets into scans - sensor::packet_format pf = sensor::get_format(info); - ScanBatcher batch_to_scan(info.format.columns_per_frame, pf); + ScanBatcher batch_to_scan(info); /* * The network client provides some convenience wrappers around socket APIs @@ -121,8 +123,8 @@ int main(int argc, char* argv[]) { */ // Place to store raw packets as they pass between threads - ouster::sensor::LidarPacket lidar_packet(pf.lidar_packet_size); - ouster::sensor::ImuPacket imu_packet(pf.imu_packet_size); + ouster::sensor::LidarPacket lidar_packet; + ouster::sensor::ImuPacket imu_packet; /* In this example we spin two threads one to receive lidar packets while the @@ -143,36 +145,31 @@ int main(int argc, char* argv[]) { std::thread packet_receiving_thread([&]() { while (n_scans < N_SCANS) { // wait until sensor data is available - sensor::client_state st = sensor::poll_client(*handle); + auto ev = client.get_packet(lidar_packet, imu_packet, 1.0); // check for timeout - if (st == sensor::TIMEOUT) FATAL("Client has timed out"); + if (ev.type == ouster::sensor::ClientEvent::PollTimeout) + FATAL("Client has timed out"); - if (st & sensor::EXIT) FATAL("Exit was requested"); - - // check for error status - if (st & sensor::CLIENT_ERROR) - FATAL("Sensor client returned error state!"); + // check for error state + if (ev.type == ouster::sensor::ClientEvent::Error) + FATAL("Exit was requested"); // check for lidar data, read a packet and add it to the current // batch - if (st & sensor::LIDAR_DATA) { + if (ev.type == ouster::sensor::ClientEvent::LidarPacket) { std::unique_lock lock(mtx); receiving_cv.wait( lock, [&packet_processed] { return packet_processed; }); - if (!sensor::read_lidar_packet(*handle, lidar_packet)) { - FATAL("Failed to read a packet of the expected size!"); - } packet_processed = false; processing_cv.notify_one(); } // check if IMU data is available (but don't do anything with it) - if (st & sensor::IMU_DATA) { + if (ev.type == ouster::sensor::ClientEvent::ImuPacket) { std::unique_lock lock(mtx); receiving_cv.wait( lock, [&packet_processed] { return packet_processed; }); - sensor::read_imu_packet(*handle, imu_packet); // we are not going to processor imu data // so we will keep packet_processed set to true } diff --git a/examples/client_example.cpp b/examples/client_example.cpp index 0c4e4cf4..176923c6 100644 --- a/examples/client_example.cpp +++ b/examples/client_example.cpp @@ -12,26 +12,20 @@ #include "ouster/client.h" #include "ouster/impl/build.h" #include "ouster/lidar_scan.h" +#include "ouster/sensor_scan_source.h" #include "ouster/types.h" using namespace ouster; const size_t N_SCANS = 5; -void FATAL(const char* msg) { - std::cerr << msg << std::endl; - std::exit(EXIT_FAILURE); -} - int main(int argc, char* argv[]) { - if (argc != 2 && argc != 3) { - std::cerr - << "Version: " << ouster::SDK_VERSION_FULL << " (" - << ouster::BUILD_SYSTEM << ")" - << "\n\nUsage: client_example []" - "\n\n is optional: leave blank for " - "automatic destination detection" - << std::endl; + if (argc < 2) { + std::cerr << "Version: " << ouster::SDK_VERSION_FULL << " (" + << ouster::BUILD_SYSTEM << ")" + << "\n\nUsage: client_example " + "[]..." + << std::endl; return argc == 1 ? EXIT_SUCCESS : EXIT_FAILURE; } @@ -41,98 +35,64 @@ int main(int argc, char* argv[]) { std::cerr << "Ouster client example " << ouster::SDK_VERSION << std::endl; /* - * The sensor client consists of the network client and a library for - * reading and working with data. + * The SensorScanSource is the high level client for working with Ouster + * sensors. It is used to connect to sensors, configure them and batch + * LidarScans from them. * - * The network client supports reading and writing a limited number of - * configuration parameters and receiving data without working directly with - * the socket APIs. See the `client.h` for more details. The minimum - * required parameters are the sensor hostname/ip and the data destination - * hostname/ip. + * It supports unicast, multicast and multiple sensors on the same ports. */ - const std::string sensor_hostname = argv[1]; - const std::string data_destination = (argc == 3) ? argv[2] : ""; - std::cerr << "Connecting to \"" << sensor_hostname << "\"...\n"; + // Build list of all sensors to connect to + std::vector sensors; - auto handle = sensor::init_client(sensor_hostname, data_destination); - if (!handle) FATAL("Failed to connect"); - std::cerr << "Connection to sensor succeeded" << std::endl; + std::vector count; + for (int a = 1; a < argc; a++) { + const std::string sensor_hostname = argv[a]; - /* - * Configuration and calibration parameters can be queried directly from the - * sensor. These are required for parsing the packet stream and calculating - * accurate point clouds. - */ - std::cerr << "Gathering metadata..." << std::endl; - auto metadata = sensor::get_metadata(*handle, 10); + std::cerr << "Connecting to \"" << sensor_hostname << "\"...\n"; - // Raw metadata can be parsed into a `sensor_info` struct - sensor::sensor_info info(metadata); + ouster::sensor::sensor_config config; + config.udp_dest = "@auto"; // autodetect the UDP destination IP + // config.udp_port_lidar = 0; // If you set any of the ports to 0, an + // ephemeral port is used + ouster::sensor::Sensor s(sensor_hostname, config); + sensors.push_back(s); + count.push_back(0); + } - size_t w = info.format.columns_per_frame; - size_t h = info.format.pixels_per_column; + // Finally create the client. This will configure all the sensors + // and wait for them to initialize. + // After this, the source immediately begins collecting data in the + // background + ouster::sensor::SensorScanSource source(sensors); - ouster::sensor::ColumnWindow column_window = info.format.column_window; + std::cerr << "Connection to sensors succeeded" << std::endl; - auto fw_ver = info.get_version(); + // Now we can print metadata about each sensor since it was collected by the + // source already While we are at it build necessary lookup tables + std::vector luts; + for (const auto& info : source.get_sensor_info()) { + std::cerr << "Sensor " << info.sn << " Information:" << std::endl; - std::cerr << " Firmware version: " << fw_ver.major << "." << fw_ver.minor - << "." << fw_ver.patch; - std::cerr << "\n Serial number: " << info.sn - << "\n Product line: " << info.prod_line - << "\n Scan dimensions: " << w << " x " << h - << "\n Column window: [" << column_window.first << ", " - << column_window.second << "]" << std::endl; + size_t w = info.format.columns_per_frame; + size_t h = info.format.pixels_per_column; - // A LidarScan holds lidar data for an entire rotation of the device - std::vector scans{ - N_SCANS, LidarScan{w, h, info.format.udp_profile_lidar}}; + ouster::sensor::ColumnWindow column_window = info.format.column_window; - // A ScanBatcher can be used to batch packets into scans - sensor::packet_format pf = sensor::get_format(info); - ScanBatcher batch_to_scan(info.format.columns_per_frame, pf); + std::cerr << " Firmware version: " << info.image_rev + << "\n Serial number: " << info.sn + << "\n Product line: " << info.prod_line + << "\n Scan dimensions: " << w << " x " << h + << "\n Column window: [" << column_window.first << ", " + << column_window.second << "]" << std::endl; - /* - * The network client provides some convenience wrappers around socket APIs - * to facilitate reading lidar and IMU data from the network. It is also - * possible to configure the sensor offline and read data directly from a - * UDP socket. - */ - std::cerr << "Capturing points... "; - - // buffer to store raw packet data - auto lidar_packet = sensor::LidarPacket(pf.lidar_packet_size); - auto imu_packet = sensor::ImuPacket(pf.imu_packet_size); - - for (size_t i = 0; i < N_SCANS;) { - // wait until sensor data is available - sensor::client_state st = sensor::poll_client(*handle); - - // check for error status - if (st & sensor::CLIENT_ERROR) - FATAL("Sensor client returned error state!"); - - // check for lidar data, read a packet and add it to the current batch - if (st & sensor::LIDAR_DATA) { - if (!sensor::read_lidar_packet(*handle, lidar_packet)) { - FATAL("Failed to read a packet of the expected size!"); - } - - // batcher will return "true" when the current scan is complete - if (batch_to_scan(lidar_packet, scans[i])) { - // retry until we receive a full set of valid measurements - // (accounting for azimuth_window settings if any) - if (scans[i].complete(info.format.column_window)) i++; - } - } - - // check if IMU data is available (but don't do anything with it) - if (st & sensor::IMU_DATA) { - sensor::read_imu_packet(*handle, imu_packet); - } + // Pre-compute a table for efficiently calculating point clouds from + // range + luts.push_back(ouster::make_xyz_lut( + info, true /* if extrinsics should be used or not */)); } - std::cerr << "ok" << std::endl; + + std::cerr << "Capturing scans... "; /* * The example code includes functions for efficiently and accurately @@ -141,16 +101,22 @@ int main(int argc, char* argv[]) { * * [0] http://eigen.tuxfamily.org */ - std::cerr << "Computing point clouds... " << std::endl; - // pre-compute a table for efficiently calculating point clouds from - // range - XYZLut lut = ouster::make_xyz_lut(info); - std::vector clouds; + int file_ind = 0; + std::string file_base{"cloud_"}; + // Loop until we get at least the desired number of scans from each sensor + while (true) { + std::pair> result = source.get_scan(); - for (const LidarScan& scan : scans) { - // compute a point cloud using the lookup table - clouds.push_back(ouster::cartesian(scan, lut)); + auto& scan = *result.second; + auto index = result.first; + + // grab scans until we get N from each sensor + if (!result.second) continue; + + // Now process the cloud and save it + // First compute a point cloud using the lookup table + auto cloud = ouster::cartesian(scan, luts[index]); // channel fields can be queried as well auto n_valid_first_returns = @@ -172,19 +138,8 @@ int main(int argc, char* argv[]) { << n_valid_first_returns << " valid first returns at " << ts_ms.count() << " ms" << std::endl; } - } - - /* - * Write output to CSV files. The output can be viewed in a point cloud - * viewer like CloudCompare: - * - * [0] https://github.com/cloudcompare/cloudcompare - */ - std::cerr << "Writing files... " << std::endl; - int file_ind = 0; - std::string file_base{"cloud_"}; - for (const LidarScan::Points& cloud : clouds) { + // Finally save the scan std::string filename = file_base + std::to_string(file_ind++) + ".csv"; std::ofstream out; out.open(filename); @@ -199,7 +154,17 @@ int main(int argc, char* argv[]) { out.close(); std::cerr << " Wrote " << filename << std::endl; + + // Increment our count of that scan and check if we got all 5 + count[index]++; + bool all = true; + for (size_t p = 0; p < count.size(); p++) { + if (count[p] != N_SCANS) all = false; + } + if (all) break; } + std::cerr << "Done" << std::endl; + return EXIT_SUCCESS; } diff --git a/examples/helpers.cpp b/examples/helpers.cpp index 45fdffde..08c4406b 100644 --- a/examples/helpers.cpp +++ b/examples/helpers.cpp @@ -23,7 +23,7 @@ void get_complete_scan( int first_frame_id = 0; auto pf = get_format(info); - ouster::ScanBatcher batch_to_scan(info.format.columns_per_frame, pf); + ouster::ScanBatcher batch_to_scan(info); // Buffer to store raw packet data ouster::sensor::LidarPacket packet(pf.lidar_packet_size); diff --git a/examples/lidar_scan_example.cpp b/examples/lidar_scan_example.cpp index 18287e9c..66c67b17 100644 --- a/examples/lidar_scan_example.cpp +++ b/examples/lidar_scan_example.cpp @@ -144,8 +144,21 @@ int main(int argc, char* argv[]) { std::cerr << std::endl; }; + std::cerr << "\nLet's create a scan with a custom field." << std::endl; + //![doc-stag-cpp-scan-add-field] + auto custom_scan = ouster::LidarScan(info); + custom_scan.add_field("my-custom-field", + ouster::fd_array(info.h(), info.w())); + + // Custom fields + custom_scan.field("my-custom-field") = 1; // set all pixels + custom_scan.field("my-custom-field").block(10, 10, 20, 20) = + 255; // set a block of pixels + //![doc-etag-cpp-scan-add-field] + print_el(legacy_scan, std::string("Legacy Scan")); print_el(profile_scan, std::string("Profile Scan")); print_el(dual_returns_scan, std::string("Dual Returns Scan")); print_el(reduced_fields_scan, std::string("Reduced fields Scan")); + print_el(custom_scan, std::string("Custom fields Scan")); } diff --git a/examples/mtp_client_example.cpp b/examples/mtp_client_example.cpp deleted file mode 100644 index 3018eb0a..00000000 --- a/examples/mtp_client_example.cpp +++ /dev/null @@ -1,84 +0,0 @@ -/** - * Copyright (c) 2023, Ouster, Inc. - * All rights reserved. - */ - -#include - -#include "ouster/client.h" - -using namespace ouster; - -const size_t UDP_BUF_SIZE = 65536; - -int main(int argc, char* argv[]) { - if (argc != 3) { - std::cerr << "\n\nUsage: " - << std::endl; - - return argc == 1 ? EXIT_SUCCESS : EXIT_FAILURE; - } - - const std::string sensor_hostname = argv[1]; - - bool main = false; - - if (std::string(argv[2]) == "main") { - main = true; - } else if (std::string(argv[2]) == "secondary") { - main = false; - } else { - std::cerr << "Invalid second argument: " << argv[2] - << " only values of main or secondary are valid" << std::endl; - return EXIT_FAILURE; - } - - sensor::sensor_config config; - if (!sensor::get_config(sensor_hostname, config)) { - std::cerr << "Failed to get sensor config" << std::endl; - return EXIT_FAILURE; - } - config.udp_dest = "239.201.201.201"; - if (sensor::in_multicast(config.udp_dest.value())) { - std::cerr << "In multicast" << std::endl; - } else { - std::cerr << "Not a multicast address" << std::endl; - return -1; - } - - std::shared_ptr cli = - sensor::mtp_init_client(sensor_hostname, config, "", main); - - if (!cli) { - std::cerr << "Failed to initialize sensor" << std::endl; - return EXIT_FAILURE; - } - - auto metadata = sensor::get_metadata(*cli); - sensor::sensor_info info(metadata); - sensor::packet_format pf = sensor::get_format(info); - auto packet_buf = std::make_unique(UDP_BUF_SIZE); - - bool done = false; - while (!done) { - sensor::client_state state = sensor::poll_client(*cli); - if (state == sensor::EXIT) { - std::cerr << "caught signal, exiting" << std::endl; - done = true; - } - if (state == sensor::TIMEOUT) { - std::cerr << "Timed out" << std::endl; - continue; - } - if (state & sensor::LIDAR_DATA) { - if (sensor::read_lidar_packet(*cli, packet_buf.get(), pf)) { - std::cerr << "Read Lidar Packet" << std::endl; - } - } - if (state & sensor::IMU_DATA) { - if (sensor::read_imu_packet(*cli, packet_buf.get(), pf)) { - std::cerr << "Read IMU packet" << std::endl; - } - } - } -} diff --git a/examples/osf_writer_example.cpp b/examples/osf_writer_example.cpp index d5f62c50..02b4d883 100644 --- a/examples/osf_writer_example.cpp +++ b/examples/osf_writer_example.cpp @@ -37,4 +37,4 @@ int main(int argc, char* argv[]) { // Write it to file on stream 0 writer.save(0, scan); } -//! [doc-2tag-osf-write-cpp] +//! [doc-etag-osf-write-cpp] diff --git a/examples/representations_example.cpp b/examples/representations_example.cpp index 571ecce6..fff5f7fa 100644 --- a/examples/representations_example.cpp +++ b/examples/representations_example.cpp @@ -28,7 +28,8 @@ img_t get_x_in_image_form(const LidarScan& scan, bool destaggered, const size_t h = info.format.pixels_per_column; // Get the XYZ in ouster::Points (n x 3 Eigen array) form - XYZLut lut = make_xyz_lut(info); + // Do not apply extrinsics in this case (the false) + XYZLut lut = make_xyz_lut(info, false); auto cloud = cartesian(scan.field(sensor::ChanField::RANGE), lut); // Access x and reshape as needed @@ -61,7 +62,7 @@ int main(int argc, char* argv[]) { size_t w = info.format.columns_per_frame; size_t h = info.format.pixels_per_column; - auto scan = LidarScan(w, h, info.format.udp_profile_lidar); + auto scan = LidarScan(info); std::cerr << "Reading in scan from pcap..." << std::endl; get_complete_scan(handle, scan, info); @@ -69,7 +70,7 @@ int main(int argc, char* argv[]) { // 1. Getting XYZ std::cerr << "1. Calculating 3d Points... " << std::endl; //! [doc-stag-cpp-xyz] - XYZLut lut = make_xyz_lut(info); + XYZLut lut = make_xyz_lut(info, true); auto range = scan.field(sensor::ChanField::RANGE); auto cloud = cartesian(range, lut); //! [doc-etag-cpp-xyz] diff --git a/ouster_client/CMakeLists.txt b/ouster_client/CMakeLists.txt index 72ac16e6..304cceae 100644 --- a/ouster_client/CMakeLists.txt +++ b/ouster_client/CMakeLists.txt @@ -7,9 +7,9 @@ include(Coverage) # ==== Libraries ==== add_library(ouster_client src/client.cpp src/types.cpp src/sensor_info.cpp src/netcompat.cpp src/lidar_scan.cpp - src/image_processing.cpp src/udp_packet_source.cpp src/parsing.cpp - src/sensor_http.cpp src/sensor_http_imp.cpp src/sensor_tcp_imp.cpp src/logging.cpp - src/field.cpp src/profile_extension.cpp src/util.cpp) + src/image_processing.cpp src/parsing.cpp src/sensor_client.cpp + src/sensor_http.cpp src/sensor_http_imp.cpp src/sensor_scan_source.cpp + src/sensor_tcp_imp.cpp src/logging.cpp src/field.cpp src/profile_extension.cpp src/util.cpp src/metadata.cpp) target_link_libraries(ouster_client PUBLIC Eigen3::Eigen @@ -23,23 +23,21 @@ CodeCoverageFunctionality(ouster_client) add_library(OusterSDK::ouster_client ALIAS ouster_client) -# If ouster_client is built as >=c++17, the nonstd::optional backport -# will just be an alias for std::optional. In that case, client code -# must also build as c++17 to use the same implementation of optional -get_target_property(OUSTER_CLIENT_CXX_STANDARD ouster_client CXX_STANDARD) -if(OUSTER_CLIENT_CXX_STANDARD GREATER_EQUAL 17) - target_compile_features(ouster_client INTERFACE cxx_std_17) -endif() - if(WIN32) target_link_libraries(ouster_client PUBLIC ws2_32) endif() -target_include_directories(ouster_client PUBLIC - $ - $) -target_include_directories(ouster_client SYSTEM PUBLIC - $ - $) + +target_include_directories(ouster_client + PUBLIC + $ + $) + +target_include_directories(ouster_client SYSTEM + PUBLIC + $ + $ + PRIVATE + $) # ==== Install ==== install(TARGETS ouster_client diff --git a/ouster_client/include/optional-lite/nonstd/optional.hpp b/ouster_client/include/optional-lite/nonstd/optional.hpp index aa54e936..ece768c0 100644 --- a/ouster_client/include/optional-lite/nonstd/optional.hpp +++ b/ouster_client/include/optional-lite/nonstd/optional.hpp @@ -41,7 +41,7 @@ // optional selection and configuration: #if !defined( optional_CONFIG_SELECT_OPTIONAL ) -# define optional_CONFIG_SELECT_OPTIONAL ( optional_HAVE_STD_OPTIONAL ? optional_OPTIONAL_STD : optional_OPTIONAL_NONSTD ) +# define optional_CONFIG_SELECT_OPTIONAL optional_OPTIONAL_NONSTD #endif // Control presence of exception handling (try and auto discover): diff --git a/ouster_client/include/ouster/field.h b/ouster_client/include/ouster/field.h index e579965c..3cf9a121 100644 --- a/ouster_client/include/ouster/field.h +++ b/ouster_client/include/ouster/field.h @@ -75,9 +75,11 @@ struct FieldDescriptor { size_t type; /** - * size in bytes of the described field + * Calculates the size in bytes of the described field + * + * @return type size in bytes */ - size_t bytes; + size_t bytes() const; // TODO: ideally we need something like llvm::SmallVector here -- Tim T. @@ -91,6 +93,12 @@ struct FieldDescriptor { */ std::vector strides; + /** + * size of the underlying type, in bytes + * + */ + size_t element_size; + /** * Get type hash * @@ -111,13 +119,6 @@ struct FieldDescriptor { */ size_t size() const; - /** - * Get size of the underlying type, in bytes - * - * @return type size in bytes - */ - int element_size() const; - /** * Get type tag, if can be translated to ChanFieldType, otherwise * returns ChanFieldType::UNREGISTERED @@ -127,19 +128,21 @@ struct FieldDescriptor { sensor::ChanFieldType tag() const; bool operator==(const FieldDescriptor& other) const noexcept { - return type == other.type && bytes == other.bytes && - shape == other.shape && strides == other.strides; + return type == other.type && shape == other.shape && + strides == other.strides && element_size == other.element_size; } /** * Swaps descriptors + * + * @param[in,out] other Handle to swapped FieldDescriptor. */ void swap(FieldDescriptor& other); /** * Check if the type is eligible for conversion * - * @return true if eligible, otherwise false + * @return true if eligible, otherwise false. */ template bool eligible_type() const { @@ -154,14 +157,15 @@ struct FieldDescriptor { /** * Check if descriptor types are compatible * - * @return true if compatible, otherwise false + * @param[in] other A constant of type FieldDescriptor. + * @return true if compatible, otherwise false. */ bool is_type_compatible(const FieldDescriptor& other) const noexcept; /** * Return number of dimensions of the described field * - * @return number of dimensions + * @return number of dimensions. */ size_t ndim() const noexcept; @@ -170,23 +174,23 @@ struct FieldDescriptor { /** * Get a field descriptor for a chunk of typed memory * - * useful when storing arbitrary sized structs + * useful when storing arbitrary sized structs. * - * @tparam T type of memory to be stored; this gets used by safety checks - * @param[in] bytes number of bytes in memory + * @tparam T Type of memory to be stored; this gets used by safety checks. + * @param[in] bytes Number of bytes in memory. * * @return FieldDescriptor */ template static FieldDescriptor memory(size_t bytes) { - return {type_hash(), bytes, {}, {}}; + return {type_hash(), {}, {}, bytes}; } /** * Get a field descriptor for an array * - * @tparam T array type - * @param[in] shape vector of array dimensions + * @tparam T Array type + * @param[in] shape Shape vector of array dimensions. * * @return FieldDescriptor */ @@ -195,23 +199,15 @@ struct FieldDescriptor { static_assert(!std::is_same::value, "FieldDescriptor::array is disallowed, use " "FieldDescriptor::memory() instead"); - size_t total = std::accumulate(shape.begin(), shape.end(), size_t{1}, - std::multiplies{}); - size_t bytes = sizeof(T) * total; - - if (!bytes) - throw std::invalid_argument( - "failed creating array descriptor, one " - "of dimensions is zero"); - - return {type_hash(), bytes, shape, impl::calculate_strides(shape)}; + return {type_hash(), shape, impl::calculate_strides(shape), + sizeof(T)}; } /** * Get a field descriptor for an array * - * @param[in] tag tag of array type - * @param[in] shape vector of array dimensions + * @param[in] tag Tag of array type. + * @param[in] shape Vector of array dimensions. * * @return FieldDescriptor */ @@ -248,8 +244,8 @@ struct FieldDescriptor { /** * Parameter pack shorthand for FieldDescriptor::array - * - * @return FieldDescriptor + * @param[in] args Variadic arguments that are forwarded to the function. + * @return FieldDescriptor array */ template auto fd_array(Args&&... args) -> FieldDescriptor { @@ -278,8 +274,8 @@ class FieldView { /** * Initialize FieldView with a pointer and a descriptor * - * @param[in] ptr memory pointer - * @param[in] desc field descriptor + * @param[in] ptr Memory pointer. + * @param[in] desc Field descriptor. */ FieldView(void* ptr, const FieldDescriptor& desc); @@ -484,6 +480,8 @@ class FieldView { * subview = arr[:,10,20] * \endcode * + * @throws std::invalid_argument If FieldView ran out of dimensions to + * subview or if FieldView cannot subview over the shape limits * @param[in] idx parameter pack of int indices or idx_range (keep()) * * @return FieldView subview @@ -506,16 +504,13 @@ class FieldView { char* ptr = reinterpret_cast(ptr_) + impl::strided_index(desc().strides, impl::range_or_idx(idx)...) * - desc().element_size(); + desc().element_size; auto new_desc = FieldDescriptor{}; new_desc.type = desc().type; new_desc.shape = impl::range_args_reshape(desc().shape, idx...); new_desc.strides = impl::range_args_restride(desc().strides, idx...); - new_desc.bytes = - std::accumulate(new_desc.shape.begin(), new_desc.shape.end(), 1, - std::multiplies{}) * - desc().element_size(); + new_desc.element_size = desc().element_size; return {reinterpret_cast(ptr), new_desc}; } @@ -545,7 +540,7 @@ class FieldView { auto new_desc = FieldDescriptor{}; new_desc.type = desc().type; - new_desc.bytes = desc().bytes; + new_desc.element_size = desc().element_size; new_desc.shape = std::vector{static_cast(dims)...}; new_desc.strides = impl::calculate_strides(new_desc.shape); return {const_cast(ptr_), new_desc}; diff --git a/ouster_client/include/ouster/impl/lidar_scan_impl.h b/ouster_client/include/ouster/impl/lidar_scan_impl.h index 08739bce..56dbabdf 100644 --- a/ouster_client/include/ouster/impl/lidar_scan_impl.h +++ b/ouster_client/include/ouster/impl/lidar_scan_impl.h @@ -327,6 +327,20 @@ void scan_to_packets(const LidarScan& ls, std::memset(packet.buf.data(), 0, packet.buf.size()); packet.host_timestamp = ls.packet_timestamp()[p_id]; + // Set alert flags, which may vary from packet to packet + pw.set_alert_flags(lidar_buf, ls.alert_flags()[p_id]); + + // Set shot-limiting and shutdown fields, which should be the same for + // all packets in the scan + pw.set_shutdown(lidar_buf, ls.thermal_shutdown()); + pw.set_shot_limiting(lidar_buf, ls.shot_limiting()); + pw.set_shutdown_countdown(lidar_buf, ls.shutdown_countdown); + pw.set_shot_limiting_countdown(lidar_buf, ls.shot_limiting_countdown); + + // Set other scan-level attributes + + // TODO(dguridi): add an official PacketType enum in types.h + pw.set_packet_type(lidar_buf, 0x1); pw.set_frame_id(lidar_buf, frame_id); pw.set_init_id(lidar_buf, init_id); pw.set_prod_sn(lidar_buf, prod_sn); @@ -354,9 +368,10 @@ void scan_to_packets(const LidarScan& ls, packet); } else if (pw.udp_profile_lidar != ouster::sensor::UDPProfileLidar::PROFILE_LIDAR_LEGACY) { - uint32_t* ptr = reinterpret_cast(packet.buf.data() + - packet.buf.size() - 4); - *ptr = 0xdeadbeef; // eUDP packets end in 0xdeadbeef + assert(packet.buf.size() > sizeof(uint64_t)); + uint64_t crc = pw.calculate_crc(packet.buf.data()); + memcpy(packet.buf.data() + packet.buf.size() - sizeof(crc), &crc, + sizeof(crc)); } *iter++ = packet; diff --git a/ouster_client/include/ouster/impl/netcompat.h b/ouster_client/include/ouster/impl/netcompat.h index 3f676f6d..c5d90a06 100644 --- a/ouster_client/include/ouster/impl/netcompat.h +++ b/ouster_client/include/ouster/impl/netcompat.h @@ -42,6 +42,14 @@ #define SOCKET_ERROR -1 +/** + * Windows for some reason globally defines BAUD_9600 and BAUD_115200 + * which causes issues when you try and use those inside of something + * like an enum. Undefine BAUD_9600 and BAUD_115200 coming from windows. + */ +#undef BAUD_9600 +#undef BAUD_115200 + #endif // --------- End Platform Differentiation Block --------- namespace ouster { diff --git a/ouster_client/include/ouster/impl/packet_writer.h b/ouster_client/include/ouster/impl/packet_writer.h index af1cd61f..e64d7b80 100644 --- a/ouster_client/include/ouster/impl/packet_writer.h +++ b/ouster_client/include/ouster/impl/packet_writer.h @@ -15,10 +15,6 @@ namespace impl { * Writing counterpart to packet_format, used for packet generation */ class packet_writer : public packet_format { - template - void set_block_impl(Eigen::Ref> field, const std::string& i, - uint8_t* lidar_buf) const; - public: using packet_format::packet_format; // construct from packet format @@ -28,15 +24,20 @@ class packet_writer : public packet_format { uint8_t* nth_px(int n, uint8_t* col_buf) const; uint8_t* footer(uint8_t* lidar_buf) const; + void set_alert_flags(uint8_t* lidar_buf, uint8_t alert_flags) const; void set_col_status(uint8_t* col_buf, uint32_t status) const; void set_col_timestamp(uint8_t* col_buf, uint64_t ts) const; void set_col_measurement_id(uint8_t* col_buf, uint16_t m_id) const; void set_frame_id(uint8_t* lidar_buf, uint32_t frame_id) const; void set_init_id(uint8_t* lidar_buf, uint32_t init_id) const; + void set_packet_type(uint8_t* lidar_buf, uint16_t packet_type) const; void set_prod_sn(uint8_t* lidar_buf, uint64_t sn) const; - - template - void set_px(uint8_t* px_buf, const std::string& i, T value) const; + void set_shot_limiting(uint8_t* lidar_buf, uint8_t status) const; + void set_shot_limiting_countdown(uint8_t* lidar_buf, + uint8_t shot_limiting_countdown) const; + void set_shutdown(uint8_t* lidar_buf, uint8_t status) const; + void set_shutdown_countdown(uint8_t* lidar_buf, + uint8_t shutdown_countdown) const; template void set_block(Eigen::Ref> field, const std::string& i, diff --git a/ouster_client/include/ouster/lidar_scan.h b/ouster_client/include/ouster/lidar_scan.h index 42c14efc..a61d0cd0 100644 --- a/ouster_client/include/ouster/lidar_scan.h +++ b/ouster_client/include/ouster/lidar_scan.h @@ -125,9 +125,20 @@ class LidarScan { Field packet_timestamp_; Field pose_; + /** + * The alert flags field, from eUDP packet headers. + */ + Field alert_flags_; + LidarScan(size_t w, size_t h, LidarScanFieldTypes field_types, size_t columns_per_packet); + /** + * The number of packets used to produce a full scan given the width in + * pixels and the number of columns per packet. + */ + size_t packet_count_{0}; + public: /** * Pointer offsets to deal with strides. @@ -159,6 +170,18 @@ class LidarScan { */ uint64_t frame_status{0}; + /** + * Thermal shutdown counter. Please refer to the firmware documentation for + * more information. + */ + uint8_t shutdown_countdown{0}; + + /** + * Shot limiting counter. Please refer to the firmware documentation for + * more information. + */ + uint8_t shot_limiting_countdown{0}; + /** * The current frame ID. * @@ -182,6 +205,14 @@ class LidarScan { */ LidarScan(size_t w, size_t h); + /** + * Initialize a scan with fields configured as default for the provided + * SensorInfo's configuration + * + * @param[in] sensor_info description of sensor to create scan for + */ + LidarScan(const sensor::sensor_info& sensor_info); + /** * Initialize a scan with the default fields for a particular udp profile. * @@ -255,11 +286,15 @@ class LidarScan { /** * Get frame shot limiting status + * + * @return true if sensor is shot limiting */ sensor::ShotLimitingStatus shot_limiting() const; /** * Get frame thermal shutdown status + * + * @return true if sensor is in thermal shutdown state. */ sensor::ThermalShutdownStatus thermal_shutdown() const; @@ -339,9 +374,9 @@ class LidarScan { * * @throw std::invalid_argument if key duplicates a preexisting field * - * @param[in] type descriptor of the field to add + * @param[in] type Descriptor of the field to add. * - * @return field + * @return field The value of the field added. */ Field& add_field(const FieldType& type); @@ -351,6 +386,8 @@ class LidarScan { * @throw std::invalid_argument if field under key does not exist * * @param[in] name string key of the field to remove + * + * @return field The deleted field. */ Field del_field(const std::string& name); @@ -366,12 +403,14 @@ class LidarScan { /** * Get the FieldType of all fields in the scan * - * @return the type associated with every field in the scan + * @return The type associated with every field in the scan. */ LidarScanFieldTypes field_types() const; /** * Reference to the internal fields map + * + * @return The unordered map of field type and field. */ std::unordered_map& fields(); @@ -393,17 +432,35 @@ class LidarScan { /** * Access the packet timestamp headers (usually host time). * - * @return a view of timestamp as a w-element vector. + * @return a view of timestamp as a vector with w / columns-per-packet + * elements. */ Eigen::Ref> packet_timestamp(); /** * Access the host timestamp headers (usually host time). * - * @return a view of timestamp as a w-element vector. + * @return a view of timestamp as a vector with w / columns-per-packet + * elements. */ Eigen::Ref> packet_timestamp() const; + /** + * Access the packet alert flags headers. + * + * @return a view of timestamp as a vector with w / columns-per-packet + * elements. + */ + Eigen::Ref> alert_flags(); + + /** + * Access the packet alert flags headers. + * + * @return a view of timestamp as a vector with w / columns-per-packet + * elements. + */ + Eigen::Ref> alert_flags() const; + /** * Return the first valid packet timestamp * @@ -411,6 +468,13 @@ class LidarScan { */ uint64_t get_first_valid_packet_timestamp() const; + /** + * Return the first valid column timestamp + * + * @return the first valid column timestamp, 0 if none available + */ + uint64_t get_first_valid_column_timestamp() const; + /** * Access the measurement id headers. * @@ -448,6 +512,13 @@ class LidarScan { */ bool complete(sensor::ColumnWindow window) const; + /** + * Returns the number of lidar packets used to produce this scan. + * + * @return the number of packets + */ + size_t packet_count() const; + friend bool operator==(const LidarScan& a, const LidarScan& b); }; @@ -557,19 +628,17 @@ XYZLut make_xyz_lut(size_t w, size_t h, double range_unit, /** * Convenient overload that uses parameters from the supplied sensor_info. * Projections to XYZ made with this XYZLut will be in the sensor coordinate - * frame defined in the sensor documentation. + * frame defined in the sensor documentation unless use_extrinics is true. + * Then the projections will be in the coordinate frame defined by the provided + * extrinsics in the metadata. * * @param[in] sensor metadata returned from the client. + * @param[in] use_extrinsics if true, applies the ``sensor.extrinsic`` transform + * to the resulting "sensor frame" coordinates * * @return xyz direction and offset vectors for each point in the lidar scan. */ -inline XYZLut make_xyz_lut(const sensor::sensor_info& sensor) { - return make_xyz_lut( - sensor.format.columns_per_frame, sensor.format.pixels_per_column, - sensor::range_unit, sensor.beam_to_lidar_transform, - sensor.lidar_to_sensor_transform, sensor.beam_azimuth_angles, - sensor.beam_altitude_angles); -} +XYZLut make_xyz_lut(const sensor::sensor_info& sensor, bool use_extrinsics); /** \defgroup ouster_client_lidar_scan_cartesian Ouster Client lidar_scan.h * XYZLut related items. @@ -658,21 +727,30 @@ class ScanBatcher { std::vector cache; uint64_t cache_packet_ts; bool cached_packet = false; + int64_t finished_scan_id = -1; + size_t expected_packets; + size_t batched_packets = 0; void _parse_by_col(const uint8_t* packet_buf, LidarScan& ls); void _parse_by_block(const uint8_t* packet_buf, LidarScan& ls); + void finalize_scan(LidarScan& ls, bool raw_headers); public: sensor::packet_format pf; ///< The packet format object used for decoding + // clang-format off /** * Create a batcher given information about the scan and packet format. + * @deprecated Use ScanBatcher::ScanBatcher(const sensor_info&) instead. * * @param[in] w number of columns in the lidar scan. One of 512, 1024, or * 2048. * @param[in] pf expected format of the incoming packets used for parsing. */ + [[deprecated("Use ScanBatcher::ScanBatcher(const sensor_info&) instead. " + "This is planned to be removed in Q4 2024.")]] ScanBatcher(size_t w, const sensor::packet_format& pf); + // clang-format on /** * Create a batcher given information about the scan and packet format. @@ -719,6 +797,78 @@ class ScanBatcher { LidarScan& ls); }; +namespace pose_util { +typedef Eigen::Matrix Points; +typedef Eigen::Matrix Poses; +typedef Eigen::Matrix Pose; + +/** + * This function takes in a set of 3D points and a set of 4x4 pose matrices + * + * @param[out] dewarped An eigen matrix of shape (N, 3) to hold the dewarped + * 3D points, where the same number of points are transformed by each + * corresponding pose matrix. + * @param[in] points A Eigen matrix of shape (N, 3) representing the 3D points. + * Each row corresponds to a point in 3D space. + * @param[in] poses A Eigen matrix of shape (W, 16) representing W 4x4 + * transformation matrices. Each row is a flattened 4x4 pose matrix + */ + +void dewarp(Eigen::Ref dewarped, const Eigen::Ref points, + const Eigen::Ref poses); + +/** + * This function takes in a set of 3D points and a set of 4x4 pose matrices + * + * @param[in] points A Eigen matrix of shape (N, 3) representing the 3D points. + * Each row corresponds to a point in 3D space. + * @param[in] poses A Eigen matrix of shape (W, 16) representing W 4x4 + * transformation matrices. Each row is a flattened 4x4 pose matrix + * + * @return A matrix of shape (N, 3) containing the dewarped 3D points, + * where the same number of points are transformed by each corresponding pose + * matrix. + */ + +Points dewarp(const Eigen::Ref points, + const Eigen::Ref poses); + +/** + * Applies a single 4x4 pose transformation to a set of 3D points. + * + * This function takes in a set of 3D points and applies a single 4x4 + * transformation matrix (Pose) to all points. + * + * @param[out] transformed A matrix of shape (N, 3) containing the transformed + * 3D points, where each point is rotated and translated by the given pose. + * @param[in] points A matrix of shape (N, 3) representing the 3D points. + * Each row corresponds to a point in 3D space. + * @param[in] pose A vector of 16 elements representing a flattened 4x4 + * transformation matrix. + */ + +void transform(Eigen::Ref transformed, + const Eigen::Ref points, + const Eigen::Ref pose); +/** + * Applies a single 4x4 pose transformation to a set of 3D points. + * + * This function takes in a set of 3D points and applies a single 4x4 + * transformation matrix (Pose) to all points. + * + * @param[in] points A matrix of shape (N, 3) representing the 3D points. + * Each row corresponds to a point in 3D space. + * @param[in] pose A vector of 16 elements representing a flattened 4x4 + * transformation matrix. + * + * @return A matrix of shape (N, 3) containing the transformed 3D points, + * where each point is rotated and translated by the given pose. + */ + +Points transform(const Eigen::Ref points, + const Eigen::Ref pose); + +} // namespace pose_util } // 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 06d8131c..4de0e70c 100644 --- a/ouster_client/include/ouster/sensor_http.h +++ b/ouster_client/include/ouster/sensor_http.h @@ -31,6 +31,9 @@ struct UserDataAndPolicy { * An interface to communicate with Ouster sensors via http requests */ class SensorHttp { + ouster::util::version version_; + std::string hostname_; + protected: /** * Constructs an http interface to communicate with the sensor. @@ -43,6 +46,22 @@ class SensorHttp { */ virtual ~SensorHttp() = default; + /** + * Returns the cached sensor FW version retrieved on construction. + * + * @return returns the sensor FW version + */ + inline const ouster::util::version& firmware_version() const { + return version_; + } + + /** + * Returns the hostname for the associated sensor. + * + * @return returns the sensor FW version + */ + inline const std::string& hostname() const { return hostname_; } + /** * Queries the sensor metadata. * @@ -88,6 +107,8 @@ class SensorHttp { * Retrieves the active configuration on the sensor * * @param[in] timeout_sec The timeout for the request in seconds. + * + * @return active configuration parameters set on the sensor */ virtual Json::Value active_config_params(int timeout_sec = 1) const = 0; @@ -95,6 +116,8 @@ class SensorHttp { * Retrieves the staged configuration on the sensor * * @param[in] timeout_sec The timeout for the request in seconds. + * + * @return staged configuration parameters set on the sensor */ virtual Json::Value staged_config_params(int timeout_sec = 1) const = 0; @@ -109,6 +132,8 @@ class SensorHttp { * Retrieves beam intrinsics of the sensor. * * @param[in] timeout_sec The timeout for the request in seconds. + * + * @return beam_intrinsics retrieved from sensor */ virtual Json::Value beam_intrinsics(int timeout_sec = 1) const = 0; @@ -116,6 +141,8 @@ class SensorHttp { * Retrieves imu intrinsics of the sensor. * * @param[in] timeout_sec The timeout for the request in seconds. + * + * @return imu_intrinsics received from sensor */ virtual Json::Value imu_intrinsics(int timeout_sec = 1) const = 0; @@ -123,6 +150,8 @@ class SensorHttp { * Retrieves lidar intrinsics of the sensor. * * @param[in] timeout_sec The timeout for the request in seconds. + * + * @return lidar_intrinsics retrieved from sensor */ virtual Json::Value lidar_intrinsics(int timeout_sec = 1) const = 0; @@ -130,6 +159,8 @@ class SensorHttp { * Retrieves lidar data format. * * @param[in] timeout_sec The timeout for the request in seconds. + * + * @return lidar_data_format received from sensor */ virtual Json::Value lidar_data_format(int timeout_sec = 1) const = 0; @@ -137,6 +168,8 @@ class SensorHttp { * Gets the calibaration status of the sensor. * * @param[in] timeout_sec The timeout for the request in seconds. + * + * @return calibration status received from sensor */ virtual Json::Value calibration_status(int timeout_sec = 1) const = 0; @@ -158,6 +191,8 @@ class SensorHttp { * Gets the user data stored on the sensor. * * @param[in] timeout_sec The timeout for the request in seconds. + * + * @return user data retrieved from sensor */ virtual std::string get_user_data(int timeout_sec = 1) const = 0; @@ -165,6 +200,8 @@ class SensorHttp { * Gets the user data stored on the sensor and the retention policy. * * @param[in] timeout_sec The timeout for the request in seconds. + * + * @return user data and policy setting retrieved from the sensor */ virtual UserDataAndPolicy get_user_data_and_policy( int timeout_sec = 1) const = 0; @@ -194,6 +231,8 @@ class SensorHttp { * @param[in] hostname hostname of the sensor to communicate with. * @param[in] timeout_sec The timeout to use in seconds for the version * request, this argument is optional. + * + * @return firmware version string from sensor */ static std::string firmware_version_string( const std::string& hostname, @@ -205,6 +244,8 @@ class SensorHttp { * @param[in] hostname hostname of the sensor to communicate with. * @param[in] timeout_sec The timeout to use in seconds for the version * request, this argument is optional. + * + * @return parsed firmware version from sensor */ static ouster::util::version firmware_version( const std::string& hostname, @@ -216,6 +257,8 @@ class SensorHttp { * @param[in] hostname hostname of the sensor to communicate with. * @param[in] timeout_sec The timeout to use in seconds for the version * request, this argument is optional. + * + * @return a version specific implementation of the SensorHTTP instance */ static std::unique_ptr create( const std::string& hostname, diff --git a/ouster_client/include/ouster/types.h b/ouster_client/include/ouster/types.h index e6acf137..d112cfb4 100644 --- a/ouster_client/include/ouster/types.h +++ b/ouster_client/include/ouster/types.h @@ -19,6 +19,7 @@ #include #include +#include "json/json.h" #include "nonstd/optional.hpp" #include "version.h" @@ -267,8 +268,7 @@ struct sensor_config { optional udp_port_imu; ///< The destination port for the imu ///< data to be sent to - // TODO: replace ts_mode and ld_mode when timestamp_mode and - // lidar_mode get changed to CapsCase + // TODO: change timestamp_mode and lidar_mode to UpperCamel /** * The timestamp mode for the sensor to use. * Refer to timestamp_mode for more details. @@ -482,14 +482,15 @@ class product_info { protected: /** * Constructor to initialize each of the members off of. - * - * @internal + * @brief Constructor for product_info that takes params (internal only) * * @param[in] product_info_string The full product line string. * @param[in] form_factor The sensor form factor. * @param[in] short_range If the sensor is short range or not. * @param[in] beam_config The beam configuration for the sensor. * @param[in] beam_count The number of beams for a sensor. + * + * @internal */ product_info(std::string product_info_string, std::string form_factor, bool short_range, std::string beam_config, int beam_count); @@ -563,7 +564,9 @@ struct sensor_info { std::string user_data{}; ///< userdata from sensor if available /* Constructor from metadata */ - explicit sensor_info(const std::string& metadata, bool skip_beam_validation = false); + [[deprecated("skip_beam_validation does not do anything anymore")]] + explicit sensor_info(const std::string& metadata, bool skip_beam_validation); + explicit sensor_info(const std::string& metadata); /* Empty constructor -- keep for */ sensor_info(); @@ -584,6 +587,20 @@ struct sensor_info { bool has_fields_equal(const sensor_info& other) const; + /** + * Retrieves the width of a frame + * + * @return width of a frame. + */ + auto w() const -> decltype(format.columns_per_frame); ///< returns the width of a frame (equivalent to format.columns_per_frame) + + /** + * Retrieves the height of a frame + * + * @return height of a frame. + */ + auto h() const -> decltype(format.pixels_per_column); ///< returns the height of a frame (equivalent to format.pixels_per_column) + private: bool was_legacy_ = false; // clang-format on @@ -995,7 +1012,9 @@ firmware_version_from_metadata(const std::string& metadata); // clang-format off typedef const char* cf_type; -/** Tag to identitify a paricular value reported in the sensor channel data +/** + * @namespace ChanField + * Tag to identitify a paricular value reported in the sensor channel data * block. */ namespace ChanField { static constexpr cf_type RANGE = "RANGE"; ///< 1st return range in mm @@ -1050,6 +1069,15 @@ enum ChanFieldType { */ size_t field_type_size(ChanFieldType ft); +/** + * Get the bit mask of the ChanFieldType. + * + * @param[in] ft the field type + * + * @return 64 bit mask + */ +uint64_t field_type_mask(ChanFieldType ft); + /** * Get string representation of a channel field. * @@ -1074,13 +1102,6 @@ std::string to_string(ChanFieldType ft); */ class packet_format { protected: - template - T px_field(const uint8_t* px_buf, const std::string& i) const; - - template - void block_field_impl(Eigen::Ref> field, const std::string& i, - const uint8_t* packet_buf) const; - struct Impl; std::shared_ptr impl_; @@ -1148,6 +1169,15 @@ class packet_format { */ uint64_t prod_sn(const uint8_t* lidar_buf) const; + /** + * Read the alert flags. + * + * @param[in] lidar_buf the lidar buf. + * + * @return the alert flags byte. + */ + uint8_t alert_flags(const uint8_t* lidar_buf) const; + /** * Read the packet thermal shutdown countdown * @@ -1196,11 +1226,18 @@ class packet_format { /** * A const forward iterator over field / type pairs. + * + * @return Iterator pointing to the first element in the field type of + * packets. + * */ FieldIter begin() const; /** * A const forward iterator over field / type pairs. + * + * @return Iterator pointing to the last element in the field type of + * packets. */ FieldIter end() const; @@ -1251,12 +1288,35 @@ class packet_format { * @return column status. */ uint32_t col_status(const uint8_t* col_buf) const; - + /** + * @brief Encodes the column value. + * + * This function encodes the column value. + * + * @deprecated Use col_measurement_id instead. This function will be removed + * in future versions. + * + * @param[in] col_buf A measurement block pointer returned by `nth_col()`. + * + * @return Encoded column value. + */ [[deprecated("Use col_measurement_id instead")]] uint32_t col_encoder( const uint8_t* col_buf) const; ///< @deprecated Encoder count is deprecated as it is redundant ///< with measurement id, barring a multiplication factor which ///< varies by lidar mode. Use col_measurement_id instead + /** + * @brief Retrieves the current frame id + * + * This function returns the frame id of a column + * + * @deprecated Use frame_id instead. This function will be removed + * in future versions. + * + * @param[in] col_buf A measurement block pointer returned by `nth_col()`. + * + * @return The current frame id. + */ [[deprecated("Use frame_id instead")]] uint16_t col_frame_id( const uint8_t* col_buf) const; ///< @deprecated Use frame_id instead @@ -1278,7 +1338,7 @@ class packet_format { /** * Returns maximum available size of parsing block usable with block_field * - * if packet format does not allow for block parsing, returns 0 + * @return if packet format does not allow for block parsing, returns 0 */ int block_parsable() const; @@ -1407,6 +1467,24 @@ class packet_format { * @return number of bits */ int field_bitness(const std::string& f) const; + + /** + * Return the CRC contained in the packet if present + * + * @param[in] lidar_buf the lidar buffer. + * + * @return crc contained in the packet if present + */ + optional crc(const uint8_t* lidar_buf) const; + + /** + * Calculate the CRC for the given packet. + * + * @param[in] lidar_buf the lidar buffer. + * + * @return calculated crc of the packet + */ + uint64_t calculate_crc(const uint8_t* lidar_buf) const; }; /** @defgroup OusterClientTypeGetFormat Get Packet Format functions */ @@ -1457,7 +1535,7 @@ struct Packet { } }; -/* +/** * Reasons for failure of packet validation. */ enum class PacketValidationFailure { @@ -1466,6 +1544,30 @@ enum class PacketValidationFailure { ID = 2 ///< The prod_sn or init_id does not match the metadata }; +/** + * Enum for packet validation types. + */ +enum class PacketValidationType { + LIDAR, ///< Validate as if the buffer was a lidar buffer + IMU, ///< Validate as if the buffer was an imu buffer + GUESS_TYPE ///< Try to guess the type and validate as that +}; + +/** + * Validate a packet buffer against a given type. + * + * @param[in] info The sensor info to try to check the buffer against. + * @param[in] format The packet format to try to check the buffer against. + * @param[in] buf The packet buffer to validate. + * @param[in] buf_size The size of the packet buffer. + * @param[in] type Optional type of packet to try and validate as. + * @return Result of the validation + */ +PacketValidationFailure validate_packet( + const sensor_info& info, const packet_format& format, const uint8_t* buf, + uint64_t buf_size, + PacketValidationType type = PacketValidationType::GUESS_TYPE); + /** * Encapsulate a lidar packet buffer and attributes associated with it. */ diff --git a/ouster_client/include/ouster/udp_packet_source.h b/ouster_client/include/ouster/udp_packet_source.h deleted file mode 100644 index 15e2a17a..00000000 --- a/ouster_client/include/ouster/udp_packet_source.h +++ /dev/null @@ -1,622 +0,0 @@ -/** - * Copyright (c) 2023, Ouster, Inc. - * All rights reserved. - * - * @file - * @brief Wrapper around sensor::client to provide buffering - * - * *Not* a public API. Currently part of the Python bindings implementation. - * - * Maintains a single-producer / single-consumer circular buffer that can be - * populated by a thread without holding the GIL to deal the relatively small - * default OS buffer size and high sensor UDP data rate. Must be thread-safe to - * allow reading data without holding the GIL while other references to the - * client exist. - */ - -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "ouster/client.h" -#include "ouster/impl/ring_buffer.h" -#include "ouster/types.h" - -namespace ouster { -namespace sensor { -namespace impl { - -struct Event { - int source; - client_state state; - - bool operator==(const Event& other) const { - return source == other.source && state == other.state; - } -}; - -} // namespace impl -} // namespace sensor -} // namespace ouster - -namespace std { - -template <> -struct hash { - std::size_t operator()(const ouster::sensor::impl::Event& e) const { - auto h = std::hash{}; - return h(e.source) ^ h(static_cast(e.state)); - } -}; - -} // namespace std - -namespace ouster { -namespace sensor { -namespace impl { - -using EventSet = std::unordered_set; - -/** - * Thread safe event queue. - * - * Safe to use with many-to-many producers/consumers, although some - * considerations apply. - * - * Two main ways of usage are: one queue per multiple consumers, using - * next(events) to have consumers wait on specific events, or using - * multiple queues, one per consumer, as implemented in publisher/subscriber. - */ -class EventQueue { - mutable std::mutex m; - mutable std::condition_variable cv; - std::deque q; - - template - Event _next(Predicate&& p) { - Event e; - { - std::unique_lock lock{m}; - cv.wait(lock, p); - - e = q.front(); - q.pop_front(); - } - cv.notify_all(); - return e; - } - - template - Event _next_timeout(float sec, Predicate&& p) { - Event e; - { - std::unique_lock lock{m}; - using fsec = std::chrono::duration; - bool timeout = !cv.wait_for(lock, fsec{sec}, p); - - if (timeout) return {-1, client_state::TIMEOUT}; - - e = q.front(); - q.pop_front(); - } - cv.notify_all(); - return e; - } - - public: - /** - * Push an event to the back of the queue. - * - * Notifies all threads waiting on the queue. - * - * @param[in] e event - */ - void push(Event e) { - { - std::lock_guard lock{m}; - q.push_back(e); - } - cv.notify_all(); - } - - /** - * Push a [first,last) range of events to the back of the queue. - * - * Notifies all threads waiting on the queue. - * - * @param[in] first iterator to the first event - * @param[in] last past-the-end iterator - */ - template - void push(EventIterT first, EventIterT last) { - { - std::lock_guard lock{m}; - q.insert(q.end(), first, last); - } - cv.notify_all(); - } - - /** - * Push an event to the front of the queue. - * - * Notifies all threads waiting on the queue. - * - * @param[in] e event - */ - void push_priority(Event e) { - { - std::lock_guard lock{m}; - q.push_front(e); - } - cv.notify_all(); - } - - /** - * Pop an event from the front of the queue. - * If the queue is empty, blocks until an event is pushed. - * - * Notifies all other threads waiting on the queue. - * - * @return event - */ - Event pop() { - return _next([this] { return !q.empty(); }); - } - - /** - * Pop an event from the front of the queue. - * If the queue is empty, blocks until an event is pushed or timeout_sec - * seconds have passed - * - * Notifies all other threads waiting on the queue. - * - * @param[in] timeout_sec timeout time in seconds - * @return event or Event{-1, client_state::TIMEOUT} in case of timeout - */ - Event pop(float timeout_sec) { - return _next_timeout(timeout_sec, [this] { return !q.empty(); }); - } - - /** - * Pop an event of the set of events from the front of the queue. - * Blocks until a suitable event is at the front of the queue. - * - * Notifies all other threads waiting on the queue. - * - * @param[in] events subset of events to wait for - * @return event - */ - Event next(const EventSet& events) { - return _next([this, &events] { - return !q.empty() && events.count(q.front()) == 1; - }); - } - - /** - * Pop an event of the set of events from the front of the queue. - * Blocks until a suitable event is at the front of the queue, or - * timeout_sec seconds have passed. - * - * Notifies all other threads waiting on the queue. - * - * @param[in] timeout_sec timeout time in seconds - * @param[in] events subset of events to wait for - * @return event or Event{-1, client_state::TIMEOUT} in case of timeout - */ - Event next(float timeout_sec, const EventSet& events) { - return _next_timeout(timeout_sec, [this, &events] { - return !q.empty() && events.count(q.front()) == 1; - }); - } - - /** - * Flush the queue, immediately returning all elements. - * - * @return queue with all remaining events - */ - // TODO: [[nodiscard]] once we move to cpp17 -- Tim T. - std::deque flush() { - std::lock_guard lock{m}; - std::deque out; - out.swap(q); - return out; - } -}; - -class Publisher { - protected: - EventSet events_; - std::shared_ptr q_; - - public: - /** - * Construct publisher accepting a corresponding set of events - * - * @param[in] events set of events to accept - */ - Publisher(EventSet events) - : events_(std::move(events)), q_(std::make_shared()) {} - - /** - * Construct empty publisher with events to be set later - */ - Publisher() : Publisher(EventSet{}) {} - - /** - * Sets publisher to accept events of type `e` - * - * @param[in] e Event type to accept - */ - void set_accept(Event e) { events_.insert(e); } - - /** - * Checks whether publisher accepts event type. - * - * @param[in] e Event type - * @return true if publisher accepts events of type e - */ - bool accepts(Event e) const { return events_.count(e); } - - /** - * Publish event to the publisher queue. - * - * @param[in] e event - * @param[in] to_front if true, publishes event to the front of the queue - */ - void publish(Event e, bool to_front = false) { - if (accepts(e)) { - if (to_front) { - q_->push_priority(e); - } else { - q_->push(e); - } - } - } - - /** - * Retrieve internal event queue. - * - * Mainly used for constructing subscribers. - * - * @return shared pointer to EventQueue - */ - std::shared_ptr queue() { return q_; } -}; - -class Subscriber { - protected: - std::shared_ptr q_; - std::shared_ptr> rb_; - - static bool _has_packet(Event e) { - return e.state & (client_state::LIDAR_DATA | client_state::IMU_DATA); - } - - public: - Subscriber(std::shared_ptr q, - std::shared_ptr> rb) - : q_(q), rb_(rb) {} - - Subscriber(Subscriber&& other) : q_(), rb_() { - std::swap(q_, other.q_); - std::swap(rb_, other.rb_); - } - - /** - * Pop the next event from the queue. - * - * Blocks thread until event is available. - * - * @return event - */ - Event pop() { return q_->pop(); } - - /** - * Pop the next event from the queue. - * - * Blocks thread until event is available, or timeout is reached. - * - * @param[in] timeout_sec timeout in seconds - * @return event or {-1, client_state::TIMEOUT} - */ - Event pop(float timeout_sec) { return q_->pop(timeout_sec); } - - /** - * Retrieve the packet to the corresponding event. - * - * Packet is guaranteed to stay valid until advance() is called. - * Will throw if the event does not correspond to any packets. - * - * @param[in] e event - * @return packet corresponding to the event - */ - Packet& packet(Event e) { return rb_->front(e); } - const Packet& packet(Event e) const { return rb_->front(e); } - - /** - * Advance the ring buffer read index for the corresponding event. - */ - void advance(Event e) { - if (_has_packet(e)) rb_->pop(e); - } - - /** - * Flush the queue, releasing all corresponding packets from the ring buffer - */ - void flush() { - auto events = q_->flush(); - for (const auto& e : events) { - if (e.state == client_state::EXIT) { - // return exit event back to the queue for later processing - q_->push_priority(e); - } else { - advance(e); - } - } - } -}; - -class Producer { - protected: - std::vector> pubs_; - std::vector> clients_; - std::shared_ptr> rb_; - - std::mutex mtx_; - std::atomic stop_; - - bool _verify() const; - - public: - Producer() - : rb_(std::make_shared>()), stop_(false) {} - - // TODO: move out to client_state extensions of some sort - /* Extra bit flag compatible with client_state to signal buffer overflow. */ - static constexpr int CLIENT_OVERFLOW = 0x10; - - /** - * Add client and allocate buffers for it. - * - * @param[in] cli shared_ptr with initialized client - * @param[in] lidar_buf_size size of the lidar buffer, in packets - * @param[in] lidar_packet_size size of the lidar packet, in bytes - * @param[in] imu_buf_size size of the imu buffer, in packets - * @param[in] imu_packet_size size of the imu packet, in bytes - * @return id of the client used in produced events e.g. - * Event{id, client_state} - */ - int add_client(std::shared_ptr cli, size_t lidar_buf_size, - size_t lidar_packet_size, size_t imu_buf_size, - size_t imu_packet_size); - - /** - * Add client and allocate buffers for it. - * - * Calculates the buffer sizes for the client based on hz rate and provided - * seconds_to_buffer parameter. - * - * @param[in] cli shared_ptr with initialized client - * @param[in] info sensor_info corresponding to the client - * @param[in] seconds_to_buffer amount of seconds worth of buffer allocation - * @return id of the client used in produced events e.g. - * Event{id, client_state} - */ - int add_client(std::shared_ptr cli, const sensor_info& info, - float seconds_to_buffer); - - /** - * Subscribe to a preassembled publisher. - * - * @param[in] pub shared_ptr containing preassembled publisher - * @return shared_ptr with subscriber corresponding to the publisher - */ - std::shared_ptr subscribe(std::shared_ptr pub); - - /** - * Subscribe to a specific set of events. - * - * @param[in] events set of events to subscribe to - * @return shared_ptr with subscriber waiting on the events - */ - std::shared_ptr subscribe(EventSet events); - - /** - * Write data from the network into the circular buffer, reporting events to - * publishers. - * - * Internally verifies that at least some publishers are subscribed to all - * of the events that could be reported by the producer, otherwise returns - * early. - * - * Will return when either shutdown() is called by one of the threads, or - * when CLIENT_ERROR or EXIT are reported by clients. - */ - void run(); - - /** - * Signal the producer to exit and reports EXIT event to all listening - * subscribers, then waits for producer thread to exit before returning. - * - * Additionally, clears all internal publishers and buffers. - */ - void shutdown(); - - /** - * Reports total amount of currently stored packets in internal buffers. - * - * NOTE: this is not a great metric since it does not report specific - * buffers, but the total amount instead. - */ - size_t size() const { return rb_->size(); } - - /** - * Reports total allocated capacity of packets stored in internal buffers. - * - * NOTE: this is not a great metric since it does not report specific - * buffers, but the total amount instead. - */ - size_t capacity() const { return rb_->capacity(); } -}; - -class UDPPacketSource : protected Producer, protected Subscriber { - void _accept_client_events(int id); - - public: - UDPPacketSource(); - - /** - * Add client and allocate buffers for it. - * - * @param[in] cli shared_ptr with initialized client - * @param[in] lidar_buf_size size of the lidar buffer, in packets - * @param[in] lidar_packet_size size of the lidar packet, in bytes - * @param[in] imu_buf_size size of the imu buffer, in packets - * @param[in] imu_packet_size size of the imu packet, in bytes - * @return id of the client used in produced events e.g. - * Event{id, client_state} - */ - void add_client(std::shared_ptr cli, size_t lidar_buf_size, - size_t lidar_packet_size, size_t imu_buf_size, - size_t imu_packet_size); - - /** - * Add client and allocate buffers for it. - * - * Calculates the buffer sizes for the client based on hz rate and provided - * seconds_to_buffer parameter. - * - * @param[in] cli shared_ptr with initialized client - * @param[in] info sensor_info corresponding to the client - * @param[in] seconds_to_buffer amount of seconds worth of buffer allocation - * @return id of the client used in produced events e.g. - * Event{id, client_state} - */ - void add_client(std::shared_ptr cli, const sensor_info& info, - float seconds_to_buffer); - - using Producer::capacity; - using Producer::shutdown; - using Producer::size; - void produce() { Producer::run(); } - - using Subscriber::advance; - using Subscriber::flush; - using Subscriber::packet; - using Subscriber::pop; -}; - -class BufferedUDPSource : protected Producer, protected Subscriber { - BufferedUDPSource(); - - public: - /** - * Listen for sensor data using client - * - * @param[in] client externally created client - * @param[in] lidar_buf_size size of the lidar buffer, in packets - * @param[in] lidar_packet_size size of the lidar packet, in bytes - * @param[in] imu_buf_size size of the imu buffer, in packets - * @param[in] imu_packet_size size of the imu packet, in bytes - */ - BufferedUDPSource(std::shared_ptr client, size_t lidar_buf_size, - size_t lidar_packet_size, size_t imu_buf_size, - size_t imu_packet_size); - - /** - * Listen for sensor data using client - * - * Calculates the buffer sizes for the client based on hz rate and provided - * seconds_to_buffer parameter. - * - * @param[in] client externally created client - * @param[in] info sensor_info corresponding to the client - * @param[in] seconds_to_buffer amount of seconds worth of buffer allocation - */ - BufferedUDPSource(std::shared_ptr client, const sensor_info& info, - float seconds_to_buffer); - - using Producer::capacity; - using Producer::shutdown; - using Producer::size; - void produce() { Producer::run(); } - - using Subscriber::flush; - - /** - * Pop the next client_state from the queue. - * - * Blocks thread until client_state is available. - * - * @return client_state - */ - client_state pop() { return Subscriber::pop().state; } - - /** - * Pop the next client_state from the queue. - * - * Blocks thread until client_state is available, or timeout is reached. - * - * @param[in] timeout_sec timeout in seconds - * @return client_state or client_state::TIMEOUT - */ - client_state pop(float timeout_sec) { - return Subscriber::pop(timeout_sec).state; - } - - /** - * Retrieve the packet to the corresponding client_state. - * - * Packet is guaranteed to stay valid until advance() is called. - * Will throw if the event does not correspond to any packets. - * - * @param[in] st client_state - * @return packet corresponding to st - */ - Packet& packet(client_state st) { return Subscriber::packet({0, st}); } - const Packet& packet(client_state st) const { - return Subscriber::packet({0, st}); - } - - /** - * Advances read in internal buffers - * - * @param[in] st client_state to advance. Does nothing if st is not one of - * LIDAR_DATA or IMU_DATA - */ - void advance(client_state st) { Subscriber::advance({0, st}); } - - /** - * Read next available packet in the buffer. - * - * If client_state returns LIDAR_DATA, submitted lidar packet will be - * populated, similarly if client_state returns IMU_DATA, submitted - * imu packet will be populated instead. - * - * Blocks if the queue is empty for up to `timeout_sec` (zero means wait - * forever). Should only be called by the consumer thread. If reading from - * the network was blocked because the buffer was full, the the - * CLIENT_OVERFLOW flag will be set on the next returned status. - * - * @param[in] lidarp lidar packet to read into - * @param[in] imup imu packet to read into - * @param[in] timeout_sec maximum time to wait for data. - * @return client status, see sensor::poll_client(). - */ - client_state consume(LidarPacket& lidarp, ImuPacket& imup, - float timeout_sec); -}; - -} // namespace impl -} // namespace sensor -} // namespace ouster diff --git a/ouster_client/src/client.cpp b/ouster_client/src/client.cpp index 9e67195d..113dc7ee 100644 --- a/ouster_client/src/client.cpp +++ b/ouster_client/src/client.cpp @@ -41,7 +41,6 @@ struct client { SOCKET lidar_fd; SOCKET imu_fd; std::string hostname; - Json::Value meta; ~client() { impl::socket_close(lidar_fd); impl::socket_close(imu_fd); @@ -51,9 +50,7 @@ struct client { // defined in types.cpp Json::Value config_to_json(const sensor_config& config); -namespace { - -// default udp receive buffer size on windows is very low -- use 256K +// default udp receive buffer size on windows is very low -- use 1MB const int RCVBUF_SIZE = 1024 * 1024; int32_t get_sock_port(SOCKET sock_fd) { @@ -74,182 +71,103 @@ int32_t get_sock_port(SOCKET sock_fd) { return SOCKET_ERROR; } -SOCKET udp_data_socket(int port) { - struct addrinfo hints, *info_start, *ai; - - memset(&hints, 0, sizeof hints); - hints.ai_family = AF_UNSPEC; - hints.ai_socktype = SOCK_DGRAM; - hints.ai_flags = AI_PASSIVE; - - auto port_s = std::to_string(port); - - int ret = getaddrinfo(NULL, port_s.c_str(), &hints, &info_start); - if (ret != 0) { - logger().error("udp getaddrinfo(): {}", gai_strerror(ret)); - return SOCKET_ERROR; - } - if (info_start == NULL) { - logger().error("udp getaddrinfo(): empty result"); - return SOCKET_ERROR; - } - +SOCKET mtp_data_socket(int port, const std::vector& udp_dest_hosts, + const std::string& mtp_dest_host = "") { // try to bind a dual-stack ipv6 socket, but fall back to ipv4 only if that - // fails (when ipv6 is disabled via kernel parameters). Use two passes to - // deal with glibc addrinfo ordering: - // https://sourceware.org/bugzilla/show_bug.cgi?id=9981 + // fails (when ipv6 is disabled via kernel parameters) for (auto preferred_af : {AF_INET6, AF_INET}) { - for (ai = info_start; ai != NULL; ai = ai->ai_next) { - if (ai->ai_family != preferred_af) continue; - - // choose first addrinfo where bind() succeeds - SOCKET sock_fd = - socket(ai->ai_family, ai->ai_socktype, ai->ai_protocol); - if (!impl::socket_valid(sock_fd)) { - logger().warn("udp socket(): {}", impl::socket_get_error()); - continue; - } + // choose first addrinfo where bind() succeeds + SOCKET sock_fd = socket(preferred_af, SOCK_DGRAM, 0); + if (!impl::socket_valid(sock_fd)) { + logger().warn("udp socket(): {}", impl::socket_get_error()); + continue; + } - int off = 0; - if (ai->ai_family == AF_INET6 && - setsockopt(sock_fd, IPPROTO_IPV6, IPV6_V6ONLY, (char*)&off, - sizeof(off))) { - logger().warn("udp setsockopt(): {}", impl::socket_get_error()); - impl::socket_close(sock_fd); - continue; - } + int off = 0; + if (preferred_af == AF_INET6 && + setsockopt(sock_fd, IPPROTO_IPV6, IPV6_V6ONLY, (char*)&off, + sizeof(off))) { + logger().warn("udp setsockopt(): {}", impl::socket_get_error()); + impl::socket_close(sock_fd); + continue; + } - if (impl::socket_set_reuse(sock_fd)) { - logger().warn("udp socket_set_reuse(): {}", - impl::socket_get_error()); - } + if (impl::socket_set_reuse(sock_fd)) { + logger().warn("udp socket_set_reuse(): {}", + impl::socket_get_error()); + } - if (::bind(sock_fd, ai->ai_addr, (socklen_t)ai->ai_addrlen)) { + if (preferred_af == AF_INET6) { + struct sockaddr_in6 address; + memset(&address, 0, sizeof(address)); + address.sin6_family = AF_INET6; + address.sin6_addr = in6addr_any; + address.sin6_port = htons(port); + address.sin6_scope_id = 0; + if (::bind(sock_fd, (struct sockaddr*)&address, sizeof(address))) { logger().warn("udp bind(): {}", impl::socket_get_error()); impl::socket_close(sock_fd); continue; } - - // bind() succeeded; set some options and return - if (impl::socket_set_non_blocking(sock_fd)) { - logger().warn("udp fcntl(): {}", impl::socket_get_error()); - impl::socket_close(sock_fd); - continue; - } - - if (setsockopt(sock_fd, SOL_SOCKET, SO_RCVBUF, (char*)&RCVBUF_SIZE, - sizeof(RCVBUF_SIZE))) { - logger().warn("udp setsockopt(): {}", impl::socket_get_error()); + } else { + struct sockaddr_in address; + memset(&address, 0, sizeof(address)); + address.sin_family = AF_INET; + address.sin_addr.s_addr = INADDR_ANY; + address.sin_port = htons(port); + if (::bind(sock_fd, (struct sockaddr*)&address, sizeof(address))) { + logger().warn("udp bind(): {}", impl::socket_get_error()); impl::socket_close(sock_fd); continue; } - - freeaddrinfo(info_start); - return sock_fd; } - } - - // could not bind() a UDP server socket - freeaddrinfo(info_start); - logger().error("failed to bind udp socket"); - return SOCKET_ERROR; -} - -SOCKET mtp_data_socket(int port, const std::string& udp_dest_host = "", - const std::string& mtp_dest_host = "") { - struct addrinfo hints, *info_start, *ai; - - memset(&hints, 0, sizeof hints); - hints.ai_family = AF_UNSPEC; - hints.ai_socktype = SOCK_DGRAM; - hints.ai_flags = AI_PASSIVE; - - auto port_s = std::to_string(port); - - int ret = getaddrinfo(NULL, port_s.c_str(), &hints, &info_start); - if (ret != 0) { - logger().error("mtp getaddrinfo(): {}", gai_strerror(ret)); - return SOCKET_ERROR; - } - if (info_start == NULL) { - logger().error("mtp getaddrinfo(): empty result"); - return SOCKET_ERROR; - } - for (auto preferred_af : {AF_INET}) { // TODO test with AF_INET6 - for (ai = info_start; ai != NULL; ai = ai->ai_next) { - if (ai->ai_family != preferred_af) continue; - - // choose first addrinfo where bind() succeeds - SOCKET sock_fd = - socket(ai->ai_family, ai->ai_socktype, ai->ai_protocol); - if (!impl::socket_valid(sock_fd)) { - logger().warn("mtp socket(): {}", impl::socket_get_error()); - continue; - } - - if (impl::socket_set_reuse(sock_fd)) { - logger().warn("mtp socket_set_reuse(): {}", - impl::socket_get_error()); - } - - if (::bind(sock_fd, ai->ai_addr, (socklen_t)ai->ai_addrlen)) { - logger().warn("mtp bind(): {}", impl::socket_get_error()); - impl::socket_close(sock_fd); - continue; + // bind() succeeded; join to multicast groups + for (const auto& udp_dest_host : udp_dest_hosts) { + ip_mreq mreq; + mreq.imr_multiaddr.s_addr = inet_addr(udp_dest_host.c_str()); + if (!mtp_dest_host.empty()) { + mreq.imr_interface.s_addr = inet_addr(mtp_dest_host.c_str()); + } else { + mreq.imr_interface.s_addr = htonl(INADDR_ANY); } - // bind() succeeded; join to multicast group on with preferred - // address connect only if addresses are not empty - if (!udp_dest_host.empty()) { - ip_mreq mreq; - mreq.imr_multiaddr.s_addr = inet_addr(udp_dest_host.c_str()); - if (!mtp_dest_host.empty()) { - mreq.imr_interface.s_addr = - inet_addr(mtp_dest_host.c_str()); - } else { - mreq.imr_interface.s_addr = htonl(INADDR_ANY); - } - - if (setsockopt(sock_fd, IPPROTO_IP, IP_ADD_MEMBERSHIP, - (char*)&mreq, sizeof(mreq))) { - logger().warn("mtp setsockopt(): {}", - impl::socket_get_error()); - impl::socket_close(sock_fd); - continue; - } - } - - // join to multicast group succeeded; set some options and return - if (impl::socket_set_non_blocking(sock_fd)) { - logger().warn("mtp fcntl(): {}", impl::socket_get_error()); + if (setsockopt(sock_fd, IPPROTO_IP, IP_ADD_MEMBERSHIP, (char*)&mreq, + sizeof(mreq))) { + logger().warn("udp setsockopt(): {}", impl::socket_get_error()); impl::socket_close(sock_fd); continue; } + } - if (setsockopt(sock_fd, SOL_SOCKET, SO_RCVBUF, (char*)&RCVBUF_SIZE, - sizeof(RCVBUF_SIZE))) { - logger().warn("mtp setsockopt(): {}", impl::socket_get_error()); - impl::socket_close(sock_fd); - continue; - } + // join to multicast group succeeded; set some options and return + if (impl::socket_set_non_blocking(sock_fd)) { + logger().warn("udp fcntl(): {}", impl::socket_get_error()); + impl::socket_close(sock_fd); + continue; + } - freeaddrinfo(info_start); - return sock_fd; + if (setsockopt(sock_fd, SOL_SOCKET, SO_RCVBUF, (char*)&RCVBUF_SIZE, + sizeof(RCVBUF_SIZE))) { + logger().warn("udp setsockopt(): {}", impl::socket_get_error()); + impl::socket_close(sock_fd); + continue; } + + return sock_fd; } // could not bind() a MTP server socket - freeaddrinfo(info_start); - logger().error("failed to bind mtp socket"); + logger().error("failed to bind udp socket"); return SOCKET_ERROR; } -Json::Value collect_metadata(const std::string& hostname, int timeout_sec) { +SOCKET udp_data_socket(int port) { return mtp_data_socket(port, {}); } + +Json::Value collect_metadata(SensorHttp& sensor_http, int timeout_sec) { // 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}; @@ -261,7 +179,7 @@ Json::Value collect_metadata(const std::string& hostname, int timeout_sec) { "A timeout occurred while waiting for the sensor to " "initialize."); } - status = sensor_http->sensor_info(timeout_sec)["status"].asString(); + status = sensor_http.sensor_info(timeout_sec)["status"].asString(); if (status != "INITIALIZING") { break; } @@ -270,7 +188,7 @@ Json::Value collect_metadata(const std::string& hostname, int timeout_sec) { std::string user_data = ""; try { - user_data = sensor_http->get_user_data(timeout_sec); + user_data = sensor_http.get_user_data(timeout_sec); } catch (const std::runtime_error& e) { if (strcmp(e.what(), "user data API not supported on this FW version") != 0) { @@ -279,12 +197,31 @@ Json::Value collect_metadata(const std::string& hostname, int timeout_sec) { } try { - auto metadata = sensor_http->metadata(timeout_sec); + auto metadata = sensor_http.metadata(timeout_sec); metadata["ouster-sdk"]["client_version"] = client_version(); metadata["ouster-sdk"]["output_source"] = "collect_metadata"; metadata["user_data"] = user_data; + // We can't insert this logic into the light init_client since its + // advantage is that it doesn't make network calls but we need it to run + // every time there is a valid connection to the sensor So we insert it + // here + // TODO: remove after release of FW 3.2/3.3 (sufficient warning) + auto fw_version = sensor_http.firmware_version(); + + // only warn for people on the latest FW, as people on older FWs may not + // care + if (fw_version.major >= 3 && + metadata["config_params"]["udp_profile_lidar"] == "LEGACY") { + logger().warn( + "Please note that the Legacy Lidar Profile will be deprecated " + "in the sensor FW soon. If you plan to upgrade your FW, we " + "recommend using the Single Return Profile instead. For users " + "sticking with older FWs, the Ouster SDK will continue to " + "parse " + "the legacy lidar profile."); + } return metadata; } catch (const std::runtime_error& e) { throw std::runtime_error( @@ -294,22 +231,24 @@ Json::Value collect_metadata(const std::string& hostname, int timeout_sec) { } } -} // namespace +bool get_config(SensorHttp& sensor_http, sensor_config& config, + bool active = true, + int timeout_sec = DEFAULT_HTTP_REQUEST_TIMEOUT_SECONDS) { + auto res = sensor_http.get_config_params(active, timeout_sec); + config = parse_config(res); + return true; +} bool get_config(const std::string& hostname, sensor_config& config, bool active, int timeout_sec) { auto sensor_http = SensorHttp::create(hostname, timeout_sec); - auto res = sensor_http->get_config_params(active, timeout_sec); - config = parse_config(res); - return true; + return get_config(*sensor_http, config, active, timeout_sec); } -bool set_config(const std::string& hostname, const sensor_config& config, +bool set_config(SensorHttp& sensor_http, const sensor_config& config, 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(timeout_sec); + auto config_params = sensor_http.active_config_params(timeout_sec); Json::Value config_params_copy = config_params; // set all desired config parameters @@ -338,14 +277,19 @@ bool set_config(const std::string& hostname, const sensor_config& config, } } + // detect and handle @auto udp dest properly + if (config.udp_dest == "@auto") { + config_flags |= CONFIG_UDP_DEST_AUTO; + } + // set automatic udp dest, if flag specified if (config_flags & CONFIG_UDP_DEST_AUTO) { - if (config.udp_dest) + if (config.udp_dest && config.udp_dest != "@auto") throw std::invalid_argument( "UDP_DEST_AUTO flag set but provided config has udp_dest"); - sensor_http->set_udp_dest_auto(timeout_sec); + sensor_http.set_udp_dest_auto(timeout_sec); - auto staged = sensor_http->staged_config_params(timeout_sec); + auto staged = sensor_http.staged_config_params(timeout_sec); // now we set config_params according to the staged udp_dest from the // sensor @@ -367,24 +311,32 @@ bool set_config(const std::string& hostname, const sensor_config& config, // send full string -- depends on older FWs not rejecting a blob even // when it contains unknown keys auto config_params_str = Json::writeString(builder, config_params); - sensor_http->set_config_param(".", config_params_str, timeout_sec); + sensor_http.set_config_param(".", config_params_str, timeout_sec); // reinitialize to make all staged parameters effective - sensor_http->reinitialize(timeout_sec); + sensor_http.reinitialize(timeout_sec); } // save if indicated if (config_flags & CONFIG_PERSIST) { - sensor_http->save_config_params(timeout_sec); + sensor_http.save_config_params(timeout_sec); } return true; } +bool set_config(const std::string& hostname, const sensor_config& config, + uint8_t config_flags, int timeout_sec) { + auto sensor_http = SensorHttp::create(hostname, timeout_sec); + return set_config(*sensor_http, config, config_flags, timeout_sec); +} + std::string get_metadata(client& cli, int timeout_sec) { // Note, this function calls functions that throw std::runtime_error // on timeout. + auto sensor_http = SensorHttp::create(cli.hostname, timeout_sec); + Json::Value meta; try { - cli.meta = collect_metadata(cli.hostname, timeout_sec); + meta = collect_metadata(*sensor_http, timeout_sec); } catch (const std::exception& e) { logger().warn(std::string("Unable to retrieve sensor metadata: ") + e.what()); @@ -395,27 +347,7 @@ std::string get_metadata(client& cli, int timeout_sec) { builder["enableYAMLCompatibility"] = true; builder["precision"] = 6; builder["indentation"] = " "; - auto metadata_string = Json::writeString(builder, cli.meta); - - // We can't insert this logic into the light init_client since its advantage - // is that it doesn't make network calls but we need it to run every time - // there is a valid connection to the sensor So we insert it here - // TODO: remove after release of FW 3.2/3.3 (sufficient warning) - sensor_config config; - get_config(cli.hostname, config); - 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 && - config.udp_profile_lidar == UDPProfileLidar::PROFILE_LIDAR_LEGACY) { - logger().warn( - "Please note that the Legacy Lidar Profile will be deprecated " - "in the sensor FW soon. If you plan to upgrade your FW, we " - "recommend using the Single Return Profile instead. For users " - "sticking with older FWs, the Ouster SDK will continue to parse " - "the legacy lidar profile."); - } - return metadata_string; + return Json::writeString(builder, meta); } bool init_logger(const std::string& log_level, const std::string& log_file_path, @@ -462,6 +394,7 @@ std::shared_ptr init_client(const std::string& hostname, return std::shared_ptr(); try { + auto sensor_http = SensorHttp::create(hostname, timeout_sec); sensor::sensor_config config; uint8_t config_flags = 0; if (udp_dest_host.empty()) @@ -474,12 +407,12 @@ std::shared_ptr init_client(const std::string& hostname, if (imu_port) config.udp_port_imu = imu_port; if (persist_config) config_flags |= CONFIG_PERSIST; config.operating_mode = OPERATING_NORMAL; - set_config(hostname, config, config_flags); + set_config(*sensor_http, config, config_flags, timeout_sec); // will block until no longer INITIALIZING - cli->meta = collect_metadata(hostname, timeout_sec); + auto meta = collect_metadata(*sensor_http, timeout_sec); // check for sensor error states - auto status = cli->meta["sensor_info"]["status"].asString(); + auto status = meta["sensor_info"]["status"].asString(); if (status == "ERROR" || status == "UNCONFIGURED") return std::shared_ptr(); } catch (const std::runtime_error& e) { @@ -508,9 +441,8 @@ std::shared_ptr mtp_init_client(const std::string& hostname, auto cli = std::make_shared(); cli->hostname = hostname; - cli->lidar_fd = mtp_data_socket(lidar_port, udp_dest, mtp_dest_host); - cli->imu_fd = mtp_data_socket( - imu_port); // no need to join multicast group second time + cli->lidar_fd = mtp_data_socket(lidar_port, {udp_dest}, mtp_dest_host); + cli->imu_fd = mtp_data_socket(imu_port, {udp_dest}, mtp_dest_host); if (!impl::socket_valid(cli->lidar_fd) || !impl::socket_valid(cli->imu_fd)) return std::shared_ptr(); @@ -521,17 +453,18 @@ std::shared_ptr mtp_init_client(const std::string& hostname, sensor_config config_copy{config}; try { + auto sensor_http = SensorHttp::create(hostname, timeout_sec); uint8_t config_flags = 0; if (lidar_port) config_copy.udp_port_lidar = lidar_port; if (imu_port) config_copy.udp_port_imu = imu_port; if (persist_config) config_flags |= CONFIG_PERSIST; config_copy.operating_mode = OPERATING_NORMAL; - set_config(hostname, config_copy, config_flags); + set_config(*sensor_http, config_copy, config_flags, timeout_sec); // will block until no longer INITIALIZING - cli->meta = collect_metadata(hostname, timeout_sec); + auto meta = collect_metadata(*sensor_http, timeout_sec); // check for sensor error states - auto status = cli->meta["sensor_info"]["status"].asString(); + auto status = meta["sensor_info"]["status"].asString(); if (status == "ERROR" || status == "UNCONFIGURED") return std::shared_ptr(); } catch (const std::runtime_error& e) { @@ -641,7 +574,7 @@ bool read_lidar_packet(const client& cli, uint8_t* buf, } bool read_lidar_packet(const client& cli, LidarPacket& packet) { - auto now = std::chrono::high_resolution_clock::now(); + auto now = std::chrono::system_clock::now(); packet.host_timestamp = std::chrono::duration_cast( now.time_since_epoch()) @@ -658,7 +591,7 @@ bool read_imu_packet(const client& cli, uint8_t* buf, const packet_format& pf) { } bool read_imu_packet(const client& cli, ImuPacket& packet) { - auto now = std::chrono::high_resolution_clock::now(); + auto now = std::chrono::system_clock::now(); packet.host_timestamp = std::chrono::duration_cast( now.time_since_epoch()) diff --git a/ouster_client/src/field.cpp b/ouster_client/src/field.cpp index a99ec555..fa40bd14 100644 --- a/ouster_client/src/field.cpp +++ b/ouster_client/src/field.cpp @@ -17,6 +17,10 @@ std::vector calculate_strides(const std::vector& shape) { auto strides = std::vector{}; strides.reserve(shape.size()); for (auto dim : shape) { + if (dim == 0) { + strides.push_back(1); + continue; + } total /= dim; strides.push_back(total); } @@ -73,7 +77,7 @@ size_t FieldDescriptor::size() const { std::multiplies{}); } -int FieldDescriptor::element_size() const { return bytes / size(); } +size_t FieldDescriptor::bytes() const { return size() * element_size; } sensor::ChanFieldType FieldDescriptor::tag() const { using sensor::impl::type_cft; @@ -107,9 +111,9 @@ sensor::ChanFieldType FieldDescriptor::tag() const { void FieldDescriptor::swap(FieldDescriptor& other) { std::swap(type, other.type); - std::swap(bytes, other.bytes); std::swap(shape, other.shape); std::swap(strides, other.strides); + std::swap(element_size, other.element_size); } bool FieldDescriptor::is_type_compatible( @@ -126,7 +130,7 @@ FieldView::FieldView(void* ptr, const FieldDescriptor& desc) FieldView::operator bool() const noexcept { return !!get(); } -size_t FieldView::bytes() const noexcept { return desc_.bytes; } +size_t FieldView::bytes() const noexcept { return desc_.bytes(); } size_t FieldView::size() const { return desc_.size(); } @@ -158,7 +162,7 @@ Field::~Field() { free(ptr_); } Field::Field(const FieldDescriptor& desc, FieldClass field_class) : FieldView(nullptr, desc), class_{field_class} { - ptr_ = calloc(desc.bytes, sizeof(uint8_t)); + ptr_ = calloc(desc.bytes(), sizeof(uint8_t)); if (!ptr_) { throw std::runtime_error("Field: host allocation failed"); } @@ -173,7 +177,7 @@ Field& Field::operator=(Field&& other) noexcept { Field::Field(const Field& other) : FieldView(nullptr, other.desc()), class_{other.class_} { - ptr_ = malloc(desc().bytes); + ptr_ = malloc(desc().bytes()); if (!ptr_) { throw std::runtime_error("Field: host allocation failed"); } @@ -207,7 +211,7 @@ FieldView uint_view(const FieldView& other) { } FieldDescriptor desc; - switch (other.desc().element_size()) { + switch (other.desc().element_size) { case 1: desc = FieldDescriptor::array(other.shape()); break; @@ -224,7 +228,9 @@ FieldView uint_view(const FieldView& other) { // shape size check should usually suffice, but this may trigger // on strange cases like views bound to arrays of custom structs throw std::invalid_argument( - "uint_view: got wrong element size, are you using an array " + "uint_view: got wrong element size " + + std::to_string(other.desc().element_size) + + ", are you using an array " "of primitives?"); } diff --git a/ouster_client/src/lidar_scan.cpp b/ouster_client/src/lidar_scan.cpp index 7b3073cd..e62286c4 100644 --- a/ouster_client/src/lidar_scan.cpp +++ b/ouster_client/src/lidar_scan.cpp @@ -57,33 +57,38 @@ namespace impl { template using Table = std::array, N>; -static const Table legacy_field_slots{ +static const Table legacy_field_slots{ {{sensor::ChanField::RANGE, ChanFieldType::UINT32}, - {sensor::ChanField::SIGNAL, ChanFieldType::UINT32}, - {sensor::ChanField::NEAR_IR, ChanFieldType::UINT32}, - {sensor::ChanField::REFLECTIVITY, ChanFieldType::UINT32}}}; + {sensor::ChanField::SIGNAL, ChanFieldType::UINT16}, + {sensor::ChanField::NEAR_IR, ChanFieldType::UINT16}, + {sensor::ChanField::REFLECTIVITY, ChanFieldType::UINT8}, + {sensor::ChanField::FLAGS, ChanFieldType::UINT8}}}; -static const Table dual_field_slots{{ +static const Table dual_field_slots{{ {sensor::ChanField::RANGE, ChanFieldType::UINT32}, {sensor::ChanField::RANGE2, ChanFieldType::UINT32}, {sensor::ChanField::SIGNAL, ChanFieldType::UINT16}, {sensor::ChanField::SIGNAL2, ChanFieldType::UINT16}, {sensor::ChanField::REFLECTIVITY, ChanFieldType::UINT8}, {sensor::ChanField::REFLECTIVITY2, ChanFieldType::UINT8}, + {sensor::ChanField::FLAGS, ChanFieldType::UINT8}, + {sensor::ChanField::FLAGS2, ChanFieldType::UINT8}, {sensor::ChanField::NEAR_IR, ChanFieldType::UINT16}, }}; -static const Table single_field_slots{{ +static const Table single_field_slots{{ {sensor::ChanField::RANGE, ChanFieldType::UINT32}, {sensor::ChanField::SIGNAL, ChanFieldType::UINT16}, - {sensor::ChanField::REFLECTIVITY, ChanFieldType::UINT16}, + {sensor::ChanField::REFLECTIVITY, ChanFieldType::UINT8}, + {sensor::ChanField::FLAGS, ChanFieldType::UINT8}, {sensor::ChanField::NEAR_IR, ChanFieldType::UINT16}, }}; -static const Table lb_field_slots{{ +static const Table lb_field_slots{{ {sensor::ChanField::RANGE, ChanFieldType::UINT32}, - {sensor::ChanField::REFLECTIVITY, ChanFieldType::UINT16}, + {sensor::ChanField::REFLECTIVITY, ChanFieldType::UINT8}, {sensor::ChanField::NEAR_IR, ChanFieldType::UINT16}, + {sensor::ChanField::FLAGS, ChanFieldType::UINT8}, }}; static const Table five_word_slots{{ @@ -94,12 +99,14 @@ static const Table five_word_slots{{ {sensor::ChanField::RAW32_WORD5, ChanFieldType::UINT32}, }}; -static const Table fusa_two_word_slots{{ +static const Table fusa_two_word_slots{{ {sensor::ChanField::RANGE, ChanFieldType::UINT32}, {sensor::ChanField::REFLECTIVITY, ChanFieldType::UINT8}, {sensor::ChanField::NEAR_IR, ChanFieldType::UINT16}, {sensor::ChanField::RANGE2, ChanFieldType::UINT32}, {sensor::ChanField::REFLECTIVITY2, ChanFieldType::UINT8}, + {sensor::ChanField::FLAGS, ChanFieldType::UINT8}, + {sensor::ChanField::FLAGS2, ChanFieldType::UINT8}, }}; struct DefaultFieldsEntry { @@ -183,8 +190,7 @@ static FieldDescriptor get_field_type_descriptor(const LidarScan& scan, return FieldDescriptor::array(ft.element_type, dims); } else if (ft.field_class == FieldClass::PACKET_FIELD) { std::vector dims; - dims.push_back(scan.w / scan.columns_per_packet_ + - (scan.w % scan.columns_per_packet_ ? 1 : 0)); + dims.push_back(scan.packet_count()); dims.insert(dims.end(), ft.extra_dims.begin(), ft.extra_dims.end()); return FieldDescriptor::array(ft.element_type, dims); } else { // FieldClass::SCAN_FIELD @@ -192,10 +198,20 @@ static FieldDescriptor get_field_type_descriptor(const LidarScan& scan, } } +LidarScan::LidarScan(const sensor::sensor_info& info) + : LidarScan{info.format.columns_per_frame, info.format.pixels_per_column, + info.format.udp_profile_lidar, info.format.columns_per_packet} { +} + // specify sensor:: namespace for doxygen matching LidarScan::LidarScan(size_t w, size_t h, LidarScanFieldTypes field_types, size_t columns_per_packet) - : w{w}, h{h}, columns_per_packet_(columns_per_packet) { + : packet_count_{(w + columns_per_packet - 1) / + columns_per_packet}, // equivalent to + // int(ceil(w/columns_per_packet)) + w{w}, + h{h}, + columns_per_packet_(columns_per_packet) { if (w * h == 0) { throw std::invalid_argument( "Cannot construct non-empty LidarScan with " @@ -210,10 +226,11 @@ LidarScan::LidarScan(size_t w, size_t h, LidarScanFieldTypes field_types, measurement_id_ = Field{fd_array(w), FieldClass::COLUMN_FIELD}; status_ = Field{fd_array(w), FieldClass::COLUMN_FIELD}; packet_timestamp_ = - Field{fd_array(w / columns_per_packet + - (w % columns_per_packet ? 1 : 0)), - FieldClass::PACKET_FIELD}; + Field{fd_array(packet_count_), FieldClass::PACKET_FIELD}; pose_ = Field{fd_array(w, 4, 4), {}}; + alert_flags_ = + Field{fd_array(packet_count_), FieldClass::PACKET_FIELD}; + /** * These may be unnecessary to set to identity */ @@ -225,7 +242,8 @@ LidarScan::LidarScan(size_t w, size_t h, LidarScanFieldTypes field_types, LidarScan::LidarScan(const LidarScan& ls_src, const LidarScanFieldTypes& field_types) - : w(ls_src.w), + : packet_count_(ls_src.packet_count_), + w(ls_src.w), h(ls_src.h), columns_per_packet_(ls_src.columns_per_packet_), frame_status(ls_src.frame_status), @@ -295,12 +313,22 @@ bool LidarScan::has_field(const std::string& name) const { } Field& LidarScan::add_field(const FieldType& type) { - if (has_field(type.name) > 0) { + if (has_field(type.name)) { throw std::invalid_argument("Duplicated field '" + type.name + "'"); } - // no other checking is necessary since the user isnt providing dimensions - // that need validation + // just validate that we didnt add a 0 size pixel field + if (type.field_class == FieldClass::PIXEL_FIELD) { + // none of the dimensions should be zero + for (const auto& dim : type.extra_dims) { + if (dim == 0) { + throw std::invalid_argument( + "Cannot add pixel field with 0 elements."); + } + } + } + + // no other checking is necessary fields()[type.name] = Field(get_field_type_descriptor(*this, type), type.field_class); @@ -309,7 +337,7 @@ Field& LidarScan::add_field(const FieldType& type) { Field& LidarScan::add_field(const std::string& name, FieldDescriptor desc, FieldClass field_class) { - if (has_field(name) > 0) + if (has_field(name)) throw std::invalid_argument("Duplicated field '" + name + "'"); if (field_class == FieldClass::PIXEL_FIELD) { @@ -323,6 +351,13 @@ Field& LidarScan::add_field(const std::string& name, FieldDescriptor desc, std::to_string(desc.shape[0]) + "x" + std::to_string(desc.shape[1]) + " vs " + std::to_string(h) + "x" + std::to_string(w)); + // none of the dimensions should be zero + for (const auto& dim : desc.shape) { + if (dim == 0) { + throw std::invalid_argument( + "Cannot add pixel field with 0 elements."); + } + } } if (field_class == FieldClass::COLUMN_FIELD) { @@ -335,14 +370,12 @@ Field& LidarScan::add_field(const std::string& name, FieldDescriptor desc, } if (field_class == FieldClass::PACKET_FIELD) { - const size_t desired_w = - w / columns_per_packet_ + (w % columns_per_packet_ ? 1 : 0); - if (desc.shape[0] != desired_w) + if (desc.shape[0] != packet_count_) throw std::invalid_argument( "Packet field shape must match " "number of packets. Width was " + std::to_string(desc.shape[0]) + " vs required width of " + - std::to_string(desired_w)); + std::to_string(packet_count_)); } fields()[name] = Field{desc, field_class}; @@ -438,6 +471,14 @@ Eigen::Ref> LidarScan::packet_timestamp() return packet_timestamp_; } +Eigen::Ref> LidarScan::alert_flags() { + return alert_flags_; +} + +Eigen::Ref> LidarScan::alert_flags() const { + return alert_flags_; +} + uint64_t LidarScan::get_first_valid_packet_timestamp() const { int total_packets = packet_timestamp().size(); int columns_per_packet = w / total_packets; @@ -453,6 +494,16 @@ uint64_t LidarScan::get_first_valid_packet_timestamp() const { return 0; } +uint64_t LidarScan::get_first_valid_column_timestamp() const { + auto stat = status(); + for (int i = 0; i < timestamp().size(); i++) { + if ((stat[i] & 1) > 0) { + return timestamp()[i]; + } + } + return 0; +} + Eigen::Ref> LidarScan::measurement_id() { return measurement_id_; } @@ -491,6 +542,8 @@ bool LidarScan::complete(sensor::ColumnWindow window) const { } } +size_t LidarScan::packet_count() const { return packet_count_; } + bool operator==(const LidarScan& a, const LidarScan& b) { return a.frame_id == b.frame_id && a.w == b.w && a.h == b.h && a.frame_status == b.frame_status && @@ -577,8 +630,10 @@ std::string to_string(const LidarScan& ls) { } ss << ") "; - FieldView flat_view = f.reshape(1, f.size()); - impl::visit_field_2d(flat_view, read_eigen, ss); + if (f.bytes() > 0) { + FieldView flat_view = f.reshape(1, f.size()); + impl::visit_field_2d(flat_view, read_eigen, ss); + } ss << std::endl; }; @@ -677,6 +732,24 @@ XYZLut make_xyz_lut(size_t w, size_t h, double range_unit, return lut; } +XYZLut make_xyz_lut(const sensor::sensor_info& sensor, bool use_extrinsics) { + mat4d transform = sensor.lidar_to_sensor_transform; + if (use_extrinsics) { + // apply extrinsics after lidar_to_sensor_transform so the + // resulting LUT will produce the coordinates in + // "extrinsics frame" instead of "sensor frame" + mat4d ext_transform = sensor.extrinsic; + ext_transform(0, 3) /= sensor::range_unit; + ext_transform(1, 3) /= sensor::range_unit; + ext_transform(2, 3) /= sensor::range_unit; + transform = ext_transform * sensor.lidar_to_sensor_transform; + } + return make_xyz_lut( + sensor.format.columns_per_frame, sensor.format.pixels_per_column, + sensor::range_unit, sensor.beam_to_lidar_transform, transform, + sensor.beam_azimuth_angles, sensor.beam_altitude_angles); +} + LidarScan::Points cartesian(const LidarScan& scan, const XYZLut& lut) { return cartesian(scan.field(sensor::ChanField::RANGE), lut); } @@ -703,10 +776,37 @@ ScanBatcher::ScanBatcher(size_t w, const sensor::packet_format& pf) pf(pf) { if (pf.columns_per_packet == 0) throw std::invalid_argument("unexpected columns_per_packet: 0"); + // Since we don't know the azimuth window, assume it is full. + const size_t desired_w = + w / pf.columns_per_packet + (w % pf.columns_per_packet ? 1 : 0); + expected_packets = desired_w; } ScanBatcher::ScanBatcher(const sensor::sensor_info& info) - : ScanBatcher(info.format.columns_per_frame, sensor::get_format(info)) {} + : ScanBatcher(info.format.columns_per_frame, sensor::get_format(info)) { + // Calculate the number of packets required to have a complete scan + int max_packets = expected_packets; + if (info.format.column_window.second < info.format.column_window.first) { + // the valid azimuth window wraps through 0 + int start_packet = + info.format.column_window.second / pf.columns_per_packet; + int end_packet = + info.format.column_window.first / pf.columns_per_packet; + expected_packets = start_packet + 1 + (max_packets - end_packet); + // subtract one if start and end are in the same block + if (start_packet == end_packet) { + expected_packets -= 1; + } + } else { + // no wrapping of azimuth the window through 0 + int start_packet = + info.format.column_window.first / pf.columns_per_packet; + int end_packet = + info.format.column_window.second / pf.columns_per_packet; + + expected_packets = end_packet - start_packet + 1; + } +} namespace { @@ -922,6 +1022,15 @@ bool ScanBatcher::operator()(const ouster::sensor::LidarPacket& packet, return (*this)(packet.buf.data(), packet.host_timestamp, ls); } +void ScanBatcher::finalize_scan(LidarScan& ls, bool raw_headers) { + impl::foreach_channel_field(ls, pf, zero_field_cols{}, next_valid_m_id, w); + + if (raw_headers) { + impl::visit_field(ls, sensor::ChanField::RAW_HEADERS, zero_field_cols{}, + "", next_headers_m_id, w); + } +} + bool ScanBatcher::operator()(const uint8_t* packet_buf, uint64_t packet_ts, LidarScan& ls) { if (ls.w != w || ls.h != h) @@ -934,7 +1043,6 @@ bool ScanBatcher::operator()(const uint8_t* packet_buf, uint64_t packet_ts, // process cached packet and packet ts if (cached_packet) { cached_packet = false; - ls.frame_id = -1; this->operator()(cache.data(), cache_packet_ts, ls); } @@ -942,44 +1050,60 @@ bool ScanBatcher::operator()(const uint8_t* packet_buf, uint64_t packet_ts, const bool raw_headers = impl::raw_headers_enabled(pf, ls); - if (ls.frame_id == -1) { + if (ls.frame_id == -1 || finished_scan_id >= 0) { // expecting to start batching a new scan + if (finished_scan_id >= 0) { + // drop duplicate or packets from previous frame + if (finished_scan_id == f_id) { + // drop old duplicate packets + return false; + } else if (finished_scan_id == + ((f_id + 1) % + (static_cast(pf.max_frame_id) + 1))) { + // drop reordered packets from the previous frame + return false; + } + } + finished_scan_id = -1; next_valid_m_id = 0; next_headers_m_id = 0; + batched_packets = 0; ls.frame_id = f_id; zero_header_cols(ls, 0, w); ls.packet_timestamp().setZero(); const uint8_t f_thermal_shutdown = pf.thermal_shutdown(packet_buf); const uint8_t f_shot_limiting = pf.shot_limiting(packet_buf); ls.frame_status = frame_status(f_thermal_shutdown, f_shot_limiting); + + // The countdown values are supposed to be the same for all packets in a + // given scan. + ls.shutdown_countdown = pf.countdown_thermal_shutdown(packet_buf); + ls.shot_limiting_countdown = pf.countdown_shot_limiting(packet_buf); } else if (ls.frame_id == ((f_id + 1) % (static_cast(pf.max_frame_id) + 1))) { // drop reordered packets from the previous frame return false; } else if (ls.frame_id != f_id) { - // got a packet from a new frame - impl::foreach_channel_field(ls, pf, zero_field_cols{}, next_valid_m_id, - w); - - if (raw_headers) { - impl::visit_field(ls, sensor::ChanField::RAW_HEADERS, - zero_field_cols{}, "", next_headers_m_id, w); - } + // got a packet from a new frame, release the old one + finished_scan_id = ls.frame_id; + finalize_scan(ls, raw_headers); // store packet buf and ts data to the cache for later processing std::memcpy(cache.data(), packet_buf, cache.size()); cache_packet_ts = packet_ts; cached_packet = true; - return true; } + batched_packets++; + // handling packet level data: packet_timestamp const uint8_t* col0_buf = pf.nth_col(0, packet_buf); const uint16_t packet_id = pf.col_measurement_id(col0_buf) / pf.columns_per_packet; if (packet_id < ls.packet_timestamp().rows()) { ls.packet_timestamp()[packet_id] = packet_ts; + ls.alert_flags()[packet_id] = pf.alert_flags(packet_buf); } // handling column and pixel level data @@ -1002,6 +1126,14 @@ bool ScanBatcher::operator()(const uint8_t* packet_buf, uint64_t packet_ts, _parse_by_col(packet_buf, ls); } + // if we have enough packets and are packet-complete release the scan + if (batched_packets >= expected_packets && + (size_t)ls.packet_timestamp().count() == expected_packets) { + finished_scan_id = f_id; + finalize_scan(ls, raw_headers); + return true; + } + return false; } @@ -1025,4 +1157,56 @@ bool operator!=(const FieldType& a, const FieldType& b) { a.field_class != b.field_class || a.extra_dims != b.extra_dims; } +namespace pose_util { +void dewarp(Eigen::Ref dewarped, const Eigen::Ref points, + const Eigen::Ref poses) { + const size_t W = poses.rows(); // Number of pose matrices + const size_t H = points.rows() / poses.rows(); // Points per pose matrix + +#ifdef __OUSTER_UTILIZE_OPENMP__ +#pragma omp parallel for schedule(static) +#endif + for (size_t w = 0; w < W; ++w) { + Eigen::Map> + pose_matrix(poses.row(w).data()); + const Eigen::Matrix3d rotation = pose_matrix.topLeftCorner<3, 3>(); + const Eigen::Vector3d translation = pose_matrix.topRightCorner<3, 1>(); + + for (size_t i = 0; i < H; ++i) { + const Eigen::Index ix = i * W + w; + Eigen::Map s(points.row(ix).data()); + Eigen::Map p(dewarped.row(ix).data()); + p = rotation * s + translation; + } + } +} + +Points dewarp(const Eigen::Ref points, + const Eigen::Ref poses) { + Points dewarped(points.rows(), points.cols()); + dewarp(dewarped, points, poses); + return dewarped; +} + +void transform(Eigen::Ref transformed, + const Eigen::Ref points, + const Eigen::Ref pose) { + Eigen::Matrix pose_matrix = + Eigen::Map>( + pose.data()); + + Eigen::Matrix3d rotation = pose_matrix.topLeftCorner<3, 3>(); + Eigen::Vector3d translation = pose_matrix.topRightCorner<3, 1>(); + + transformed = + (points * rotation.transpose()).rowwise() + translation.transpose(); +} + +Points transform(const Eigen::Ref points, + const Eigen::Ref pose) { + Points transformed(points.rows(), points.cols()); + transform(transformed, points, pose); + return transformed; +} +} // namespace pose_util } // namespace ouster diff --git a/ouster_client/src/parsing.cpp b/ouster_client/src/parsing.cpp index fb9136eb..cd28c37a 100644 --- a/ouster_client/src/parsing.cpp +++ b/ouster_client/src/parsing.cpp @@ -8,7 +8,6 @@ #include #include #include -#include #include #include #include @@ -29,13 +28,148 @@ constexpr int imu_packet_size = 48; template using Table = std::array, N>; +/** + * Helper struct to load/store bit sequences from packets + * + * NOTE: getters and setters require up to 64 bits of valid memory past the bit + * we are attempting to set/retrieve, so caution is advised. + */ struct FieldInfo { ChanFieldType ty_tag; size_t offset; uint64_t mask; int shift; + + /** + * Retrieves the value from the buffer. + * NOTE: the check that T is of at least the size of ChanFieldType used + * is deferred because this function is used in the hot loop + * + * @param[in] buffer buffer to retrieve the value from. + * + * @return value + */ + template + T get(const uint8_t* buffer) const { + uint64_t word = *reinterpret_cast(buffer + offset); + word &= mask; + if (shift > 0) { + word >>= shift; + } else if (shift < 0) { + word <<= std::abs(shift); + } + + T out{}; + std::memcpy(&out, &word, sizeof(out)); + return out; + } + + /** + * Stores the value into the buffer. + * NOTE: the check that T is of at least the size of ChanFieldType used + * is deferred because this function is used in the hot loop + * + * @param[in] buffer buffer to retrieve the value from. + * @param[in] value value to store + */ + template + void set(uint8_t* buffer, T value) const { + uint64_t word = 0; + std::memcpy(&word, &value, sizeof(value)); + if (shift > 0) word <<= shift; + if (shift < 0) word >>= std::abs(shift); + word &= mask; + uint64_t* ptr = reinterpret_cast(buffer + offset); + *ptr &= ~mask; + *ptr |= word; + } +}; + +/** + * FieldInfo factory function + * + * NOTE: FieldInfo getters and setters require up to 64 bits of valid memory + * past the bit_start, caution is advised. + * + * @param[in] bit_start starting bit of the value in the buffer + * @param[in] bit_size size, in bits, of the value in the buffer + * @param[in] upshift amount of bits to shift the value up, if any; this is + * used in packet values that are truncating lower significance bits, + * e.g. in low bandwidth profiles + * + * @return FieldInfo + */ +FieldInfo field_info(size_t bit_start, size_t bit_size, size_t upshift = 0) { + FieldInfo info{}; + + size_t needs_bits = bit_size + upshift; + if (needs_bits > 64) { + throw std::invalid_argument( + "failed creating FieldInfo: value cannot store more than 64 bits"); + } + + info.offset = bit_start / 8; + bit_start = bit_start % 8; + + for (size_t i = bit_start; i < bit_start + bit_size; ++i) { + info.mask |= uint64_t{1} << i; + } + + info.shift = bit_start; + info.shift -= upshift; + + size_t size_bytes = needs_bits / 8 + ((needs_bits % 8) ? 1 : 0); + + switch (size_bytes) { + case 1: + info.ty_tag = ChanFieldType::UINT8; + break; + case 2: + info.ty_tag = ChanFieldType::UINT16; + break; + case 3: + case 4: + info.ty_tag = ChanFieldType::UINT32; + break; + case 5: + case 6: + case 7: + case 8: + info.ty_tag = ChanFieldType::UINT64; + break; + default: + info.ty_tag = ChanFieldType::VOID; + } + + return info; +} + +static int count_set_bits(uint64_t value) { + int count = 0; + while (value) { + count += value & 1; + value >>= 1; + } + return count; }; +uint64_t get_value_mask(const FieldInfo& f) { + uint64_t type_mask = sensor::field_type_mask(f.ty_tag); + + uint64_t mask = f.mask; + if (mask == 0) mask = type_mask; + if (f.shift > 0) mask >>= f.shift; + if (f.shift < 0) mask <<= std::abs(f.shift); + // final type *may* cut the resultant mask still + mask &= type_mask; + + return mask; +} + +int get_bitness(const FieldInfo& f) { + return count_set_bits(get_value_mask(f)); +} + struct ProfileEntry { const std::pair* fields; size_t n_fields; @@ -43,78 +177,78 @@ struct ProfileEntry { }; static const Table legacy_field_info{{ - {ChanField::RANGE, {UINT32, 0, 0x000fffff, 0}}, - {ChanField::FLAGS, {UINT8, 3, 0, 4}}, - {ChanField::REFLECTIVITY, {UINT16, 4, 0, 0}}, - {ChanField::SIGNAL, {UINT16, 6, 0, 0}}, - {ChanField::NEAR_IR, {UINT16, 8, 0, 0}}, - {ChanField::RAW32_WORD1, {UINT32, 0, 0, 0}}, - {ChanField::RAW32_WORD2, {UINT32, 4, 0, 0}}, - {ChanField::RAW32_WORD3, {UINT32, 8, 0, 0}}, + {ChanField::RANGE, field_info(0, 20)}, + {ChanField::FLAGS, field_info(28, 4)}, + {ChanField::REFLECTIVITY, field_info(32, 8)}, + {ChanField::SIGNAL, field_info(48, 16)}, + {ChanField::NEAR_IR, field_info(64, 16)}, + {ChanField::RAW32_WORD1, field_info(0, 32)}, + {ChanField::RAW32_WORD2, field_info(32, 32)}, + {ChanField::RAW32_WORD3, field_info(64, 32)}, }}; static const Table lb_field_info{{ - {ChanField::RANGE, {UINT32, 0, 0x7fff, -3}}, - {ChanField::FLAGS, {UINT8, 1, 0b10000000, 7}}, - {ChanField::REFLECTIVITY, {UINT8, 2, 0, 0}}, - {ChanField::NEAR_IR, {UINT16, 2, 0xff00, 4}}, - {ChanField::RAW32_WORD1, {UINT32, 0, 0, 0}}, + {ChanField::RANGE, field_info(0, 15, 3)}, + {ChanField::FLAGS, field_info(15, 1)}, + {ChanField::REFLECTIVITY, field_info(16, 8)}, + {ChanField::NEAR_IR, field_info(24, 8, 4)}, + {ChanField::RAW32_WORD1, field_info(0, 32)}, }}; static const Table dual_field_info{{ - {ChanField::RANGE, {UINT32, 0, 0x0007ffff, 0}}, - {ChanField::FLAGS, {UINT8, 2, 0b11111000, 3}}, - {ChanField::REFLECTIVITY, {UINT8, 3, 0, 0}}, - {ChanField::RANGE2, {UINT32, 4, 0x0007ffff, 0}}, - {ChanField::FLAGS2, {UINT8, 6, 0b11111000, 3}}, - {ChanField::REFLECTIVITY2, {UINT8, 7, 0, 0}}, - {ChanField::SIGNAL, {UINT16, 8, 0, 0}}, - {ChanField::SIGNAL2, {UINT16, 10, 0, 0}}, - {ChanField::NEAR_IR, {UINT16, 12, 0, 0}}, - {ChanField::RAW32_WORD1, {UINT32, 0, 0, 0}}, - {ChanField::RAW32_WORD2, {UINT32, 4, 0, 0}}, - {ChanField::RAW32_WORD3, {UINT32, 8, 0, 0}}, - {ChanField::RAW32_WORD4, {UINT32, 12, 0, 0}}, + {ChanField::RANGE, field_info(0, 19)}, + {ChanField::FLAGS, field_info(19, 5)}, + {ChanField::REFLECTIVITY, field_info(24, 8)}, + {ChanField::RANGE2, field_info(32, 19)}, + {ChanField::FLAGS2, field_info(51, 5)}, + {ChanField::REFLECTIVITY2, field_info(56, 8)}, + {ChanField::SIGNAL, field_info(64, 16)}, + {ChanField::SIGNAL2, field_info(80, 16)}, + {ChanField::NEAR_IR, field_info(96, 16)}, + {ChanField::RAW32_WORD1, field_info(0, 32)}, + {ChanField::RAW32_WORD2, field_info(32, 32)}, + {ChanField::RAW32_WORD3, field_info(64, 32)}, + {ChanField::RAW32_WORD4, field_info(96, 32)}, }}; static const Table single_field_info{{ - {ChanField::RANGE, {UINT32, 0, 0x0007ffff, 0}}, - {ChanField::FLAGS, {UINT8, 2, 0b11111000, 3}}, - {ChanField::REFLECTIVITY, {UINT8, 4, 0, 0}}, - {ChanField::SIGNAL, {UINT16, 6, 0, 0}}, - {ChanField::NEAR_IR, {UINT16, 8, 0, 0}}, - {ChanField::RAW32_WORD1, {UINT32, 0, 0, 0}}, - {ChanField::RAW32_WORD2, {UINT32, 4, 0, 0}}, - {ChanField::RAW32_WORD3, {UINT32, 8, 0, 0}}, + {ChanField::RANGE, field_info(0, 19)}, + {ChanField::FLAGS, field_info(19, 5)}, + {ChanField::REFLECTIVITY, field_info(32, 8)}, + {ChanField::SIGNAL, field_info(48, 16)}, + {ChanField::NEAR_IR, field_info(64, 16)}, + {ChanField::RAW32_WORD1, field_info(0, 32)}, + {ChanField::RAW32_WORD2, field_info(32, 32)}, + {ChanField::RAW32_WORD3, field_info(64, 32)}, }}; static const Table five_word_pixel_info{{ - {ChanField::RANGE, {UINT32, 0, 0x0007ffff, 0}}, - {ChanField::FLAGS, {UINT8, 2, 0b11111000, 3}}, - {ChanField::REFLECTIVITY, {UINT8, 3, 0, 0}}, - {ChanField::RANGE2, {UINT32, 4, 0x0007ffff, 0}}, - {ChanField::FLAGS2, {UINT8, 6, 0b11111000, 3}}, - {ChanField::REFLECTIVITY2, {UINT8, 7, 0, 0}}, - {ChanField::SIGNAL, {UINT16, 8, 0, 0}}, - {ChanField::SIGNAL2, {UINT16, 10, 0, 0}}, - {ChanField::NEAR_IR, {UINT16, 12, 0, 0}}, - {ChanField::RAW32_WORD1, {UINT32, 0, 0, 0}}, - {ChanField::RAW32_WORD2, {UINT32, 4, 0, 0}}, - {ChanField::RAW32_WORD3, {UINT32, 8, 0, 0}}, - {ChanField::RAW32_WORD4, {UINT32, 12, 0, 0}}, - {ChanField::RAW32_WORD5, {UINT32, 16, 0, 0}}, + {ChanField::RANGE, field_info(0, 19)}, + {ChanField::FLAGS, field_info(19, 5)}, + {ChanField::REFLECTIVITY, field_info(24, 8)}, + {ChanField::RANGE2, field_info(32, 19)}, + {ChanField::FLAGS2, field_info(51, 5)}, + {ChanField::REFLECTIVITY2, field_info(56, 8)}, + {ChanField::SIGNAL, field_info(64, 16)}, + {ChanField::SIGNAL2, field_info(80, 16)}, + {ChanField::NEAR_IR, field_info(96, 16)}, + {ChanField::RAW32_WORD1, field_info(0, 32)}, + {ChanField::RAW32_WORD2, field_info(32, 32)}, + {ChanField::RAW32_WORD3, field_info(64, 32)}, + {ChanField::RAW32_WORD4, field_info(96, 32)}, + {ChanField::RAW32_WORD5, field_info(128, 32)}, }}; static const Table fusa_two_word_pixel_info{{ - {ChanField::RANGE, {UINT32, 0, 0x7fff, -3}}, - {ChanField::FLAGS, {UINT8, 1, 0b10000000, 7}}, - {ChanField::REFLECTIVITY, {UINT8, 2, 0xff, 0}}, - {ChanField::NEAR_IR, {UINT16, 3, 0xff, -4}}, - {ChanField::RANGE2, {UINT32, 4, 0x7fff, -3}}, - {ChanField::FLAGS2, {UINT8, 5, 0b10000000, 7}}, - {ChanField::REFLECTIVITY2, {UINT8, 6, 0xff, 0}}, - {ChanField::RAW32_WORD1, {UINT32, 0, 0, 0}}, - {ChanField::RAW32_WORD2, {UINT32, 4, 0, 0}}, + {ChanField::RANGE, field_info(0, 15, 3)}, + {ChanField::FLAGS, field_info(15, 1)}, + {ChanField::REFLECTIVITY, field_info(16, 8)}, + {ChanField::NEAR_IR, field_info(24, 8, 4)}, + {ChanField::RANGE2, field_info(32, 15, 3)}, + {ChanField::FLAGS2, field_info(47, 1)}, + {ChanField::REFLECTIVITY2, field_info(48, 8)}, + {ChanField::RAW32_WORD1, field_info(0, 32)}, + {ChanField::RAW32_WORD2, field_info(32, 32)}, }}; Table profiles{{ @@ -144,35 +278,6 @@ static const ProfileEntry& lookup_profile_entry(UDPProfileLidar profile) { return it->second; } -static int count_set_bits(uint64_t value) { - int count = 0; - while (value) { - count += value & 1; - value >>= 1; - } - return count; -}; - -// TODO: move this out to some generalised FieldInfo utils -uint64_t get_value_mask(const FieldInfo& f) { - // first get type mask - uint64_t type_mask = (uint64_t{1} << (field_type_size(f.ty_tag) * 8)) - 1; - - uint64_t mask = f.mask; - if (mask == 0) mask = type_mask; - if (f.shift > 0) mask >>= f.shift; - if (f.shift < 0) mask <<= std::abs(f.shift); - // final type *may* cut the resultant mask still - mask &= type_mask; - - return mask; -} - -// TODO: move this out to some generalised FieldInfo utils -int get_bitness(const FieldInfo& f) { - return count_set_bits(get_value_mask(f)); -} - } // namespace impl struct packet_format::Impl { @@ -187,12 +292,24 @@ struct packet_format::Impl { size_t col_size; size_t lidar_packet_size; - size_t timestamp_offset; - size_t measurement_id_offset; - size_t status_offset; - std::map fields; + // header infos + impl::FieldInfo packet_type_info; + impl::FieldInfo frame_id_info; + impl::FieldInfo init_id_info; + impl::FieldInfo prod_sn_info; + impl::FieldInfo alert_flags_info; + impl::FieldInfo countdown_thermal_shutdown_info; + impl::FieldInfo countdown_shot_limiting_info; + impl::FieldInfo thermal_shutdown_info; + impl::FieldInfo shot_limiting_info; + + // column infos + impl::FieldInfo col_status_info; + impl::FieldInfo col_timestamp_info; + impl::FieldInfo col_measurement_id_info; + Impl(UDPProfileLidar profile, size_t pixels_per_column, size_t columns_per_packet) { bool legacy = (profile == UDPProfileLidar::PROFILE_LIDAR_LEGACY); @@ -205,12 +322,6 @@ struct packet_format::Impl { col_footer_size = legacy ? 4 : 0; packet_footer_size = legacy ? 0 : 32; - if (profile == UDPProfileLidar::PROFILE_FUSA_RNG15_RFL8_NIR8_DUAL) { - max_frame_id = std::numeric_limits::max(); - } else { - max_frame_id = std::numeric_limits::max(); - } - col_size = col_header_size + pixels_per_column * channel_data_size + col_footer_size; lidar_packet_size = packet_header_size + columns_per_packet * col_size + @@ -222,9 +333,76 @@ struct packet_format::Impl { fields = {entry.fields, entry.fields + entry.n_fields}; - timestamp_offset = 0; - measurement_id_offset = 8; - status_offset = legacy ? col_size - col_footer_size : 10; + // TODO: amend how we detect FUSA once we have a different mechanism + bool fusa = false; + if (profile == UDPProfileLidar::PROFILE_FUSA_RNG15_RFL8_NIR8_DUAL) { + max_frame_id = std::numeric_limits::max(); + fusa = true; + } else { + max_frame_id = std::numeric_limits::max(); + } + + using impl::field_info; + + if (legacy) { + // below are absent on legacy, results in mask==0 + packet_type_info = field_info(0, 0); + init_id_info = field_info(0, 0); + prod_sn_info = field_info(0, 0); + alert_flags_info = field_info(0, 0); + countdown_thermal_shutdown_info = field_info(0, 0); + countdown_shot_limiting_info = field_info(0, 0); + thermal_shutdown_info = field_info(0, 0); + shot_limiting_info = field_info(0, 0); + + // frame_id is baked into the first column header + frame_id_info = field_info(80, 16); + + col_status_info = field_info(8 * (col_size - col_footer_size), 32); + /** + * LEGACY col_status sits at the end of the column as opposed to + * being in column header, and FieldInfo::get takes 8-byte word, + * which would read memory values past the end of the packet. + * + * This is a crude way of making it read 8-byte word from the left + * instead. + * TODO: if we run into this issue again, add "pad_left" parameter + * to field_info(), otherwise leaving it here + * -- Tim T. + */ + col_status_info.offset -= 4; + col_status_info.mask <<= 32; + col_status_info.shift += 32; + } else if (fusa) { + packet_type_info = field_info(0, 8); + frame_id_info = field_info(32, 32); + init_id_info = field_info(8, 24); + alert_flags_info = field_info( + 64, 8); // Supposedly supported in both 2.5.X and 3.1.X + prod_sn_info = field_info(88, 40); + countdown_thermal_shutdown_info = field_info(128, 8); + countdown_shot_limiting_info = field_info(136, 8); + thermal_shutdown_info = field_info(144, 4); + shot_limiting_info = field_info(156, 4); + + col_status_info = field_info(80, 16); + } else { + packet_type_info = field_info(0, 16); + frame_id_info = field_info(16, 16); + init_id_info = field_info(32, 24); + prod_sn_info = field_info(56, 40); + alert_flags_info = field_info( + 96, 8); // Supposedly supported in both 2.5.X and 3.1.X + countdown_thermal_shutdown_info = field_info(128, 8); + countdown_shot_limiting_info = field_info(136, 8); + thermal_shutdown_info = field_info(144, 4); + shot_limiting_info = field_info(156, 4); + + col_status_info = field_info(80, 16); + } + + col_timestamp_info = field_info(0, 64); + col_measurement_id_info = field_info(64, 16); } }; @@ -272,26 +450,24 @@ class SameSizeInt { typedef uint64_t value; }; -template -void packet_format::block_field_impl(Eigen::Ref> field, - const std::string& chan, - const uint8_t* packet_buf) const { - if (sizeof(T) < sizeof(SRC)) - throw std::invalid_argument("Dest type too small for specified field"); +template +void packet_format::block_field(Eigen::Ref> field, + const std::string& chan, + const uint8_t* packet_buf) const { + impl::FieldInfo f = impl_->fields.at(chan); - const auto& f = impl_->fields.at(chan); + if (sizeof(T) < field_type_size(f.ty_tag)) + throw std::invalid_argument("Dest type too small for specified field"); - size_t offset = f.offset; - uint64_t mask = f.mask; - int shift = f.shift; size_t channel_data_size = impl_->channel_data_size; int cols = field.cols(); + T* data = field.data(); - std::array col_buf; + std::array col_buf; - for (int icol = 0; icol < columns_per_packet; icol += N) { - for (int i = 0; i < N; ++i) { + for (int icol = 0; icol < columns_per_packet; icol += BlockDim) { + for (int i = 0; i < BlockDim; ++i) { col_buf[i] = nth_col(icol + i, packet_buf); } @@ -299,60 +475,29 @@ void packet_format::block_field_impl(Eigen::Ref> field, for (int px = 0; px < pixels_per_column; ++px) { std::ptrdiff_t f_offset = cols * px + m_id; - for (int x = 0; x < N; ++x) { + for (int x = 0; x < BlockDim; ++x) { auto px_src = col_buf[x] + col_header_size + (px * channel_data_size); - typename SameSizeInt::value dst = - *reinterpret_cast::value*>( - px_src + offset); - if (mask) dst &= mask; - if (shift > 0) dst >>= shift; - if (shift < 0) dst <<= std::abs(shift); - *(data + f_offset + x) = dst; + *(data + f_offset + x) = f.get(px_src); } } } } -template -void packet_format::block_field(Eigen::Ref> field, - const std::string& chan, - const uint8_t* packet_buf) const { - const auto& f = impl_->fields.at(chan); +template +void packet_format::col_field(const uint8_t* col_buf, const std::string& i, + T* dst, int dst_stride) const { + impl::FieldInfo f = impl_->fields.at(i); - switch (f.ty_tag) { - case UINT8: - block_field_impl(field, chan, packet_buf); - break; - case UINT16: - block_field_impl(field, chan, packet_buf); - break; - case UINT32: - block_field_impl(field, chan, packet_buf); - break; - case UINT64: - block_field_impl(field, chan, packet_buf); - break; - case INT8: - block_field_impl(field, chan, packet_buf); - break; - case INT16: - block_field_impl(field, chan, packet_buf); - break; - case INT32: - block_field_impl(field, chan, packet_buf); - break; - case INT64: - block_field_impl(field, chan, packet_buf); - break; - case FLOAT32: - block_field_impl(field, chan, packet_buf); - break; - case FLOAT64: - block_field_impl(field, chan, packet_buf); - break; - default: - throw std::invalid_argument("Invalid field for packet format"); + if (sizeof(T) < field_type_size(f.ty_tag)) + throw std::invalid_argument("Dest type too small for specified field"); + + size_t channel_data_size = impl_->channel_data_size; + + for (int px = 0; px < pixels_per_column; px++) { + auto px_src = col_buf + col_header_size + (px * channel_data_size); + T* px_dst = dst + px * dst_stride; + *px_dst = f.get(px_src); } } @@ -448,130 +593,6 @@ template void packet_format::block_field( Eigen::Ref> field, const std::string& chan, const uint8_t* packet_buf) const; -template -static void col_field_impl(const uint8_t* col_buf, DST* dst, size_t offset, - uint64_t mask, int shift, int pixels_per_column, - int dst_stride, size_t channel_data_size, - size_t col_header_size) { - if (sizeof(DST) < sizeof(SRC)) - throw std::invalid_argument("Dest type too small for specified field"); - - for (int px = 0; px < pixels_per_column; px++) { - auto px_src = - col_buf + col_header_size + offset + (px * channel_data_size); - DST* px_dst = dst + px * dst_stride; - typename SameSizeInt::value dst = - *reinterpret_cast::value*>(px_src); - if (mask) dst &= mask; - if (shift > 0) dst >>= shift; - if (shift < 0) dst <<= std::abs(shift); - *px_dst = *reinterpret_cast(&dst); - } -} - -template -static void col_field_impl(const uint8_t* col_buf, float* dst, size_t offset, - uint64_t mask, int shift, int pixels_per_column, - int dst_stride, size_t channel_data_size, - size_t col_header_size) { - if (sizeof(float) < sizeof(SRC)) - throw std::invalid_argument("Dest type too small for specified field"); - - for (int px = 0; px < pixels_per_column; px++) { - auto px_src = - col_buf + col_header_size + offset + (px * channel_data_size); - float* px_dst = dst + px * dst_stride; - typename SameSizeInt::value dst = - *reinterpret_cast::value*>(px_src); - if (mask) dst &= mask; - if (shift > 0) dst >>= shift; - if (shift < 0) dst <<= std::abs(shift); - memcpy(px_dst, &dst, sizeof(float)); - } -} - -template -static void col_field_impl(const uint8_t* col_buf, double* dst, size_t offset, - uint64_t mask, int shift, int pixels_per_column, - int dst_stride, size_t channel_data_size, - size_t col_header_size) { - if (sizeof(float) < sizeof(SRC)) - throw std::invalid_argument("Dest type too small for specified field"); - - for (int px = 0; px < pixels_per_column; px++) { - auto px_src = - col_buf + col_header_size + offset + (px * channel_data_size); - double* px_dst = dst + px * dst_stride; - typename SameSizeInt::value dst = - *reinterpret_cast::value*>(px_src); - if (mask) dst &= mask; - if (shift > 0) dst >>= shift; - if (shift < 0) dst <<= std::abs(shift); - memcpy(px_dst, &dst, sizeof(double)); - } -} - -template -void packet_format::col_field(const uint8_t* col_buf, const std::string& i, - T* dst, int dst_stride) const { - const auto& f = impl_->fields.at(i); - - switch (f.ty_tag) { - case UINT8: - col_field_impl( - col_buf, dst, f.offset, f.mask, f.shift, pixels_per_column, - dst_stride, impl_->channel_data_size, impl_->col_header_size); - break; - case UINT16: - col_field_impl( - col_buf, dst, f.offset, f.mask, f.shift, pixels_per_column, - dst_stride, impl_->channel_data_size, impl_->col_header_size); - break; - case UINT32: - col_field_impl( - col_buf, dst, f.offset, f.mask, f.shift, pixels_per_column, - dst_stride, impl_->channel_data_size, impl_->col_header_size); - break; - case UINT64: - col_field_impl( - col_buf, dst, f.offset, f.mask, f.shift, pixels_per_column, - dst_stride, impl_->channel_data_size, impl_->col_header_size); - break; - case INT8: - col_field_impl( - col_buf, dst, f.offset, f.mask, f.shift, pixels_per_column, - dst_stride, impl_->channel_data_size, impl_->col_header_size); - break; - case INT16: - col_field_impl( - col_buf, dst, f.offset, f.mask, f.shift, pixels_per_column, - dst_stride, impl_->channel_data_size, impl_->col_header_size); - break; - case INT32: - col_field_impl( - col_buf, dst, f.offset, f.mask, f.shift, pixels_per_column, - dst_stride, impl_->channel_data_size, impl_->col_header_size); - break; - case INT64: - col_field_impl( - col_buf, dst, f.offset, f.mask, f.shift, pixels_per_column, - dst_stride, impl_->channel_data_size, impl_->col_header_size); - break; - case FLOAT32: - col_field_impl( - col_buf, dst, f.offset, f.mask, f.shift, pixels_per_column, - dst_stride, impl_->channel_data_size, impl_->col_header_size); - break; - case FLOAT64: - col_field_impl( - col_buf, dst, f.offset, f.mask, f.shift, pixels_per_column, - dst_stride, impl_->channel_data_size, impl_->col_header_size); - break; - default: - throw std::invalid_argument("Invalid field for packet format"); - } -} - // explicitly instantiate for each field type template void packet_format::col_field(const uint8_t*, const std::string&, uint8_t*, int) const; @@ -610,116 +631,41 @@ packet_format::FieldIter packet_format::end() const { /* Packet headers */ uint16_t packet_format::packet_type(const uint8_t* lidar_buf) const { - if (udp_profile_lidar == UDPProfileLidar::PROFILE_LIDAR_LEGACY) { - // LEGACY profile has no packet_type - use 0 to code as 'legacy' - return 0; - } - uint16_t res = 0; - if (udp_profile_lidar == - UDPProfileLidar::PROFILE_FUSA_RNG15_RFL8_NIR8_DUAL) { - // FuSa profile has 8-bit packet_type - std::memcpy(&res, lidar_buf + 0, sizeof(uint8_t)); - } else { - std::memcpy(&res, lidar_buf + 0, sizeof(uint16_t)); - } - return res; + return impl_->packet_type_info.get(lidar_buf); } uint32_t packet_format::frame_id(const uint8_t* lidar_buf) const { - if (udp_profile_lidar == UDPProfileLidar::PROFILE_LIDAR_LEGACY) { - uint16_t res = 0; - std::memcpy(&res, nth_col(0, lidar_buf) + 10, sizeof(uint16_t)); - return res; - } - - // eUDP frame id is 16 bits, but FUSA frame id is 32 bits - if (udp_profile_lidar == - UDPProfileLidar::PROFILE_FUSA_RNG15_RFL8_NIR8_DUAL) { - uint32_t res = 0; - std::memcpy(&res, lidar_buf + 4, sizeof(res)); - return res; - } else { - uint16_t res = 0; - std::memcpy(&res, lidar_buf + 2, sizeof(res)); - return res; - } + return impl_->frame_id_info.get(lidar_buf); } uint32_t packet_format::init_id(const uint8_t* lidar_buf) const { - if (udp_profile_lidar == UDPProfileLidar::PROFILE_LIDAR_LEGACY) { - // LEGACY profile has no init_id - use 0 to code as 'legacy' - return 0; - } - uint32_t res = 0; - if (udp_profile_lidar == - UDPProfileLidar::PROFILE_FUSA_RNG15_RFL8_NIR8_DUAL) { - std::memcpy(&res, lidar_buf + 1, sizeof(uint32_t)); - } else { - std::memcpy(&res, lidar_buf + 4, sizeof(uint32_t)); - } - return res & 0x00ffffff; + return impl_->init_id_info.get(lidar_buf); } uint64_t packet_format::prod_sn(const uint8_t* lidar_buf) const { - if (udp_profile_lidar == UDPProfileLidar::PROFILE_LIDAR_LEGACY) { - // LEGACY profile has no prod_sn (serial number) - use 0 to code as - // 'legacy' - return 0; - } - uint64_t res = 0; - if (udp_profile_lidar == - UDPProfileLidar::PROFILE_FUSA_RNG15_RFL8_NIR8_DUAL) { - std::memcpy(&res, lidar_buf + 11, sizeof(uint64_t)); - } else { - std::memcpy(&res, lidar_buf + 7, sizeof(uint64_t)); - } - return res & 0x000000ffffffffff; + return impl_->prod_sn_info.get(lidar_buf); +} + +uint8_t packet_format::alert_flags(const uint8_t* lidar_buf) const { + return impl_->alert_flags_info.get(lidar_buf); } uint16_t packet_format::countdown_thermal_shutdown( const uint8_t* lidar_buf) const { - if (udp_profile_lidar == UDPProfileLidar::PROFILE_LIDAR_LEGACY) { - // LEGACY profile has no shutdown counter in packet header - use 0 for - // 'normal operation' - return 0; - } - uint16_t res = 0; - std::memcpy(&res, lidar_buf + 16, sizeof(uint8_t)); - return res; + return impl_->countdown_thermal_shutdown_info.get(lidar_buf); } uint16_t packet_format::countdown_shot_limiting( const uint8_t* lidar_buf) const { - if (udp_profile_lidar == UDPProfileLidar::PROFILE_LIDAR_LEGACY) { - // LEGACY profile has no shot limiting countdown in packet header - use - // 0 for 'normal operation' - return 0; - } - uint16_t res = 0; - std::memcpy(&res, lidar_buf + 17, sizeof(uint8_t)); - return res; + return impl_->countdown_shot_limiting_info.get(lidar_buf); } uint8_t packet_format::thermal_shutdown(const uint8_t* lidar_buf) const { - if (udp_profile_lidar == UDPProfileLidar::PROFILE_LIDAR_LEGACY) { - // LEGACY profile has no shutdown status in packet header - use 0 for - // 'normal operation' - return 0; - } - uint8_t res = 0; - std::memcpy(&res, lidar_buf + 18, sizeof(uint8_t)); - return res & 0x0f; + return impl_->thermal_shutdown_info.get(lidar_buf); } uint8_t packet_format::shot_limiting(const uint8_t* lidar_buf) const { - if (udp_profile_lidar == UDPProfileLidar::PROFILE_LIDAR_LEGACY) { - // LEGACY profile has no shot limiting in packet header - use 0 for - // 'normal operation' - return 0; - } - uint8_t res = 0; - std::memcpy(&res, lidar_buf + 19, sizeof(uint8_t)); - return res & 0x0f; + return impl_->shot_limiting_info.get(lidar_buf); } const uint8_t* packet_format::footer(const uint8_t* lidar_buf) const { @@ -735,25 +681,15 @@ const uint8_t* packet_format::nth_col(int n, const uint8_t* lidar_buf) const { } uint32_t packet_format::col_status(const uint8_t* col_buf) const { - uint32_t res = 0; - std::memcpy(&res, col_buf + impl_->status_offset, sizeof(uint32_t)); - if (udp_profile_lidar == UDPProfileLidar::PROFILE_LIDAR_LEGACY) { - return res; // LEGACY was 32 bits of all 1s - } else { - return res & 0xffff; // For eUDP packets, we want the last 16 bits - } + return impl_->col_status_info.get(col_buf); } uint64_t packet_format::col_timestamp(const uint8_t* col_buf) const { - uint64_t res = 0; - std::memcpy(&res, col_buf + impl_->timestamp_offset, sizeof(uint64_t)); - return res; + return impl_->col_timestamp_info.get(col_buf); } uint16_t packet_format::col_measurement_id(const uint8_t* col_buf) const { - uint16_t res = 0; - std::memcpy(&res, col_buf + impl_->measurement_id_offset, sizeof(uint16_t)); - return res; + return impl_->col_measurement_id_info.get(col_buf); } uint32_t packet_format::col_encoder(const uint8_t* col_buf) const { @@ -782,21 +718,6 @@ const uint8_t* packet_format::nth_px(int n, const uint8_t* col_buf) const { return col_buf + impl_->col_header_size + (n * impl_->channel_data_size); } -template -T packet_format::px_field(const uint8_t* px_buf, const std::string& i) const { - const auto& f = impl_->fields.at(i); - - if (sizeof(T) < field_type_size(f.ty_tag)) - throw std::invalid_argument("Dest type too small for specified field"); - - T res = 0; - std::memcpy(&res, px_buf + f.offset, field_type_size(f.ty_tag)); - if (f.mask) res &= f.mask; - if (f.shift > 0) res >>= f.shift; - if (f.shift < 0) res <<= std::abs(f.shift); - return res; -} - /* IMU packet parsing */ uint64_t packet_format::imu_sys_ts(const uint8_t* imu_buf) const { @@ -912,209 +833,69 @@ uint8_t* packet_writer::footer(uint8_t* lidar_buf) const { } void packet_writer::set_col_status(uint8_t* col_buf, uint32_t status) const { - if (udp_profile_lidar == UDPProfileLidar::PROFILE_LIDAR_LEGACY) { - std::memcpy(col_buf + impl_->status_offset, &status, sizeof(uint32_t)); - } else { - uint16_t s = status & 0xffff; - std::memcpy(col_buf + impl_->status_offset, &s, sizeof(uint16_t)); - } + impl_->col_status_info.set(col_buf, status); } void packet_writer::set_col_timestamp(uint8_t* col_buf, uint64_t ts) const { - std::memcpy(col_buf + impl_->timestamp_offset, &ts, sizeof(ts)); + impl_->col_timestamp_info.set(col_buf, ts); } void packet_writer::set_col_measurement_id(uint8_t* col_buf, uint16_t m_id) const { - std::memcpy(col_buf + impl_->measurement_id_offset, &m_id, sizeof(m_id)); + impl_->col_measurement_id_info.set(col_buf, m_id); } void packet_writer::set_frame_id(uint8_t* lidar_buf, uint32_t frame_id) const { - // eUDP frame id is 16 bits, but FUSA frame id is 32 bits - if (udp_profile_lidar == - UDPProfileLidar::PROFILE_FUSA_RNG15_RFL8_NIR8_DUAL) { - std::memcpy(lidar_buf + 4, &frame_id, sizeof(frame_id)); - return; - } - - uint16_t f_id = static_cast(frame_id); - if (udp_profile_lidar == UDPProfileLidar::PROFILE_LIDAR_LEGACY) { - std::memcpy(nth_col(0, lidar_buf) + 10, &f_id, sizeof(f_id)); - return; - } - - std::memcpy(lidar_buf + 2, &f_id, sizeof(f_id)); + impl_->frame_id_info.set(lidar_buf, frame_id); } -// Helpers for weird sized ints -// TODO: generalise when/if we need other uintXX_t fractionals -class uint24_t { - protected: - uint8_t _internal[3]; - - public: - uint24_t() {} - - uint24_t(const uint32_t val) { *this = val; } - - uint24_t(const uint24_t& val) { *this = val; } - - operator uint32_t() const { - return (_internal[2] << 16) | (_internal[1] << 8) | (_internal[0] << 0); - } - - uint24_t& operator=(const uint24_t& input) { - _internal[0] = input._internal[0]; - _internal[1] = input._internal[1]; - _internal[2] = input._internal[2]; - - return *this; - } - - uint24_t& operator=(const uint32_t input) { - _internal[0] = ((unsigned char*)&input)[0]; - _internal[1] = ((unsigned char*)&input)[1]; - _internal[2] = ((unsigned char*)&input)[2]; - - return *this; - } -}; - -class uint40_t { - protected: - uint8_t _internal[5]; - - public: - uint40_t() {} - - uint40_t(const uint64_t val) { *this = val; } - - uint40_t(const uint40_t& val) { *this = val; } - - operator uint64_t() const { - return (((uint64_t)_internal[4]) << 32) | - (((uint64_t)_internal[3]) << 24) | - (((uint64_t)_internal[2]) << 16) | - (((uint64_t)_internal[1]) << 8) | - (((uint64_t)_internal[0]) << 0); - } - - uint40_t& operator=(const uint40_t& input) { - _internal[0] = input._internal[0]; - _internal[1] = input._internal[1]; - _internal[2] = input._internal[2]; - _internal[3] = input._internal[3]; - _internal[4] = input._internal[4]; - - return *this; - } - - uint40_t& operator=(const uint64_t input) { - _internal[0] = ((unsigned char*)&input)[0]; - _internal[1] = ((unsigned char*)&input)[1]; - _internal[2] = ((unsigned char*)&input)[2]; - _internal[3] = ((unsigned char*)&input)[3]; - _internal[4] = ((unsigned char*)&input)[4]; +void packet_writer::set_init_id(uint8_t* lidar_buf, uint32_t init_id) const { + impl_->init_id_info.set(lidar_buf, init_id); +} - return *this; - } -}; +void packet_writer::set_packet_type(uint8_t* lidar_buf, + uint16_t packet_type) const { + impl_->packet_type_info.set(lidar_buf, packet_type); +} -#pragma pack(push, 1) -// Relevant parts of packet headers as described/named in sensor documentation -struct FUSAHeader { - uint8_t packet_type; - uint24_t init_id; - uint32_t frame_id; - uint24_t padding; - uint40_t serial_no; -}; +void packet_writer::set_prod_sn(uint8_t* lidar_buf, uint64_t sn) const { + impl_->prod_sn_info.set(lidar_buf, sn); +} -struct ConfigurableHeader { - uint16_t packet_type; - uint16_t frame_id; - uint24_t init_id; - uint40_t serial_no; -}; -#pragma pack(pop) +void packet_writer::set_alert_flags(uint8_t* lidar_buf, + uint8_t alert_flags) const { + impl_->alert_flags_info.set(lidar_buf, alert_flags); +} -void packet_writer::set_init_id(uint8_t* lidar_buf, uint32_t init_id) const { - if (udp_profile_lidar == UDPProfileLidar::PROFILE_LIDAR_LEGACY) { - // LEGACY profile has no init_id - return; - } - if (udp_profile_lidar == - UDPProfileLidar::PROFILE_FUSA_RNG15_RFL8_NIR8_DUAL) { - auto hdr = (FUSAHeader*)lidar_buf; - hdr->init_id = init_id; - } else { - auto hdr = (ConfigurableHeader*)lidar_buf; - hdr->init_id = init_id; - } +void packet_writer::set_shutdown(uint8_t* lidar_buf, uint8_t status) const { + impl_->thermal_shutdown_info.set(lidar_buf, status); } -void packet_writer::set_prod_sn(uint8_t* lidar_buf, uint64_t sn) const { - if (udp_profile_lidar == UDPProfileLidar::PROFILE_LIDAR_LEGACY) { - // LEGACY profile has no prod_sn - return; - } - if (udp_profile_lidar == - UDPProfileLidar::PROFILE_FUSA_RNG15_RFL8_NIR8_DUAL) { - auto hdr = (FUSAHeader*)lidar_buf; - hdr->serial_no = sn; - } else { - auto hdr = (ConfigurableHeader*)lidar_buf; - hdr->serial_no = sn; - } +void packet_writer::set_shot_limiting(uint8_t* lidar_buf, + uint8_t status) const { + impl_->shot_limiting_info.set(lidar_buf, status); } -template -void packet_writer::set_px(uint8_t* px_buf, const std::string& i, - T value) const { - const auto& f = impl_->fields.at(i); +void packet_writer::set_shutdown_countdown(uint8_t* lidar_buf, + uint8_t shutdown_countdown) const { + impl_->countdown_thermal_shutdown_info.set(lidar_buf, shutdown_countdown); +} - typename SameSizeInt::value int_value; - memcpy(&int_value, &value, sizeof(T)); - if (f.shift > 0) int_value <<= f.shift; - if (f.shift < 0) int_value >>= std::abs(f.shift); - if (f.mask) int_value &= f.mask; - auto ptr = - reinterpret_cast::value*>(px_buf + f.offset); - *ptr &= ~f.mask; - *ptr |= int_value; +void packet_writer::set_shot_limiting_countdown( + uint8_t* lidar_buf, uint8_t shot_limiting_countdown) const { + impl_->countdown_shot_limiting_info.set(lidar_buf, shot_limiting_countdown); } -template void packet_writer::set_px(uint8_t*, const std::string&, - uint8_t) const; -template void packet_writer::set_px(uint8_t*, const std::string&, - uint16_t) const; -template void packet_writer::set_px(uint8_t*, const std::string&, - uint32_t) const; -template void packet_writer::set_px(uint8_t*, const std::string&, - uint64_t) const; -template void packet_writer::set_px(uint8_t*, const std::string&, int8_t) const; -template void packet_writer::set_px(uint8_t*, const std::string&, - int16_t) const; -template void packet_writer::set_px(uint8_t*, const std::string&, - int32_t) const; -template void packet_writer::set_px(uint8_t*, const std::string&, - int64_t) const; -template void packet_writer::set_px(uint8_t*, const std::string&, float) const; -template void packet_writer::set_px(uint8_t*, const std::string&, double) const; - -template -void packet_writer::set_block_impl(Eigen::Ref> field, - const std::string& chan, - uint8_t* lidar_buf) const { +template +void packet_writer::set_block(Eigen::Ref> field, + const std::string& chan, + uint8_t* lidar_buf) const { constexpr int N = 32; if (columns_per_packet > N) throw std::runtime_error("Recompile set_block_impl with larger N"); - const auto& f = impl_->fields.at(chan); + impl::FieldInfo f = impl_->fields.at(chan); - size_t offset = f.offset; - uint64_t mask = f.mask; - int shift = f.shift; size_t channel_data_size = impl_->channel_data_size; int cols = field.cols(); @@ -1135,59 +916,11 @@ void packet_writer::set_block_impl(Eigen::Ref> field, auto px_dst = col_buf[x] + col_header_size + (px * channel_data_size); - uint64_t value = *(data + f_offset + x); - if (shift > 0) value <<= shift; - if (shift < 0) value >>= std::abs(shift); - if (mask) value &= mask; - DST* ptr = reinterpret_cast::value*>( - px_dst + offset); - *ptr &= ~mask; - *ptr |= value; + f.set(px_dst, *(data + f_offset + x)); } } } -template -void packet_writer::set_block(Eigen::Ref> field, - const std::string& i, uint8_t* lidar_buf) const { - const auto& f = impl_->fields.at(i); - - switch (f.ty_tag) { - case UINT8: - set_block_impl(field, i, lidar_buf); - break; - case UINT16: - set_block_impl(field, i, lidar_buf); - break; - case UINT32: - set_block_impl(field, i, lidar_buf); - break; - case UINT64: - set_block_impl(field, i, lidar_buf); - break; - case INT8: - set_block_impl(field, i, lidar_buf); - break; - case INT16: - set_block_impl(field, i, lidar_buf); - break; - case INT32: - set_block_impl(field, i, lidar_buf); - break; - case INT64: - set_block_impl(field, i, lidar_buf); - break; - case FLOAT32: - set_block_impl(field, i, lidar_buf); - break; - case FLOAT64: - set_block_impl(field, i, lidar_buf); - break; - default: - throw std::invalid_argument("Invalid field for packet format"); - } -} - template void packet_writer::set_block(Eigen::Ref> field, const std::string& i, uint8_t* lidar_buf) const; @@ -1283,6 +1016,49 @@ template void packet_writer::unpack_raw_headers( template void packet_writer::unpack_raw_headers( Eigen::Ref> field, uint8_t* lidar_buf) const; +static std::array crc64_init(void) { + // Generate LUT of all possible 8-bit CRCs to speed up CRC calculation + // This is for the ECMA-182 CRC64 implementation used on the sensor. + constexpr uint64_t poly = 0xC96C5795D7870F42; + std::array arr = {0}; + for (uint32_t i = 0; i < 256; ++i) { + uint64_t r = i; + for (uint32_t j = 0; j < 8; ++j) { + r = (r >> 1) ^ (poly & ~((r & 1) - 1)); + } + arr[i] = r; + } + + return arr; +} + +static std::array crc64_table = crc64_init(); + +uint64_t crc64_compute(const uint8_t* buf, size_t len) { + uint64_t crc = ~0; + // Use Sarwate algorithm LSB-first to calculate the CRC using the LUT. + while (len != 0) { + crc = crc64_table[*buf++ ^ (crc & 0xFF)] ^ (crc >> 8); + --len; + } + + return ~crc; +} } // namespace impl + +optional packet_format::crc(const uint8_t* lidar_buf) const { + if (udp_profile_lidar == UDPProfileLidar::PROFILE_LIDAR_LEGACY || + udp_profile_lidar == + UDPProfileLidar::PROFILE_FUSA_RNG15_RFL8_NIR8_DUAL) { + return optional(); + } + + return *(uint64_t*)&lidar_buf[lidar_packet_size - 8]; +} + +uint64_t packet_format::calculate_crc(const uint8_t* lidar_buf) const { + return impl::crc64_compute(lidar_buf, lidar_packet_size - 8); +} + } // namespace sensor } // namespace ouster diff --git a/ouster_client/src/profile_extension.cpp b/ouster_client/src/profile_extension.cpp index ec9b5137..07c5df69 100644 --- a/ouster_client/src/profile_extension.cpp +++ b/ouster_client/src/profile_extension.cpp @@ -138,7 +138,11 @@ void add_custom_profile( udp_profile, name, {}, {}, chan_data_size}; for (auto&& pair : fields) { profile.slots.emplace_back(pair.first, pair.second.ty_tag); - profile.fields.emplace_back(pair.first, pair.second); + ouster::sensor::impl::FieldInfo field = pair.second; + if (field.mask == 0) { + field.mask = sensor::field_type_mask(field.ty_tag); + } + profile.fields.emplace_back(pair.first, field); } impl::extended_profiles_data.push_back(std::move(profile)); diff --git a/ouster_client/src/sensor_http.cpp b/ouster_client/src/sensor_http.cpp index 0685b3a8..fbe406c8 100644 --- a/ouster_client/src/sensor_http.cpp +++ b/ouster_client/src/sensor_http.cpp @@ -1,5 +1,7 @@ #include "ouster/sensor_http.h" +#include + #include "curl_client.h" #include "sensor_http_imp.h" #include "sensor_tcp_imp.h" @@ -14,7 +16,18 @@ using namespace ouster::sensor::impl; string SensorHttp::firmware_version_string(const string& hostname, int timeout_sec) { auto http_client = std::make_unique("http://" + hostname); - return http_client->get("api/v1/system/firmware", timeout_sec); + auto fwjson = http_client->get("api/v1/system/firmware", timeout_sec); + + Json::Value root{}; + Json::CharReaderBuilder builder{}; + std::string errors{}; + std::stringstream ss{fwjson}; + + if (!Json::parseFromStream(builder, ss, &root, &errors)) + throw std::runtime_error{ + "Errors parsing firmware for firmware_version_string: " + errors}; + + return root["fw"].asString(); } version SensorHttp::firmware_version(const string& hostname, int timeout_sec) { @@ -35,19 +48,37 @@ std::unique_ptr SensorHttp::create(const string& hostname, if (fw.major == 2) { switch (fw.minor) { - case 0: + case 0: { // FW 2.0 doesn't work properly with http - return std::make_unique(hostname); - case 1: - return std::make_unique(hostname); - case 2: - case 3: - return std::make_unique(hostname); + auto instance = std::make_unique(hostname); + instance->version_ = fw; + instance->hostname_ = hostname; + return instance; + } + case 1: { + auto instance = std::make_unique(hostname); + instance->version_ = fw; + instance->hostname_ = hostname; + return instance; + } + case 2: { + auto instance = std::make_unique(hostname); + instance->version_ = fw; + instance->hostname_ = hostname; + return instance; + } } } - if ((fw.major == 2 && fw.minor == 4) || (fw.major == 3 && fw.minor == 0)) { - return std::make_unique(hostname); + if ((fw.major == 2 && (fw.minor == 4 || fw.minor == 3)) || + (fw.major == 3 && fw.minor == 0)) { + auto instance = std::make_unique(hostname); + instance->version_ = fw; + instance->hostname_ = hostname; + return instance; } - return std::make_unique(hostname); + auto instance = std::make_unique(hostname); + instance->version_ = fw; + instance->hostname_ = hostname; + return instance; } diff --git a/ouster_client/src/sensor_info.cpp b/ouster_client/src/sensor_info.cpp index 30e2c45b..bb4c0740 100644 --- a/ouster_client/src/sensor_info.cpp +++ b/ouster_client/src/sensor_info.cpp @@ -9,7 +9,6 @@ #include #include #include -#include #include #include #include @@ -18,12 +17,25 @@ #include "ouster/impl/build.h" #include "ouster/impl/logging.h" +#include "ouster/metadata.h" #include "ouster/types.h" #include "ouster/util.h" #include "ouster/version.h" namespace ouster { +/** + * Parse and validate a metadata stream. + * + * @param[in] json_data The metadata data. + * @param[out] sensor_info The sensor_info to populate. + * @param[out] issues The issues that occured during parsing. + * @return if there are any critical issues or not. + */ +extern bool parse_and_validate_metadata( + const std::string& json_data, ouster::sensor::sensor_info& sensor_info, + ValidatorIssues& issues); + using nonstd::make_optional; using nonstd::nullopt; using nonstd::optional; @@ -71,6 +83,14 @@ bool sensor_info::has_fields_equal(const sensor_info& other) const { this->config == other.config && this->user_data == other.user_data); } +auto sensor_info::w() const -> decltype(format.columns_per_frame) { + return format.columns_per_frame; +} + +auto sensor_info::h() const -> decltype(format.pixels_per_column) { + return format.pixels_per_column; +} + /* Default values */ sensor_info default_sensor_info(lidar_mode mode) { @@ -155,363 +175,35 @@ const std::map nonlegacy_metadata_fields = { // clang-format on -static bool is_new_format(const Json::Value& root) { - size_t nonlegacy_fields_present = 0; - std::string missing_fields = ""; - for (const auto& field_pair : nonlegacy_metadata_fields) { - auto field = field_pair.first; - auto is_obj = field_pair.second; - if (root.isMember(field)) { - nonlegacy_fields_present++; - if (is_obj && !root[field].isObject()) { - throw std::runtime_error{"Non-legacy metadata field " + field + - " must have child fields"}; - } - } else { - missing_fields += field + " "; - } - } - - if (nonlegacy_fields_present > 0 && - nonlegacy_fields_present < nonlegacy_metadata_fields.size()) { - throw std::runtime_error{"Non-legacy metadata must include fields: " + - missing_fields}; - } - - return nonlegacy_fields_present == nonlegacy_metadata_fields.size(); -} - -static void parse_metadata(sensor_info& info, const Json::Value& root, - bool skip_beam_validation) { - const std::vector minimum_metadata_fields{"config_params", - "beam_intrinsics"}; - for (auto field : minimum_metadata_fields) { - if (!root.isMember(field)) { - throw std::runtime_error{"Metadata must contain: " + field}; - } - } - - // nice to have fields which we will use defaults for if they don't exist - const std::vector desired_metadata_fields{"imu_intrinsics", - "lidar_intrinsics"}; - for (auto field : desired_metadata_fields) { - if (!root.isMember(field)) { - logger().warn("No " + field + - " found in metadata. Will be left blank or filled in " - "with default legacy values"); - } - } - - // if these are not present they are also empty strings - auto sensor_info = root["sensor_info"]; - info.build_date = sensor_info["build_date"].asString(); - info.fw_rev = sensor_info["build_rev"].asString(); - info.image_rev = sensor_info["image_rev"].asString(); - info.prod_line = sensor_info["prod_line"].asString(); - info.prod_pn = sensor_info["prod_pn"].asString(); - info.sn = sensor_info["prod_sn"].asString(); - info.status = sensor_info["status"].asString(); - - // default to 0 if init_id key not present - info.init_id = sensor_info["initialization_id"].asInt(); - - // checked that lidar_mode is present already - never empty string - auto mode = - lidar_mode_of_string(root["config_params"]["lidar_mode"].asString()); - - // "data_format" introduced in fw 2.0. Fall back to 1.13 - if (root.isMember("lidar_data_format") && - root["lidar_data_format"].isObject()) { - info.format = parse_data_format(root["lidar_data_format"]); - // data_format.fps was added for DF sensors, so we are backfilling - // fps value for OS sensors here if it's not present in metadata - if (info.format.fps == 0) { - info.format.fps = frequency_of_lidar_mode(mode); - } - } else { - logger().warn( - "No lidar_data_format found. Using default legacy data format"); - info.format = default_data_format(mode); - } - - // "lidar_origin_to_beam_origin_mm" introduced in fw 2.0 BUT missing - // on OS-DOME. Handle falling back to FW 1.13 or setting to 0 - // according to prod-line - auto beam_intrinsics = root["beam_intrinsics"]; - if (beam_intrinsics.isMember("lidar_origin_to_beam_origin_mm")) { - info.lidar_origin_to_beam_origin_mm = - beam_intrinsics["lidar_origin_to_beam_origin_mm"].asDouble(); - } else { - if (info.prod_line.find("OS-DOME-") == - 0) { // is an OS-DOME - fill with 0 - info.lidar_origin_to_beam_origin_mm = 0; - } else { // not an OS-DOME - logger().warn( - "No lidar_origin_to_beam_origin_mm found. Using default " - "value for the specified prod_line or default gen 1 values " - "if prod_line is missing"); - info.lidar_origin_to_beam_origin_mm = - default_lidar_origin_to_beam_origin( - info.prod_line); // note it is possible that - // info.prod_line is "" - } - } - - // beam_to_lidar_transform" introduced in fw 2.5/fw 3.0 - if (beam_intrinsics.isMember("beam_to_lidar_transform")) { - for (int i = 0; i < 4; i++) { - for (int j = 0; j < 4; j++) { - const Json::Value::ArrayIndex ind = i * 4 + j; - info.beam_to_lidar_transform(i, j) = - beam_intrinsics["beam_to_lidar_transform"][ind].asDouble(); - } - } - } else { - // fw is < 2.5/3.0 and we need to manually fill it in - info.beam_to_lidar_transform = mat4d::Identity(); - info.beam_to_lidar_transform(0, 3) = - info.lidar_origin_to_beam_origin_mm; - } - - if (beam_intrinsics["beam_altitude_angles"].size() != 0 && - beam_intrinsics["beam_altitude_angles"].size() != - info.format.pixels_per_column) - throw std::runtime_error{"Unexpected number of beam_altitude_angles"}; - - if (beam_intrinsics["beam_azimuth_angles"].size() != 0 && - beam_intrinsics["beam_azimuth_angles"].size() != - info.format.pixels_per_column) - throw std::runtime_error{"Unexpected number of beam_azimuth_angles"}; - - if (beam_intrinsics["beam_altitude_angles"].size() == - info.format.pixels_per_column) { - if (beam_intrinsics["beam_altitude_angles"][0].isArray()) { - // DF sensor path - for (const auto& row : beam_intrinsics["beam_altitude_angles"]) - for (const auto& v : row) - info.beam_altitude_angles.push_back(v.asDouble()); - - if (info.beam_altitude_angles.size() != - info.format.pixels_per_column * info.format.columns_per_frame) { - throw std::runtime_error{ - "Unexpected number of total beam_altitude_angles"}; - } - } else { - // OS sensor path - for (const auto& v : beam_intrinsics["beam_altitude_angles"]) - info.beam_altitude_angles.push_back(v.asDouble()); - } - } - - if (beam_intrinsics["beam_azimuth_angles"].size() == - info.format.pixels_per_column) { - if (beam_intrinsics["beam_azimuth_angles"][0].isArray()) { - // DF sensor path - for (const auto& row : beam_intrinsics["beam_azimuth_angles"]) { - for (const auto& v : row) - info.beam_azimuth_angles.push_back(v.asDouble()); - } - - if (info.beam_azimuth_angles.size() != - info.format.pixels_per_column * info.format.columns_per_frame) { - throw std::runtime_error{ - "Unexpected number of total beam_azimuth_angles"}; - } - } else { - // OS sensor path - for (const auto& v : beam_intrinsics["beam_azimuth_angles"]) - info.beam_azimuth_angles.push_back(v.asDouble()); - } - } - - // "imu_to_sensor_transform" may be absent in sensor config - // produced by Ouster Studio, so we backfill it with default value - auto imu_intrinsics = root["imu_intrinsics"]; - if (imu_intrinsics["imu_to_sensor_transform"].size() == 16) { - for (int i = 0; i < 4; i++) { - for (int j = 0; j < 4; j++) { - const Json::Value::ArrayIndex ind = i * 4 + j; - info.imu_to_sensor_transform(i, j) = - imu_intrinsics["imu_to_sensor_transform"][ind].asDouble(); - } - } - } else { - logger().warn( - "No valid imu_to_sensor_transform found. Using default for gen " - "1"); - info.imu_to_sensor_transform = default_imu_to_sensor_transform; - } - - // "lidar_to_sensor_transform" may be absent in sensor config - // produced by Ouster Studio, so we backfill it with default value - auto lidar_intrinsics = root["lidar_intrinsics"]; - if (lidar_intrinsics["lidar_to_sensor_transform"].size() == 16) { - for (int i = 0; i < 4; i++) { - for (int j = 0; j < 4; j++) { - const Json::Value::ArrayIndex ind = i * 4 + j; - info.lidar_to_sensor_transform(i, j) = - lidar_intrinsics["lidar_to_sensor_transform"][ind] - .asDouble(); - } - } - } else { - logger().warn( - "No valid lidar_to_sensor_transform found. Using default for " - "gen " - "1"); - info.lidar_to_sensor_transform = default_lidar_to_sensor_transform; - } - - auto zero_check = [](auto el, std::string name) { - if (el.size() == 0) return; - bool all_zeros = std::all_of(el.cbegin(), el.cend(), - [](double k) { return k == 0.0; }); - if (all_zeros) { - throw std::runtime_error{"Field " + name + - " in the metadata cannot all be zeros."}; - } - }; - - 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(); - - if (root.isMember("ouster-sdk")) { - auto sdk_group = root["ouster-sdk"]; - if (sdk_group["extrinsic"].size() == 16) { - for (int i = 0; i < 4; i++) { - for (int j = 0; j < 4; j++) { - const Json::Value::ArrayIndex ind = i * 4 + j; - info.extrinsic(i, j) = - sdk_group["extrinsic"][ind].asDouble(); - } - } - } else { - logger().info("No valid extrinsics found. Using identity."); - } - } - - // we are guaranteed calibration_status as a key exists so don't need to - // check again - if (root["calibration_status"].isObject()) { - if (root["calibration_status"]["reflectivity"]["valid"].isBool()) { - info.cal.reflectivity_status = - root["calibration_status"]["reflectivity"]["valid"].asBool(); - } else { - logger().warn( - "metadata field calibration_status.reflectivity.valid is " - "not Bool value, but: {}. Using False instead.", - root["calibration_status"]["reflectivity"]["valid"].asString()); - } - - if (info.cal.reflectivity_status) { - info.cal.reflectivity_timestamp = - root["calibration_status"]["reflectivity"]["timestamp"] - .asString(); - } - } - - info.config = parse_config(root["config_params"]); - info.user_data = root["user_data"].asString(); -} - -static void parse_legacy(sensor_info& info, const Json::Value& root, - bool skip_beam_validation) { - // just convert to non-legacy and run the non-legacy parse - const std::vector config_fields{ - "udp_port_imu", - "udp_port_lidar", - "lidar_mode", - }; - - const std::vector beam_intrinsics_fields{ - "lidar_origin_to_beam_origin_mm", "beam_altitude_angles", - "beam_azimuth_angles", "beam_to_lidar_transform"}; - - const std::vector sensor_info_fields{ - "prod_line", "status", "prod_pn", "prod_sn", - "initialization_id", "build_rev", "build_date", "image_rev", - }; - - // Error if we dont have required fields - const std::vector minimum_metadata_fields{"lidar_mode"}; - - for (auto field : minimum_metadata_fields) { - if (!root.isMember(field)) { - throw std::runtime_error{"Metadata must contain: " + field}; - } - } - - Json::Value result; - if (root.isMember("lidar_to_sensor_transform")) { - result["lidar_intrinsics"]["lidar_to_sensor_transform"] = - root["lidar_to_sensor_transform"]; - } - if (root.isMember("imu_to_sensor_transform")) { - result["imu_intrinsics"]["imu_to_sensor_transform"] = - root["imu_to_sensor_transform"]; - } - if (root.isMember("data_format")) { - result["lidar_data_format"] = root["data_format"]; - } - if (root.isMember("client_version")) { - result["ouster-sdk"]["client_version"] = root["client_version"]; - } - for (const auto& field : config_fields) { - if (root.isMember(field)) { - result["config_params"][field] = root[field]; - } - } - - for (const auto& field : beam_intrinsics_fields) { - if (root.isMember(field)) { - result["beam_intrinsics"][field] = root[field]; - } - } - - for (const auto& field : sensor_info_fields) { - if (root.isMember(field)) { - result["sensor_info"][field] = root[field]; - } - } - - parse_metadata(info, result, skip_beam_validation); -} - sensor_info::sensor_info() { // TODO - understand why this seg faults in CI when uncommented // logger().warn("Initializing sensor_info without original metadata // string"); } -sensor_info::sensor_info(const std::string& metadata, - bool skip_beam_validation) { +sensor_info::sensor_info(const std::string& metadata) { Json::Value root{}; Json::CharReaderBuilder builder{}; - std::string errors{}; - std::stringstream ss{metadata}; - - if (metadata.size()) { - if (!Json::parseFromStream(builder, ss, &root, &errors)) - throw std::runtime_error{"Errors parsing metadata string: " + - errors}; - } - - if (is_new_format(root)) { - was_legacy_ = false; - logger().info("parsing non-legacy metadata format"); - parse_metadata(*this, root, skip_beam_validation); + ValidatorIssues issues; + + if (metadata.size() > 0) { + parse_and_validate_metadata(metadata, *this, issues); + if (issues.critical.size() > 0) { + std::stringstream error_string; + error_string << "ERROR: Critical Metadata Issues Exist: " + << std::endl; + for (auto it : issues.critical) { + error_string << it.to_string() << std::endl; + } + throw std::runtime_error(error_string.str()); + } } else { - was_legacy_ = true; - logger().info("parsing legacy metadata format"); - parse_legacy(*this, root, skip_beam_validation); + throw std::runtime_error("ERROR: empty metadata passed in"); } } +sensor_info::sensor_info(const std::string& metadata, + bool /*skip_beam_validation*/) + : sensor_info(metadata) {} void mat4d_to_json(Json::Value& val, mat4d mat) { for (size_t i = 0; i < 4; i++) { @@ -636,7 +328,7 @@ product_info sensor_info::get_product_info() const { } sensor_info metadata_from_json(const std::string& json_file, - bool skip_beam_validation) { + bool /*skip_beam_validation*/) { std::stringstream buf{}; std::ifstream ifs{}; ifs.open(json_file); @@ -649,7 +341,7 @@ sensor_info metadata_from_json(const std::string& json_file, throw std::runtime_error{ss.str()}; } - return sensor_info(buf.str(), skip_beam_validation); + return sensor_info(buf.str()); } std::string to_string(const sensor_info& info) { return info.to_json_string(); } diff --git a/ouster_client/src/types.cpp b/ouster_client/src/types.cpp index c42eb09e..10bdcfe6 100644 --- a/ouster_client/src/types.cpp +++ b/ouster_client/src/types.cpp @@ -73,16 +73,16 @@ extern const Table polarity_strings{ extern const Table nmea_baud_rate_strings{ {{BAUD_9600, "BAUD_9600"}, {BAUD_115200, "BAUD_115200"}}}; -// clang-format off -Table udp_profile_lidar_strings{{ - {PROFILE_LIDAR_UNKNOWN, "UNKNOWN"}, - {PROFILE_LIDAR_LEGACY, "LEGACY"}, - {PROFILE_RNG19_RFL8_SIG16_NIR16_DUAL, "RNG19_RFL8_SIG16_NIR16_DUAL"}, - {PROFILE_RNG19_RFL8_SIG16_NIR16, "RNG19_RFL8_SIG16_NIR16"}, - {PROFILE_RNG15_RFL8_NIR8, "RNG15_RFL8_NIR8"}, - {PROFILE_FIVE_WORD_PIXEL, "FIVE_WORD_PIXEL"}, - {PROFILE_FUSA_RNG15_RFL8_NIR8_DUAL, "FUSA_RNG15_RFL8_NIR8_DUAL"}, -}}; +Table udp_profile_lidar_strings{ + { + {PROFILE_LIDAR_UNKNOWN, "UNKNOWN"}, + {PROFILE_LIDAR_LEGACY, "LEGACY"}, + {PROFILE_RNG19_RFL8_SIG16_NIR16_DUAL, "RNG19_RFL8_SIG16_NIR16_DUAL"}, + {PROFILE_RNG19_RFL8_SIG16_NIR16, "RNG19_RFL8_SIG16_NIR16"}, + {PROFILE_RNG15_RFL8_NIR8, "RNG15_RFL8_NIR8"}, + {PROFILE_FIVE_WORD_PIXEL, "FIVE_WORD_PIXEL"}, + {PROFILE_FUSA_RNG15_RFL8_NIR8_DUAL, "FUSA_RNG15_RFL8_NIR8_DUAL"}, + }}; Table udp_profile_imu_strings{{ {PROFILE_IMU_LEGACY, "LEGACY"}, @@ -140,12 +140,12 @@ bool operator!=(const data_format& lhs, const data_format& rhs) { return !(lhs == rhs); } -bool operator ==(const calibration_status& lhs, const calibration_status& rhs) { +bool operator==(const calibration_status& lhs, const calibration_status& rhs) { return (lhs.reflectivity_status == rhs.reflectivity_status && lhs.reflectivity_timestamp == rhs.reflectivity_timestamp); } -bool operator !=(const calibration_status& lhs, const calibration_status& rhs) { +bool operator!=(const calibration_status& lhs, const calibration_status& rhs) { return !(lhs == rhs); } @@ -173,8 +173,7 @@ bool operator==(const sensor_config& lhs, const sensor_config& rhs) { lhs.columns_per_packet == rhs.columns_per_packet && lhs.udp_profile_lidar == rhs.udp_profile_lidar && lhs.udp_profile_imu == rhs.udp_profile_imu && - lhs.gyro_fsr == rhs.gyro_fsr && - lhs.accel_fsr == rhs.accel_fsr && + lhs.gyro_fsr == rhs.gyro_fsr && lhs.accel_fsr == rhs.accel_fsr && lhs.return_order == rhs.return_order && lhs.min_range_threshold_cm == rhs.min_range_threshold_cm); } @@ -212,8 +211,12 @@ data_format default_data_format(lidar_mode mode) { case 2048: offset = repeat(16, {36, 24, 12, 0}); break; + case 4096: + offset = repeat(16, {72, 48, 24, 0}); + break; default: throw std::invalid_argument{"default_data_format"}; + break; } return {pixels_per_column, @@ -244,10 +247,7 @@ mat4d default_beam_to_lidar_transform(std::string prod_line) { return beam_to_lidar_transform; } -calibration_status default_calibration_status() { - return calibration_status{}; -} - +calibration_status default_calibration_status() { return calibration_status{}; } /* Misc operations */ @@ -304,10 +304,10 @@ template static optional rlookup(const impl::Table& table, const char* v) { auto end = table.end(); - auto res = std::find_if(table.begin(), end, - [&](const std::pair& p) { - return p.second && std::strcmp(p.second, v) == 0; - }); + auto res = std::find_if( + table.begin(), end, [&](const std::pair& p) { + return p.second && std::strcmp(p.second, v) == 0; + }); return res == end ? nullopt : make_optional(res->first); } @@ -433,6 +433,22 @@ size_t field_type_size(ChanFieldType ft) { } } +uint64_t field_type_mask(ChanFieldType ft) { + switch (field_type_size(ft)) { + case 1: + return 0xff; + case 2: + return 0xffff; + case 4: + return 0xffffffff; + case 8: + return 0xffffffffffffffff; + default: + throw std::runtime_error( + "field_type_mask error: wrong ChanFieldType"); + } +} + std::string to_string(UDPProfileLidar profile) { auto res = lookup(impl::udp_profile_lidar_strings, profile); return res ? res.value() : "UNKNOWN"; @@ -463,14 +479,12 @@ std::string to_string(ThermalShutdownStatus thermal_shutdown_status) { } std::string to_string(ReturnOrder return_order) { - auto res = - lookup(impl::return_order_strings, return_order); + auto res = lookup(impl::return_order_strings, return_order); return res ? res.value() : "UNKNOWN"; } std::string to_string(FullScaleRange full_scale_range) { - auto res = - lookup(impl::full_scale_range_strings, full_scale_range); + auto res = lookup(impl::full_scale_range_strings, full_scale_range); return res ? res.value() : "UNKNOWN"; } @@ -572,6 +586,8 @@ Json::Value cal_to_json(const calibration_status& cal) { if (cal.reflectivity_status) { root["reflectivity"]["valid"] = cal.reflectivity_status.value(); + } + if (cal.reflectivity_timestamp) { root["reflectivity"]["timestamp"] = cal.reflectivity_timestamp.value(); } @@ -720,8 +736,7 @@ sensor_config parse_config(const Json::Value& root) { // Firmware 3.1 and higher options if (!root["gyro_fsr"].empty()) { - auto gyro_fsr = - full_scale_range_of_string(root["gyro_fsr"].asString()); + auto gyro_fsr = full_scale_range_of_string(root["gyro_fsr"].asString()); if (gyro_fsr) { config.gyro_fsr = gyro_fsr; } else { @@ -773,14 +788,12 @@ sensor_config parse_config(const std::string& config) { Json::Value config_to_json(const sensor_config& config) { Json::Value root{Json::objectValue}; - if (config.udp_dest) - root["udp_dest"] = config.udp_dest.value(); + if (config.udp_dest) root["udp_dest"] = config.udp_dest.value(); if (config.udp_port_lidar) root["udp_port_lidar"] = config.udp_port_lidar.value(); - if (config.udp_port_imu) - root["udp_port_imu"] = config.udp_port_imu.value(); + if (config.udp_port_imu) root["udp_port_imu"] = config.udp_port_imu.value(); if (config.timestamp_mode) root["timestamp_mode"] = to_string(config.timestamp_mode.value()); @@ -866,17 +879,16 @@ Json::Value config_to_json(const sensor_config& config) { root["udp_profile_imu"] = to_string(config.udp_profile_imu.value()); // Firmware 3.1 and higher options - if (config.gyro_fsr) - root["gyro_fsr"] = to_string(config.gyro_fsr.value()); + if (config.gyro_fsr) root["gyro_fsr"] = to_string(config.gyro_fsr.value()); if (config.accel_fsr) - root["accel_fsr"] = to_string(config.accel_fsr.value()); + root["accel_fsr"] = to_string(config.accel_fsr.value()); if (config.min_range_threshold_cm) - root["min_range_threshold_cm"] = config.min_range_threshold_cm.value(); + root["min_range_threshold_cm"] = config.min_range_threshold_cm.value(); if (config.return_order) - root["return_order"] = to_string(config.return_order.value()); + root["return_order"] = to_string(config.return_order.value()); return root; } @@ -891,87 +903,105 @@ std::string to_string(const sensor_config& config) { return Json::writeString(builder, root); } -PacketValidationFailure LidarPacket::validate(const sensor_info& info, - const packet_format& format) { - if (buf.size() != format.lidar_packet_size) { - return PacketValidationFailure::PACKET_SIZE; +PacketValidationFailure validate_packet(const sensor_info& info, + const packet_format& format, + const uint8_t* buf, uint64_t buf_size, + PacketValidationType type) { + // Check if we need to guess the type + if (type == PacketValidationType::GUESS_TYPE) { + if (buf_size == format.imu_packet_size) { + type = PacketValidationType::IMU; + } else { + type = PacketValidationType::LIDAR; + } } - auto init_id = format.init_id(buf.data()); - if (info.init_id != 0 && init_id != 0 && init_id != info.init_id) { - return PacketValidationFailure::ID; - } + if (type == PacketValidationType::LIDAR) { + if (buf_size != format.lidar_packet_size) { + return PacketValidationFailure::PACKET_SIZE; + } - if (info.sn.length() > 0) { - auto p_sn = format.prod_sn(buf.data()); - auto m_sn = std::stoull(info.sn); - if (p_sn != 0 && p_sn != m_sn) { + auto init_id = format.init_id(buf); + if (info.init_id != 0 && init_id != 0 && init_id != info.init_id) { return PacketValidationFailure::ID; } + + if (info.sn.length() > 0) { + uint64_t p_sn = format.prod_sn(buf); + uint64_t m_sn = std::stoull(info.sn); + if ((p_sn != 0) && (p_sn != m_sn)) { + return PacketValidationFailure::ID; + } + } + return PacketValidationFailure::NONE; + } else if (type == PacketValidationType::IMU) { + if (buf_size != format.imu_packet_size) { + return PacketValidationFailure::PACKET_SIZE; + } + return PacketValidationFailure::NONE; } return PacketValidationFailure::NONE; +}; + +PacketValidationFailure LidarPacket::validate(const sensor_info& info, + const packet_format& format) { + return validate_packet(info, format, buf.data(), buf.size(), + PacketValidationType::LIDAR); } -PacketValidationFailure ImuPacket::validate(const sensor_info& /*info*/, +PacketValidationFailure ImuPacket::validate(const sensor_info& info, const packet_format& format) { - if (buf.size() != format.imu_packet_size) { - return PacketValidationFailure::PACKET_SIZE; - } - return PacketValidationFailure::NONE; + return validate_packet(info, format, buf.data(), buf.size(), + PacketValidationType::IMU); } product_info product_info::create_product_info( std::string product_info_string) { - std::regex product_regex("^(\\w+)-(\\d+|DOME)?(?:-(\\d+))?(?:-((?!SR)\\w+))?-?(SR)?"); + std::regex product_regex( + "^(\\w+)-(\\d+|DOME)?(?:-(\\d+))?(?:-((?!SR)\\w+))?-?(SR)?"); std::smatch matches; - if(product_info_string.length() > 0) { + if (product_info_string.length() > 0) { if (regex_search(product_info_string, matches, product_regex) == true) { std::string form_factor = matches.str(1) + matches.str(2); bool short_range = (matches.str(5).length() > 0); auto beam_config = matches.str(4); - if(beam_config.length() <= 0) { + if (beam_config.length() <= 0) { beam_config = "U"; } int beam_count; try { beam_count = stoi(matches.str(3)); - } - catch(const std::exception &e) { + } catch (const std::exception& e) { beam_count = 0; } - return product_info(product_info_string, - form_factor, - short_range, - beam_config, - beam_count); + return product_info(product_info_string, form_factor, short_range, + beam_config, beam_count); } else { - throw std::runtime_error("Product Info \"" + product_info_string + "\" is not a recognized product info"); + throw std::runtime_error("Product Info \"" + product_info_string + + "\" is not a recognized product info"); } } return product_info(); } -product_info::product_info() : product_info("", "", false, "", 0) {}; +product_info::product_info() : product_info("", "", false, "", 0){}; product_info::product_info(std::string product_info_string, - std::string form_factor, - bool short_range, - std::string beam_config, - int beam_count) : - full_product_info(product_info_string), - form_factor(form_factor), - short_range(short_range), - beam_config(beam_config), - beam_count(beam_count) { -} + std::string form_factor, bool short_range, + std::string beam_config, int beam_count) + : full_product_info(product_info_string), + form_factor(form_factor), + short_range(short_range), + beam_config(beam_config), + beam_count(beam_count) {} bool operator==(const product_info& lhs, const product_info& rhs) { return lhs.full_product_info == rhs.full_product_info && - lhs.form_factor == rhs.form_factor && - lhs.short_range == rhs.short_range && - lhs.beam_config == rhs.beam_config && - lhs.beam_count == rhs.beam_count; + lhs.form_factor == rhs.form_factor && + lhs.short_range == rhs.short_range && + lhs.beam_config == rhs.beam_config && + lhs.beam_count == rhs.beam_count; } bool operator!=(const product_info& lhs, const product_info& rhs) { @@ -981,7 +1011,8 @@ bool operator!=(const product_info& lhs, const product_info& rhs) { std::string to_string(const product_info& info) { std::stringstream output; output << "Product Info: " << std::endl; - output << "\tFull Product Info: \"" << info.full_product_info << "\"" << std::endl; + output << "\tFull Product Info: \"" << info.full_product_info << "\"" + << std::endl; output << "\tForm Factor: \"" << info.form_factor << "\"" << std::endl; output << "\tShort Range: \"" << info.short_range << "\"" << std::endl; output << "\tBeam Config: \"" << info.beam_config << "\"" << std::endl; @@ -993,7 +1024,8 @@ std::string to_string(const product_info& info) { namespace util { version version_from_string(const std::string& v) { - auto rgx = std::regex(R"((([\w\d]*)-([\w\d]*)-)?v?(\d*)\.(\d*)\.(\d*)-?([\d\w.]*)?\+?([\d\w.]*)?)"); + auto rgx = std::regex( + R"((([\w\d]*)-([\w\d]*)-)?v?(\d*)\.(\d*)\.(\d*)-?([\d\w.]*)?\+?([\d\w.]*)?)"); std::smatch matches; std::regex_search(v, matches, rgx); diff --git a/ouster_client/src/udp_packet_source.cpp b/ouster_client/src/udp_packet_source.cpp deleted file mode 100644 index 95ab0659..00000000 --- a/ouster_client/src/udp_packet_source.cpp +++ /dev/null @@ -1,326 +0,0 @@ -/** - * Copyright (c) 2021, Ouster, Inc. - * All rights reserved. - */ - -#include "ouster/udp_packet_source.h" - -#include -#include -#include -#include -#include - -#include "ouster/client.h" -#include "ouster/impl/client_poller.h" -#include "ouster/impl/logging.h" -#include "ouster/types.h" - -namespace ouster { -namespace sensor { -namespace impl { - -std::string to_string(client_state st) { - switch (static_cast(st)) { - case client_state::TIMEOUT: - return "TIMEOUT"; - case client_state::CLIENT_ERROR: - return "CLIENT_ERROR"; - case client_state::LIDAR_DATA: - return "LIDAR_DATA"; - case client_state::IMU_DATA: - return "IMU_DATA"; - case client_state::EXIT: - return "EXIT"; - case Producer::CLIENT_OVERFLOW: - return "OVERFLOW"; - default: - return "UNKNOWN_EVENT"; - } -} - -std::string to_string(Event e) { - return std::string("{") + std::to_string(e.source) + ", " + - to_string(e.state) + "}"; -} - -int Producer::add_client(std::shared_ptr cli, size_t lidar_buf_size, - size_t lidar_packet_size, size_t imu_buf_size, - size_t imu_packet_size) { - std::unique_lock lock{mtx_, std::defer_lock}; - if (!lock.try_lock()) - throw std::runtime_error("add_client called on a running producer"); - - if (!cli) throw std::runtime_error("add_client called with nullptr"); - - int id = clients_.size(); - clients_.push_back(cli); - rb_->allocate({id, client_state::LIDAR_DATA}, lidar_buf_size, - Packet(lidar_packet_size)); - rb_->allocate({id, client_state::IMU_DATA}, imu_buf_size, - Packet(imu_packet_size)); - return id; -} - -int Producer::add_client(std::shared_ptr cli, const sensor_info& info, - float seconds_to_buffer) { - const data_format& df = info.format; - uint32_t packets_per_frame = df.columns_per_frame / df.columns_per_packet; - float lidar_hz = static_cast(packets_per_frame) * df.fps; - float imu_hz = 100.f; - const packet_format& pf = get_format(info); - return add_client(cli, static_cast(lidar_hz * seconds_to_buffer), - pf.lidar_packet_size, - static_cast(imu_hz * seconds_to_buffer), - pf.imu_packet_size); -} - -std::shared_ptr Producer::subscribe( - std::shared_ptr pub) { - std::unique_lock lock{mtx_, std::defer_lock}; - if (!lock.try_lock()) - throw std::runtime_error("subscribe called on a running producer"); - - pubs_.push_back(pub); - return std::make_shared(pub->queue(), rb_); -} - -std::shared_ptr Producer::subscribe(EventSet events) { - auto pub = std::make_shared(events); - return subscribe(pub); -} - -bool Producer::_verify() const { - if (clients_.size() == 0) { - logger().error("Producer started with no clients"); - return false; - } - - if (pubs_.size() == 0) { - logger().error("Producer started with no publishers"); - return false; - } - - bool out = true; - - Event last_chk; - - auto n_pubs_accept = [this, &last_chk](Event e) { - last_chk = e; - auto l = [e](int total, auto& pub) { - return total + static_cast(pub->accepts(e)); - }; - return std::accumulate(pubs_.begin(), pubs_.end(), 0, l); - }; - - if (n_pubs_accept({-1, client_state::CLIENT_ERROR}) == 0) { - logger().error("Producer: none of the publishers accept {}", - to_string(last_chk)); - out = false; - } - - if (n_pubs_accept({-1, client_state::EXIT}) == 0) { - logger().error("Producer: none of the publishers accept {}", - to_string(last_chk)); - out = false; - } - - for (int i = 0, end = clients_.size(); i < end; ++i) { - if (n_pubs_accept({i, client_state::LIDAR_DATA}) != 1) { - logger().error( - "Producer: {} publishers accept {}, needs to be exactly one", - n_pubs_accept(last_chk), to_string(last_chk)); - out = false; - } - - if (n_pubs_accept({i, client_state::IMU_DATA}) != 1) { - logger().error( - "Producer: {} publishers accept {}, needs to be exactly one", - n_pubs_accept(last_chk), to_string(last_chk)); - out = false; - } - - if (n_pubs_accept({i, client_state(Producer::CLIENT_OVERFLOW)}) == 0) { - logger().error("Producer: no publishers accept {}", - to_string(last_chk)); - } - } - return out; -} - -static bool read_packet(const client& cli, Packet& packet, client_state st) { - switch (st) { - case client_state::LIDAR_DATA: - return read_lidar_packet(cli, packet.as()); - case client_state::IMU_DATA: - return read_imu_packet(cli, packet.as()); - default: - return false; - } -} - -static client_state operator&(client_state a, client_state b) { - int a_i = static_cast(a); - int b_i = static_cast(b); - return static_cast(a_i & b_i); -} - -void Producer::run() { - // check publisher/client consistency - if (!_verify()) return; - - std::vector overflows(clients_.size(), false); - - // this could be a private virtual instead - auto handle_event = [this, &overflows](Event e) { - const client_state overflow = client_state(Producer::CLIENT_OVERFLOW); - switch (e.state) { - case 0: - break; - case client_state::CLIENT_ERROR: - case client_state::EXIT: - for (auto& pub : pubs_) pub->publish(e); - break; - case client_state::LIDAR_DATA: - case client_state::IMU_DATA: - if (rb_->full(e)) { - if (!overflows[e.source]) { - overflows[e.source] = true; - for (auto& pub : pubs_) { - // publish with priority - pub->publish({e.source, overflow}, true); - } - } - } else if (read_packet(*clients_[e.source], rb_->back(e), - e.state)) { - rb_->push(e); - for (auto& pub : pubs_) pub->publish(e); - overflows[e.source] = false; - } - break; - default: - break; - } - }; - - std::lock_guard lock{mtx_}; - - std::shared_ptr poller = make_poller(); - while (!stop_) { - reset_poll(*poller); - - for (auto& cli : clients_) set_poll(*poller, *cli); - - int res = poll(*poller); - - if (res == 0) { // TIMEOUT - continue; - } else if (res < 0) { // CLIENT_ERROR / EXIT - client_state st = get_error(*poller); - handle_event({-1, st & client_state::CLIENT_ERROR}); - handle_event({-1, st & client_state::EXIT}); - break; - } else { - for (int i = 0, end = clients_.size(); i < end; ++i) { - client_state st = get_poll(*poller, *clients_[i]); - handle_event({i, st & client_state::LIDAR_DATA}); - handle_event({i, st & client_state::IMU_DATA}); - } - } - } -} - -/* - * Producer will release mtx_ only when it exits the loop. - */ -void Producer::shutdown() { - stop_ = true; - for (auto& pub : pubs_) pub->publish({-1, client_state::EXIT}); - - std::lock_guard lock{mtx_}; - // close UDP sockets when any producer has exited - clients_.clear(); - pubs_.clear(); - rb_.reset(new RingBufferMap()); - stop_ = false; -} - -UDPPacketSource::UDPPacketSource() - : Producer(), - Subscriber(std::move(*Producer::subscribe( - {{-1, client_state::CLIENT_ERROR}, {-1, client_state::EXIT}}))) {} - -void UDPPacketSource::_accept_client_events(int id) { - pubs_[0]->set_accept({id, client_state::LIDAR_DATA}); - pubs_[0]->set_accept({id, client_state::IMU_DATA}); - pubs_[0]->set_accept({id, client_state(Producer::CLIENT_OVERFLOW)}); -} - -void UDPPacketSource::add_client(std::shared_ptr cli, - size_t lidar_buf_size, - size_t lidar_packet_size, size_t imu_buf_size, - size_t imu_packet_size) { - _accept_client_events(Producer::add_client( - cli, lidar_buf_size, lidar_packet_size, imu_buf_size, imu_packet_size)); -} -void UDPPacketSource::add_client(std::shared_ptr cli, - const sensor_info& info, - float seconds_to_buffer) { - _accept_client_events(Producer::add_client(cli, info, seconds_to_buffer)); -} - -BufferedUDPSource::BufferedUDPSource() - : Producer(), - Subscriber(std::move(*Producer::subscribe( - {{-1, client_state::CLIENT_ERROR}, - {-1, client_state::EXIT}, - {0, client_state::LIDAR_DATA}, - {0, client_state::IMU_DATA}, - {0, client_state(Producer::CLIENT_OVERFLOW)}}))) {} - -BufferedUDPSource::BufferedUDPSource(std::shared_ptr client, - size_t lidar_buf_size, - size_t lidar_packet_size, - size_t imu_buf_size, - size_t imu_packet_size) - : BufferedUDPSource() { - Producer::add_client(client, lidar_buf_size, lidar_packet_size, - imu_buf_size, imu_packet_size); -} - -BufferedUDPSource::BufferedUDPSource(std::shared_ptr client, - const sensor_info& info, - float seconds_to_buffer) - : BufferedUDPSource() { - Producer::add_client(client, info, seconds_to_buffer); -} - -client_state BufferedUDPSource::consume(LidarPacket& lidarp, ImuPacket& imup, - float timeout_sec) { - Event e = Subscriber::pop(timeout_sec); - client_state st = e.state; - - // return early without advancing queue - if (!Subscriber::_has_packet(e)) return st; - - Packet& p = Subscriber::packet(e); - - auto write_packet = [&p](auto& packet) { - auto sz = std::min(packet.buf.size(), p.buf.size()); - std::memcpy(packet.buf.data(), p.buf.data(), sz); - packet.host_timestamp = p.host_timestamp; - }; - - if (st & client_state::LIDAR_DATA) { - write_packet(lidarp); - } else if (st & client_state::IMU_DATA) { - write_packet(imup); - } - - Subscriber::advance(e); - return st; -} - -} // namespace impl -} // namespace sensor -} // namespace ouster diff --git a/ouster_osf/fb/os_sensor/lidar_scan_stream.fbs b/ouster_osf/fb/os_sensor/lidar_scan_stream.fbs index f72197b3..9af29c49 100644 --- a/ouster_osf/fb/os_sensor/lidar_scan_stream.fbs +++ b/ouster_osf/fb/os_sensor/lidar_scan_stream.fbs @@ -67,6 +67,11 @@ table LidarScanMsg { // extra fields support for extensible lidar scan custom_fields:[Field]; + + frame_status: uint64; + shutdown_countdown: uint8; + shot_limiting_countdown: uint8; + alert_flags:[uint8]; } // Scan data from a lidar sensor. One scan is a sweep of a sensor (360 degree). diff --git a/ouster_osf/fb/streaming/streaming_info.fbs b/ouster_osf/fb/streaming/streaming_info.fbs index 5d1c0798..24b8f979 100644 --- a/ouster_osf/fb/streaming/streaming_info.fbs +++ b/ouster_osf/fb/streaming/streaming_info.fbs @@ -14,6 +14,10 @@ table StreamStats { // avg size of the messages in bytes for a `stream_id` in the whole // OSF file message_avg_size:uint32; + // Receive timestamps for index, in saved order + receive_timestamps:[uint64]; + // Sensor timestamps for index, in saved order + sensor_timestamps:[uint64]; } table ChunkInfo { @@ -38,4 +42,4 @@ table StreamingInfo { } // MetadataEntry.type: ouster/v1/streaming/StreamingInfo -root_type StreamingInfo; \ No newline at end of file +root_type StreamingInfo; diff --git a/ouster_osf/include/ouster/osf/basics.h b/ouster_osf/include/ouster/osf/basics.h index d90e05d8..b3a8205a 100644 --- a/ouster_osf/include/ouster/osf/basics.h +++ b/ouster_osf/include/ouster/osf/basics.h @@ -45,7 +45,8 @@ enum OSF_VERSION { ///< 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 + V_2_0 = 20, ///< Second Generation OSF v2 + V_2_1 = 21 ///< Add full index and addtional info to LidarScans }; /** diff --git a/ouster_osf/include/ouster/osf/file.h b/ouster_osf/include/ouster/osf/file.h index 67b112e7..ca6aab21 100644 --- a/ouster_osf/include/ouster/osf/file.h +++ b/ouster_osf/include/ouster/osf/file.h @@ -227,6 +227,8 @@ class OsfFile { * Move policy: * Allow transferring ownership of the underlying file * handler (mmap). + * + * @param[in] other The OSF file to move */ OsfFile(OsfFile&& other); diff --git a/ouster_osf/include/ouster/osf/layout_streaming.h b/ouster_osf/include/ouster/osf/layout_streaming.h index 904f6506..9555c871 100644 --- a/ouster_osf/include/ouster/osf/layout_streaming.h +++ b/ouster_osf/include/ouster/osf/layout_streaming.h @@ -53,7 +53,8 @@ class StreamingLayoutCW : public ChunksWriter { * * @throws std::logic_error Exception on inconsistent timestamps. */ - void save_message(const uint32_t stream_id, const ts_t ts, + void save_message(const uint32_t stream_id, const ts_t receive_ts, + const ts_t sensor_ts, const std::vector& buf) override; /** @@ -72,10 +73,12 @@ class StreamingLayoutCW : public ChunksWriter { * for a specific set of new messages. * * @param[in] stream_id The stream id to associate with the message. - * @param[in] ts The timestamp for the messages. + * @param[in] receive_ts The receive timestamp for the messages. + * @param[in] sensor_ts The sensor timestamp for the messages. * @param[in] msg_buf A vector of message buffers to gather stats about. */ - void stats_message(const uint32_t stream_id, const ts_t ts, + void stats_message(const uint32_t stream_id, const ts_t receive_ts, + const ts_t sensor_ts, const std::vector& msg_buf); /** diff --git a/ouster_osf/include/ouster/osf/meta_streaming_info.h b/ouster_osf/include/ouster/osf/meta_streaming_info.h index cb92cd84..fe651718 100644 --- a/ouster_osf/include/ouster/osf/meta_streaming_info.h +++ b/ouster_osf/include/ouster/osf/meta_streaming_info.h @@ -97,6 +97,22 @@ struct StreamStats { */ uint32_t message_avg_size; + /** + * The receive timestamps of each message in the stream. + * + * Flat Buffer Reference: + * fb/streaming/streaming_info.fbs :: StreamStats :: receive_timestamps + */ + std::vector receive_timestamps; + + /** + * The sensor timestamps of each message in the stream. + * + * Flat Buffer Reference: + * fb/streaming/streaming_info.fbs :: StreamStats :: sensor_timestamps + */ + std::vector sensor_timestamps; + /** * Default constructor, sets everthing to 0. */ @@ -106,24 +122,30 @@ struct StreamStats { * Construct a StreamStats with the specified values * * @param[in] s_id Specify the stream_id to use. - * @param[in] t Set the start and end timestamps to the specified value. + * @param[in] receive_ts Set the start and end timestamps to the specified + * value and add it to the receive timestamps. + * @param[in] sensor_ts Add to the sensor timestamps. * @param[in] msg_size Set the average message size to the specified value. */ - StreamStats(uint32_t s_id, ts_t t, uint32_t msg_size); + StreamStats(uint32_t s_id, ts_t receive_ts, ts_t sensor_ts, + uint32_t msg_size); /** * Update values within the StreamStats * - * @param[in] t Add another timestamp and calculate the start and end - * values. + * @param[in] receive_ts Add another receive timestamp and calculate the + * start and end values. + * @param[in] sensor_ts Add another sensor timestamp * @param[in] msg_size Add another message size and calculate the average. */ - void update(ts_t t, uint32_t msg_size); + void update(ts_t receive_ts, ts_t sensor_ts, uint32_t msg_size); }; /** * Get the string representation for a ChunkInfo object. * + * @param[in] chunk_info ChunkInfo object to be converted to string + * * @return The string representation for a ChunkInfo object. */ std::string to_string(const ChunkInfo& chunk_info); @@ -131,6 +153,8 @@ std::string to_string(const ChunkInfo& chunk_info); /** * Get the string representation for a StreamStats object. * + * @param[in] stream_stats StreamStats object to be converted to string. + * * @return The string representation for a StreamStats object. */ std::string to_string(const StreamStats& stream_stats); diff --git a/ouster_osf/include/ouster/osf/metadata.h b/ouster_osf/include/ouster/osf/metadata.h index 45999b39..1f9a35f3 100644 --- a/ouster_osf/include/ouster/osf/metadata.h +++ b/ouster_osf/include/ouster/osf/metadata.h @@ -49,6 +49,8 @@ struct MetadataTraits { * Helper function that returns the MetadataEntry type of concrete metadata. * * @tparam MetadataDerived The derived subclass cpp type. + * + * @return metadata type */ template inline const std::string metadata_type() { @@ -537,6 +539,8 @@ class MetadataStore { * Add a specified MetadataEntry to the store * * @param[in] entry The entry to add to the store. + * + * @return The metadata id of the entry added. */ uint32_t add(MetadataEntry&& entry); diff --git a/ouster_osf/include/ouster/osf/operations.h b/ouster_osf/include/ouster/osf/operations.h index 14030b48..12468567 100644 --- a/ouster_osf/include/ouster/osf/operations.h +++ b/ouster_osf/include/ouster/osf/operations.h @@ -14,7 +14,6 @@ #include "ouster/osf/metadata.h" #include "ouster/types.h" -/// @todo fix parameter directions in api doc namespace ouster { namespace osf { diff --git a/ouster_osf/include/ouster/osf/reader.h b/ouster_osf/include/ouster/osf/reader.h index 2fcfaf88..0d9bcfa2 100644 --- a/ouster_osf/include/ouster/osf/reader.h +++ b/ouster_osf/include/ouster/osf/reader.h @@ -555,6 +555,14 @@ class Reader { */ bool has_message_idx() const; + /** + * Whether OSF contains the message timestamp index in the metadata + * necessary to quickly collate and jump to a specific message time." + * + * @return Whether OSF contains the message timestamp index + */ + bool has_timestamp_idx() const; + /** * 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 @@ -763,6 +771,18 @@ class MessageRef { return Stream::decode_msg(buffer(), *meta, meta_provider_); } + template + std::unique_ptr decode_msg(T& t) 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_, t); + } + /** * Get the underlying raw message byte vector. * @@ -921,6 +941,7 @@ class ChunkRef { * A shortcut for state()->start_ts * * @relates state + * @return starting timestamp in the received chunk */ ts_t start_ts() const; @@ -929,6 +950,7 @@ class ChunkRef { * A shortcut for state()->end_ts * * @relates state + * @return last timestamp in the received chunk */ ts_t end_ts() const; diff --git a/ouster_osf/include/ouster/osf/stream_lidar_scan.h b/ouster_osf/include/ouster/osf/stream_lidar_scan.h index c06eae86..9d101a41 100644 --- a/ouster_osf/include/ouster/osf/stream_lidar_scan.h +++ b/ouster_osf/include/ouster/osf/stream_lidar_scan.h @@ -146,10 +146,12 @@ class LidarScanStream : public MessageStream { * we also might want to have the corresponding function to read back * sequentially from Stream that doesn't seem like fit into this model... * - * @param[in] ts The timestamp to use for the lidar scan. + * @param[in] receive_ts The receive timestamp to use for the lidar scan. + * @param[in] sensor_ts The sensor timestamp to use for the lidar scan. * @param[in] lidar_scan The lidar scan to write. */ - void save(const ouster::osf::ts_t ts, const obj_type& lidar_scan); + void save(const ouster::osf::ts_t receive_ts, + const ouster::osf::ts_t sensor_ts, const obj_type& lidar_scan); /** * Encode/serialize the object to the buffer of bytes. @@ -168,11 +170,14 @@ class LidarScanStream : public MessageStream { * @param[in] meta_provider Used to reconstruct any references to other * metadata entries dependencies * (like sensor_meta_id) + * @param[in] fields List of fields to decode. All are decoded if none + * provided. * @return Pointer to the decoded object. */ static std::unique_ptr decode_msg( const std::vector& buf, const meta_type& meta, - const MetadataStore& meta_provider); + const MetadataStore& meta_provider, + const std::vector& fields = {}); public: /** diff --git a/ouster_osf/include/ouster/osf/writer.h b/ouster_osf/include/ouster/osf/writer.h index 7addcb93..d400cca6 100644 --- a/ouster_osf/include/ouster/osf/writer.h +++ b/ouster_osf/include/ouster/osf/writer.h @@ -27,10 +27,12 @@ class ChunksWriter { * Save a message to a specified stream. * * @param[in] stream_id The stream id to associate with the message. - * @param[in] ts The timestamp for the messages. + * @param[in] receive_ts The receive timestamp for the messages. + * @param[in] sensor_ts The sensor timestamp for the messages. * @param[in] buf A vector of message buffers to record. */ - virtual void save_message(const uint32_t stream_id, const ts_t ts, + virtual void save_message(const uint32_t stream_id, const ts_t receive_ts, + const ts_t sensor_ts, const std::vector& buf) = 0; /** @@ -115,6 +117,8 @@ class Writer { * @tparam MetaParams The type of meta parameters to add. * * @param[in] params The parameters to add. + * + * @return The corresponding lidar id of the metadata entry. */ template uint32_t add_metadata(MetaParams&&... params) { @@ -126,6 +130,8 @@ class Writer { * Adds a MetadataEntry to the OSF file. * * @param[in] entry The metadata entry to add to the OSF file. + * + * @return The corresponding lidar id of the metadata entry */ uint32_t add_metadata(MetadataEntry&& entry); @@ -167,6 +173,8 @@ class Writer { * @tparam StreamParams The specified stream parameter types. * * @param[in] params The parameters to use when creating a stream. + * + * @return Stream object created. */ template Stream create_stream(StreamParams&&... params) { @@ -182,11 +190,12 @@ class Writer { * @throws std::logic_error Exception on non existent stream id. * * @param[in] stream_id The stream to save the message to. - * @param[in] ts The timestamp to use for the message. + * @param[in] receive_ts The receive timestamp to use for the message. + * @param[in] sensor_ts The sensor timestamp to use for the message. * @param[in] buf The message to save in the form of a byte vector. */ - void save_message(const uint32_t stream_id, const ts_t ts, - const std::vector& buf); + void save_message(const uint32_t stream_id, const ts_t receive_ts, + const ts_t sensor_ts, const std::vector& buf); /** * Adds info about a sensor to the OSF and returns the stream index to @@ -251,7 +260,7 @@ class Writer { * @param[in] stream_index The index of the corrosponding sensor_info to * use. * @param[in] scan The scan to save. - * @param[in] timestamp Timestamp to index this scan with. + * @param[in] timestamp Receive timestamp to index this scan with. */ void save(uint32_t stream_index, const LidarScan& scan, const ouster::osf::ts_t timestamp); @@ -567,10 +576,12 @@ class ChunkBuilder { * @throws std::logic_error Exception on a size mismatch * * @param[in] stream_id The stream to save the message to. - * @param[in] ts The timestamp to use for the message. + * @param[in] receive_ts The receive timestamp to use for the message. + * @param[in] sensor_ts The sensor timestamp to use for the message. * @param[in] msg_buf The message to save in the form of a byte vector. */ - void save_message(const uint32_t stream_id, const ts_t ts, + void save_message(const uint32_t stream_id, const ts_t receive_ts, + const ts_t sensor_ts, const std::vector& msg_buf); /** @@ -602,17 +613,21 @@ class ChunkBuilder { /** * The lowest timestamp in the chunk. + * + * @return The lowest timestamp in the chunk. */ ts_t start_ts() const; /** * The highest timestamp in the chunk. + * + * @return The highest timestamp in the chunk. */ ts_t end_ts() const; private: /** - * Internal method for updating the corret start and end + * Internal method for updating the correct start and end * timestamps. * * @param[in] ts The timestamp to check against for start and end. diff --git a/ouster_osf/src/fb_utils.cpp b/ouster_osf/src/fb_utils.cpp index 5e5f0642..4e4a3432 100644 --- a/ouster_osf/src/fb_utils.cpp +++ b/ouster_osf/src/fb_utils.cpp @@ -87,7 +87,7 @@ uint64_t builder_to_file(flatbuffers::FlatBufferBuilder& builder, 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, + header_fbb, ouster::osf::OSF_VERSION::V_2_1, ouster::osf::HEADER_STATUS::INVALID, 0, 0); header_fbb.FinishSizePrefixed(header, ouster::osf::gen::HeaderIdentifier()); return builder_to_file(header_fbb, filename, false); @@ -98,7 +98,7 @@ uint64_t finish_osf_file(const std::string& filename, 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, + header_fbb, ouster::osf::OSF_VERSION::V_2_1, ouster::osf::HEADER_STATUS::VALID, metadata_offset, metadata_offset + metadata_size); header_fbb.FinishSizePrefixed(header, ouster::osf::gen::HeaderIdentifier()); diff --git a/ouster_osf/src/layout_streaming.cpp b/ouster_osf/src/layout_streaming.cpp index d4ba8caa..4fc5ad25 100644 --- a/ouster_osf/src/layout_streaming.cpp +++ b/ouster_osf/src/layout_streaming.cpp @@ -17,7 +17,9 @@ StreamingLayoutCW::StreamingLayoutCW(Writer& writer, uint32_t chunk_size) : chunk_size_{chunk_size ? chunk_size : STREAMING_DEFAULT_CHUNK_SIZE}, writer_{writer} {} -void StreamingLayoutCW::save_message(const uint32_t stream_id, const ts_t ts, +void StreamingLayoutCW::save_message(const uint32_t stream_id, + const ts_t receive_ts, + const ts_t sensor_ts, const std::vector& msg_buf) { if (!chunk_builders_.count(stream_id)) { chunk_builders_.insert({stream_id, std::make_shared()}); @@ -26,10 +28,10 @@ void StreamingLayoutCW::save_message(const uint32_t stream_id, const ts_t ts, auto chunk_builder = chunk_builders_[stream_id]; // checking non-decreasing invariant of chunks and messages - if (chunk_builder->end_ts() > ts) { + if (chunk_builder->end_ts() > receive_ts) { std::stringstream err; - err << "ERROR: Can't write with a decreasing timestamp: " << ts.count() - << " for stream_id: " << stream_id + err << "ERROR: Can't write with a decreasing timestamp: " + << receive_ts.count() << " for stream_id: " << stream_id << " ( previous recorded timestamp: " << chunk_builder->end_ts().count() << ")"; throw std::logic_error(err.str()); @@ -39,10 +41,10 @@ void StreamingLayoutCW::save_message(const uint32_t stream_id, const ts_t ts, finish_chunk(stream_id, chunk_builder); } - chunk_builder->save_message(stream_id, ts, msg_buf); + chunk_builder->save_message(stream_id, receive_ts, sensor_ts, msg_buf); // update running statistics per stream - stats_message(stream_id, ts, msg_buf); + stats_message(stream_id, receive_ts, sensor_ts, msg_buf); } void StreamingLayoutCW::finish() { @@ -56,14 +58,17 @@ void StreamingLayoutCW::finish() { uint32_t StreamingLayoutCW::chunk_size() const { return chunk_size_; } -void StreamingLayoutCW::stats_message(const uint32_t stream_id, const ts_t ts, +void StreamingLayoutCW::stats_message(const uint32_t stream_id, + const ts_t receive_ts, + const ts_t sensor_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)}); + stream_stats_.insert({stream_id, StreamStats(stream_id, receive_ts, + sensor_ts, msg_size)}); } else { - stats_it->second.update(ts, msg_size); + stats_it->second.update(receive_ts, sensor_ts, msg_size); } } diff --git a/ouster_osf/src/meta_streaming_info.cpp b/ouster_osf/src/meta_streaming_info.cpp index e8ee7040..f344f99e 100644 --- a/ouster_osf/src/meta_streaming_info.cpp +++ b/ouster_osf/src/meta_streaming_info.cpp @@ -24,21 +24,27 @@ std::string to_string(const ChunkInfo& chunk_info) { return ss.str(); } -StreamStats::StreamStats(uint32_t s_id, ts_t t, uint32_t msg_size) +StreamStats::StreamStats(uint32_t s_id, ts_t receive_ts, ts_t sensor_ts, + uint32_t msg_size) : stream_id{s_id}, - start_ts{t}, - end_ts{t}, + start_ts{receive_ts}, + end_ts{receive_ts}, message_count{1}, - message_avg_size{msg_size} {}; + message_avg_size{msg_size} { + receive_timestamps.push_back(receive_ts.count()); + sensor_timestamps.push_back(sensor_ts.count()); +} -void StreamStats::update(ts_t t, uint32_t msg_size) { - if (start_ts > t) start_ts = t; - if (end_ts < t) end_ts = t; +void StreamStats::update(ts_t receive_ts, ts_t sensor_ts, uint32_t msg_size) { + if (start_ts > receive_ts) start_ts = receive_ts; + if (end_ts < receive_ts) end_ts = receive_ts; ++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); + receive_timestamps.push_back(receive_ts.count()); + sensor_timestamps.push_back(sensor_ts.count()); } std::string to_string(const StreamStats& stream_stats) { @@ -47,7 +53,16 @@ std::string to_string(const StreamStats& stream_stats) { << ", 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 << "}"; + << ", message_avg_size = " << stream_stats.message_avg_size + << ", host_timestamps = ["; + for (const auto& ts : stream_stats.receive_timestamps) { + ss << ts << ", "; + } + ss << "], sensor_timestamps = ["; + for (const auto& ts : stream_stats.sensor_timestamps) { + ss << ts << ", "; + } + ss << "]}"; return ss.str(); } @@ -70,7 +85,9 @@ flatbuffers::Offset create_streaming_info( 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); + stat.message_count, stat.message_avg_size, + fbb.CreateVector(stat.receive_timestamps), + fbb.CreateVector(stat.sensor_timestamps)); stream_stats_vec.push_back(ss_offset); } @@ -138,6 +155,16 @@ std::unique_ptr StreamingInfo::from_buffer( ss.end_ts = ts_t{stat->end_ts()}; ss.message_count = stat->message_count(); ss.message_avg_size = stat->message_avg_size(); + if (stat->receive_timestamps()) { + for (auto v : *stat->receive_timestamps()) { + ss.receive_timestamps.push_back(v); + } + } + if (stat->sensor_timestamps()) { + for (auto v : *stat->sensor_timestamps()) { + ss.sensor_timestamps.push_back(v); + } + } return std::make_pair(stat->stream_id(), ss); }); } @@ -167,6 +194,16 @@ std::string StreamingInfo::repr() const { ss["message_count"] = static_cast(stat.second.message_count); ss["message_avg_size"] = stat.second.message_avg_size; + Json::Value st = Json::arrayValue; + Json::Value rt = Json::arrayValue; + for (const auto& t : stat.second.sensor_timestamps) { + st.append(static_cast(t)); + } + for (const auto& t : stat.second.receive_timestamps) { + rt.append(static_cast(t)); + } + ss["sensor_timestamps"] = st; + ss["receive_timestamps"] = rt; si_obj["stream_stats"].append(ss); } diff --git a/ouster_osf/src/png_tools.cpp b/ouster_osf/src/png_tools.cpp index e18f38fd..b197f6a9 100644 --- a/ouster_osf/src/png_tools.cpp +++ b/ouster_osf/src/png_tools.cpp @@ -784,6 +784,9 @@ bool fieldDecodeMulti(LidarScan& lidar_scan, const ScanData& scan_data, } auto res_err = false; for (size_t i = 0; i < field_types.size(); ++i) { + if (!lidar_scan.has_field(field_types[i].first)) { + continue; + } auto err = fieldDecode(lidar_scan, scan_data, scan_idxs[i], field_types[i], px_offset); if (err) { @@ -802,16 +805,12 @@ bool scanDecodeFieldsSingleThread( const std::vector& px_offset, const ouster::LidarScanFieldTypes& field_types) { size_t fields_cnt = lidar_scan.fields().size(); - if (scan_data.size() != fields_cnt) { - logger().error( - "ERROR: lidar_scan data contains # of channels: {}" - ", expected: {} for OSF_EUDP", - scan_data.size(), fields_cnt); - return true; - } size_t next_idx = 0; for (auto ft : field_types) { - auto& f = lidar_scan.field(ft.name); + if (!lidar_scan.has_field(ft.name)) { + ++next_idx; + continue; + } if (fieldDecode(lidar_scan, scan_data, next_idx, {ft.name, ft.element_type}, px_offset)) { logger().error( @@ -830,13 +829,6 @@ bool scanDecodeFields(LidarScan& lidar_scan, const ScanData& scan_data, const std::vector& px_offset, const ouster::LidarScanFieldTypes& field_types) { size_t fields_num = field_types.size(); - if (scan_data.size() != fields_num) { - logger().error( - "ERROR: lidar_scan data contains # of channels: " - "{}, expected: {} for OSF EUDP", - scan_data.size(), fields_num); - return true; - } unsigned int con_num = std::thread::hardware_concurrency(); // looking for at least 4 cores if can't determine @@ -1409,6 +1401,11 @@ ScanChannelData encodeField(const ouster::Field& field) { return buffer; } + // empty case + if (field.bytes() == 0) { + return buffer; + } + FieldView view = uint_view(field); // collapse shape if (view.shape().size() > 2) { @@ -1449,6 +1446,11 @@ void decodeField(ouster::Field& field, const ScanChannelData& buffer) { return; } + // empty case + if (field.bytes() == 0) { + return; + } + FieldView view = uint_view(field); // collapse shape if (view.shape().size() > 2) { diff --git a/ouster_osf/src/reader.cpp b/ouster_osf/src/reader.cpp index 91ba97eb..8c895a06 100644 --- a/ouster_osf/src/reader.cpp +++ b/ouster_osf/src/reader.cpp @@ -384,6 +384,23 @@ nonstd::optional Reader::ts_by_message_idx(uint32_t stream_id, bool Reader::has_message_idx() const { return chunks_.has_message_idx(); }; +bool Reader::has_timestamp_idx() const { + // just check metadata for any elements in the stats timestamp array + for (auto& item : meta_store().find()) { + // return false if anything has the wrong number of timestamps for + // number of messages + for (auto& stats : item.second->stream_stats()) { + if (stats.second.message_count > 0 && + stats.second.receive_timestamps.size() != + stats.second.message_count) { + return false; + } + } + return true; + } + return false; +} + ChunksRange Reader::chunks() { return ChunksRange(0, file_.metadata_offset(), this); } diff --git a/ouster_osf/src/stream_lidar_scan.cpp b/ouster_osf/src/stream_lidar_scan.cpp index e515e566..b11a1686 100644 --- a/ouster_osf/src/stream_lidar_scan.cpp +++ b/ouster_osf/src/stream_lidar_scan.cpp @@ -59,6 +59,9 @@ LidarScan slice_with_cast(const LidarScan& ls_src, static_cast(ls_src.h), field_types.begin(), field_types.end()}; + ls_dest.frame_status = ls_src.frame_status; + ls_dest.shutdown_countdown = ls_src.shutdown_countdown; + ls_dest.shot_limiting_countdown = ls_src.shot_limiting_countdown; ls_dest.frame_id = ls_src.frame_id; // Copy headers @@ -66,6 +69,7 @@ LidarScan slice_with_cast(const LidarScan& ls_src, ls_dest.measurement_id() = ls_src.measurement_id(); ls_dest.status() = ls_src.status(); ls_dest.packet_timestamp() = ls_src.packet_timestamp(); + ls_dest.alert_flags() = ls_src.alert_flags(); ls_dest.pose() = ls_src.pose(); // Copy fields @@ -93,7 +97,7 @@ LidarScan slice_with_cast(const LidarScan& ls_src, // 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 +// triggering for our code because we 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 @@ -153,8 +157,9 @@ static optional rlookup(const impl::Table table, } // mapping of channel name to osf ChanField -static impl::Table +static impl::Table chanfield_strings{{ + {ouster::osf::gen::CHAN_FIELD::UNKNOWN, "UNKNOWN"}, {ouster::osf::gen::CHAN_FIELD::RANGE, ChanField::RANGE}, {ouster::osf::gen::CHAN_FIELD::RANGE2, ChanField::RANGE2}, {ouster::osf::gen::CHAN_FIELD::SIGNAL, ChanField::SIGNAL}, @@ -286,14 +291,31 @@ flatbuffers::Offset create_lidar_scan_msg( fbb.CreateVector>(custom_fields); } - return gen::CreateLidarScanMsg(fbb, channels_off, field_types_off, - timestamp_off, measurement_id_off, - status_off, ls.frame_id, pose_off, - packet_timestamp_id_off, custom_fields_off); + auto alert_flags_off = fbb.CreateVector(ls.alert_flags().data(), + ls.alert_flags().size()); + + return gen::CreateLidarScanMsg( + fbb, channels_off, field_types_off, timestamp_off, measurement_id_off, + status_off, ls.frame_id, pose_off, packet_timestamp_id_off, + custom_fields_off, ls.frame_status, ls.shutdown_countdown, + ls.shot_limiting_countdown, alert_flags_off); } +/** + * Copy the contents of the LidarScanMsg into a LidarScan. + * + * NOTE: LidarScan isn't inherently flatbuffers-based, which unfortunately means + * we're not taking advantage of one of fb's primary benefits - the ability to + * read data from messages without copying or allocating new memory for it. + * Moreover, the OSF representation of LidarScan is compressed and the + * deserialized version is not. + * + * As such, making use of the FB directly would require revisiting the design + * and a significant refactor. + */ std::unique_ptr restore_lidar_scan( - const std::vector buf, const ouster::sensor::sensor_info& info) { + const std::vector buf, const ouster::sensor::sensor_info& info, + const std::vector& fields) { auto ls_msg = flatbuffers::GetSizePrefixedRoot( buf.data()); @@ -304,19 +326,37 @@ std::unique_ptr restore_lidar_scan( // read field_types ouster::LidarScanFieldTypes field_types; if (ls_msg->field_types() && ls_msg->field_types()->size()) { - std::transform( - ls_msg->field_types()->begin(), ls_msg->field_types()->end(), - std::back_inserter(field_types), [](const gen::ChannelField* p) { - return FieldType{from_osf_enum(p->chan_field()), - from_osf_enum(p->chan_field_type()), - {}, - FieldClass::PIXEL_FIELD}; - }); + for (auto it = ls_msg->field_types()->begin(); + it != ls_msg->field_types()->end(); it++) { + auto t = FieldType{from_osf_enum(it->chan_field()), + from_osf_enum(it->chan_field_type()), + {}, + FieldClass::PIXEL_FIELD}; + field_types.push_back(t); + } } // Init lidar scan with recovered fields - auto ls = std::make_unique(width, height, field_types.begin(), - field_types.end()); + ouster::LidarScanFieldTypes field_types2 = + fields.size() == 0 ? field_types : ouster::LidarScanFieldTypes(); + for (const auto& f : fields) { + for (const auto& ft : field_types) { + if (ft.name == f) { + field_types2.push_back(ft); + break; + } + } + } + auto ls = std::make_unique(width, height, field_types2.begin(), + field_types2.end()); + + // set frame status - unfortunately since this is a new field in the FB + // schema and since LidarScan::frame_status is an integer we have no way to + // differentiate between whether the value was zero when written or wasn't + // provided by the writer. + ls->frame_status = ls_msg->frame_status(); + ls->shutdown_countdown = ls_msg->shutdown_countdown(); + ls->shot_limiting_countdown = ls_msg->shot_limiting_countdown(); ls->frame_id = ls_msg->frame_id(); @@ -405,12 +445,30 @@ std::unique_ptr restore_lidar_scan( } } + // Set alert flags per lidar packet + auto alert_flags_vec = ls_msg->alert_flags(); + if (alert_flags_vec) { + if (static_cast(ls->alert_flags().size()) == + alert_flags_vec->size()) { + for (uint8_t i = 0; i < alert_flags_vec->size(); ++i) { + ls->alert_flags()[i] = alert_flags_vec->Get(i); + } + } else if (alert_flags_vec->size() != 0) { + logger().error( + "ERROR: LidarScanMsg has " + "alert_flags of length: " + "{}, expected: {}", + alert_flags_vec->size(), ls->alert_flags().size()); + return nullptr; + } + } + // Fill Scan Data with scan channels auto msg_scan_vec = ls_msg->channels(); - if (!msg_scan_vec || !msg_scan_vec->size()) { + if (!msg_scan_vec) { logger().error( "ERROR: lidar_scan msg doesn't " - "have scan field or it's empty."); + "have scan fields."); return nullptr; } ScanData scan_data; @@ -431,6 +489,16 @@ std::unique_ptr restore_lidar_scan( auto custom_field = msg_custom_fields->Get(i); std::string name{custom_field->name()->c_str()}; + bool found = fields.size() == 0; + for (const auto& f : fields) { + if (f == name) { + found = true; + break; + } + } + if (!found) { + continue; + } ChanFieldType tag = from_osf_enum(custom_field->tag()); std::vector shape{custom_field->shape()->begin(), custom_field->shape()->end()}; @@ -445,6 +513,14 @@ std::unique_ptr restore_lidar_scan( } } + // error if any of the requested fields did not end up in the lidar scan + for (const auto& field : fields) { + if (!ls->has_field(field)) { + throw std::runtime_error("Requested field '" + field + + "' does not exist in OSF."); + } + } + return ls; } @@ -543,10 +619,11 @@ LidarScanStream::LidarScanStream(Token /*key*/, Writer& writer, // 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, +void LidarScanStream::save(const ouster::osf::ts_t receive_ts, + const ouster::osf::ts_t sensor_ts, const LidarScan& lidar_scan) { const auto& msg_buf = make_msg(lidar_scan); - writer_.save_message(meta_.id(), ts, msg_buf); + writer_.save_message(meta_.id(), receive_ts, sensor_ts, msg_buf); } std::vector LidarScanStream::make_msg(const LidarScan& lidar_scan) { @@ -559,12 +636,24 @@ std::vector LidarScanStream::make_msg(const LidarScan& lidar_scan) { return {buf, buf + size}; } +/** + * Decode the buffer (from a MessageRef, ultimately created from a + * StampedMessage - see fb/chunk.fbs) as the type appropriate for this + * LidarScanStream (at this time, this is only ever a LidarScan.) + * + * IMPORTANT: this method allocates a LidarScan, copies the data from the + * buffer, and returns it wrapped in a unique_ptr. It returns nullptr if the + * message couldn't be decoded properly. Overall, this could be more efficient + * and should probably be using value semantics. See restore_lidar_scan for + * details. + */ std::unique_ptr LidarScanStream::decode_msg( const std::vector& buf, const LidarScanStream::meta_type& meta, - const MetadataStore& meta_provider) { + const MetadataStore& meta_provider, + const std::vector& fields) { auto sensor = meta_provider.get(meta.sensor_meta_id()); auto info = sensor->info(); - return restore_lidar_scan(buf, info); + return restore_lidar_scan(buf, info, fields); } } // namespace osf diff --git a/ouster_osf/src/writer.cpp b/ouster_osf/src/writer.cpp index c523efd1..9d0b813c 100644 --- a/ouster_osf/src/writer.cpp +++ b/ouster_osf/src/writer.cpp @@ -149,7 +149,8 @@ void Writer::_save(uint32_t stream_index, const LidarScan& scan, } } - lidar_streams_[stream_index]->save(time, scan); + lidar_streams_[stream_index]->save( + time, ts_t(scan.get_first_valid_column_timestamp()), scan); } else { throw std::logic_error("ERROR: Bad Stream ID"); } @@ -217,7 +218,8 @@ uint64_t Writer::append(const uint8_t* buf, const uint64_t size) { // > > > ===================== Chunk Emiter operations ====================== -void Writer::save_message(const uint32_t stream_id, const ts_t ts, +void Writer::save_message(const uint32_t stream_id, const ts_t receive_ts, + const ts_t sensor_ts, const std::vector& msg_buf) { if (!meta_store_.get(stream_id)) { std::stringstream ss; @@ -229,7 +231,7 @@ void Writer::save_message(const uint32_t stream_id, const ts_t ts, return; } - chunks_writer_->save_message(stream_id, ts, msg_buf); + chunks_writer_->save_message(stream_id, receive_ts, sensor_ts, msg_buf); } const MetadataStore& Writer::meta_store() const { return meta_store_; } @@ -325,7 +327,8 @@ Writer::~Writer() { close(); } // ================================================================ -void ChunkBuilder::save_message(const uint32_t stream_id, const ts_t ts, +void ChunkBuilder::save_message(const uint32_t stream_id, const ts_t receive_ts, + const ts_t /*sensor_ts*/, const std::vector& msg_buf) { if (finished_) { logger().error( @@ -340,11 +343,11 @@ void ChunkBuilder::save_message(const uint32_t stream_id, const ts_t ts, " chunk size MAX_SIZE"); } - update_start_end(ts); + update_start_end(receive_ts); // wrap the buffer into StampedMessage - auto stamped_msg = - gen::CreateStampedMessageDirect(fbb_, ts.count(), stream_id, &msg_buf); + auto stamped_msg = gen::CreateStampedMessageDirect(fbb_, receive_ts.count(), + stream_id, &msg_buf); messages_.push_back(stamped_msg); } diff --git a/ouster_osf/tests/file_ops_test.cpp b/ouster_osf/tests/file_ops_test.cpp index adfc4007..aec6c9e4 100644 --- a/ouster_osf/tests/file_ops_test.cpp +++ b/ouster_osf/tests/file_ops_test.cpp @@ -103,7 +103,7 @@ TEST_F(FileOpsTest, TestFileSize) { 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); + EXPECT_EQ(1025780, 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); @@ -118,7 +118,7 @@ TEST_F(FileOpsTest, TestFileMapping) { EXPECT_TRUE(file_buf != nullptr); int64_t fsize = file_size(test_file_name); - EXPECT_EQ(1021684, fsize); + EXPECT_EQ(1025780, fsize); if (file_buf != nullptr) { std::cout << "bytes = " << to_string(file_buf, 64) << std::endl; diff --git a/ouster_osf/tests/file_test.cpp b/ouster_osf/tests/file_test.cpp index 1f052c3e..c68b48f5 100644 --- a/ouster_osf/tests/file_test.cpp +++ b/ouster_osf/tests/file_test.cpp @@ -37,11 +37,11 @@ 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.version(), OSF_VERSION::V_2_1); + EXPECT_EQ(osf_file.size(), 1025780); EXPECT_EQ(osf_file.offset(), 0); - EXPECT_EQ(osf_file.metadata_offset(), 1013976); + EXPECT_EQ(osf_file.metadata_offset(), 1015824); std::cout << "file = " << osf_file.to_string() << std::endl; EXPECT_EQ(osf_file.seek(100).offset(), 100); @@ -99,11 +99,11 @@ 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.version(), OSF_VERSION::V_2_1); + EXPECT_EQ(osf_file.size(), 1025780); EXPECT_EQ(osf_file.offset(), 0); - EXPECT_EQ(osf_file.metadata_offset(), 1013976); + EXPECT_EQ(osf_file.metadata_offset(), 1015824); std::cout << "file = " << osf_file.to_string() << std::endl; EXPECT_TRUE(osf_file.valid()); diff --git a/ouster_osf/tests/meta_streaming_info_test.cpp b/ouster_osf/tests/meta_streaming_info_test.cpp index 4d879f7a..7044886e 100644 --- a/ouster_osf/tests/meta_streaming_info_test.cpp +++ b/ouster_osf/tests/meta_streaming_info_test.cpp @@ -21,10 +21,11 @@ TEST_F(MetaStreamingInfoTests, StreamingPrintTests) { "{offset = 1, stream_id = 2, message_count = 3}"); ts_t t(5678L); - StreamStats data2(4, t, 6); + StreamStats data2(4, t, t, 6); EXPECT_EQ(to_string(data2), "{stream_id = 4, start_ts = 5678, end_ts = 5678," - " message_count = 1, message_avg_size = 6}"); + " message_count = 1, message_avg_size = 6, host_timestamps = " + "[5678, ], sensor_timestamps = [5678, ]}"); } } // namespace diff --git a/ouster_osf/tests/operations_test.cpp b/ouster_osf/tests/operations_test.cpp index 8e1a713c..7f601965 100644 --- a/ouster_osf/tests/operations_test.cpp +++ b/ouster_osf/tests/operations_test.cpp @@ -46,7 +46,8 @@ class FileSha { handleEvpError(); } - char buf[BLOCK_SIZE]; + std::vector buf; + buf.resize(BLOCK_SIZE); uint64_t i = ouster::osf::file_size(filename); bool finished = false; @@ -59,8 +60,8 @@ class FileSha { size = i; finished = true; } - reader.read(buf, size); - if (EVP_DigestUpdate(context, buf, size) != 1) { + reader.read(buf.data(), size); + if (EVP_DigestUpdate(context, buf.data(), size) != 1) { handleEvpError(); } i -= block_size; @@ -125,7 +126,7 @@ TEST_F(OperationsTest, GetOsfDumpInfo) { 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_EQ("ouster_sdk", 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")); @@ -142,8 +143,9 @@ TEST_F(OperationsTest, FileShaTest) { std::string temp_dir; EXPECT_TRUE(make_tmp_dir(temp_dir)); std::string temp_file = path_concat(temp_dir, "test_file"); - test_file_out.open(temp_file, std::fstream::out | std::fstream::trunc); - test_file_out << "Testing here for hashing" << std::endl; + test_file_out.open(temp_file, std::fstream::out | std::fstream::trunc | + std::fstream::binary); + test_file_out << "Testing here for hashing\n"; test_file_out.close(); auto sha = FileSha(temp_file); EXPECT_EQ( @@ -208,7 +210,7 @@ ouster::sensor::sensor_info _gen_new_metadata(int start_number) { new_metadata.config.lidar_mode = ouster::sensor::MODE_512x10; new_metadata.prod_line = "OS-1-128"; - new_metadata.format.pixels_per_column = 5; + new_metadata.format.pixels_per_column = 128; new_metadata.format.columns_per_packet = 2 + start_number; new_metadata.format.columns_per_frame = 3 + start_number; new_metadata.format.pixel_shift_by_row = { @@ -219,14 +221,14 @@ ouster::sensor::sensor_info _gen_new_metadata(int start_number) { ouster::sensor::PROFILE_RNG15_RFL8_NIR8; new_metadata.format.udp_profile_imu = ouster::sensor::PROFILE_IMU_LEGACY; new_metadata.format.fps = 11 + start_number; - new_metadata.beam_azimuth_angles = { - 12. + (double)start_number, 13. + (double)start_number, - 14. + (double)start_number, 15. + (double)start_number, - 16. + (double)start_number}; - new_metadata.beam_altitude_angles = { - 17. + (double)start_number, 18. + (double)start_number, - 19. + (double)start_number, 20. + (double)start_number, - 21. + (double)start_number}; + new_metadata.beam_azimuth_angles.resize( + new_metadata.format.pixels_per_column); + new_metadata.beam_altitude_angles.resize( + new_metadata.format.pixels_per_column); + for (size_t i = 0; i < new_metadata.format.pixels_per_column; i++) { + new_metadata.beam_azimuth_angles[i] = (double)i; + new_metadata.beam_altitude_angles[i] = (double)i; + } new_metadata.lidar_origin_to_beam_origin_mm = 22 + start_number; new_metadata.init_id = 23 + start_number; diff --git a/ouster_osf/tests/reader_test.cpp b/ouster_osf/tests/reader_test.cpp index 91f06cdf..f280a975 100644 --- a/ouster_osf/tests/reader_test.cpp +++ b/ouster_osf/tests/reader_test.cpp @@ -28,7 +28,7 @@ TEST_F(ReaderTest, Basics) { Reader reader(osf_file); - EXPECT_EQ("from_pcap pythonic", reader.metadata_id()); + EXPECT_EQ("ouster_sdk", reader.metadata_id()); EXPECT_EQ(991587364520LL, reader.start_ts().count()); EXPECT_EQ(991787323080LL, reader.end_ts().count()); @@ -37,68 +37,89 @@ TEST_F(ReaderTest, Basics) { EXPECT_TRUE(sensor); EXPECT_EQ( sensor->to_string(), - "{\n \"sensor_info\": \n {\n \"base_pn\": \"\",\n \"base_sn\": " - "\"\",\n \"beam_altitude_angles\": \n [\n 20.95,\n " - "20.67,\n 20.36,\n 20.03,\n 19.73,\n 19.41,\n " - "19.11,\n 18.76,\n 18.47,\n 18.14,\n 17.82,\n " - "17.5,\n 17.19,\n 16.86,\n 16.53,\n 16.2,\n " - "15.89,\n 15.56,\n 15.23,\n 14.9,\n 14.57,\n " - "14.23,\n 13.9,\n 13.57,\n 13.25,\n 12.91,\n " - "12.57,\n 12.22,\n 11.9,\n 11.55,\n 11.2,\n " - "10.87,\n 10.54,\n 10.18,\n 9.84,\n 9.51,\n " - "9.15,\n 8.81,\n 8.47,\n 8.11,\n 7.78,\n " - "7.43,\n 7.08,\n 6.74,\n 6.39,\n 6.04,\n " - "5.7,\n 5.34,\n 4.98,\n 4.64,\n 4.29,\n " - "3.93,\n 3.58,\n 3.24,\n 2.88,\n 2.53,\n " - "2.17,\n 1.82,\n 1.47,\n 1.12,\n 0.78,\n " - "0.41,\n 0.07,\n -0.28,\n -0.64,\n -0.99,\n " - "-1.35,\n -1.7,\n -2.07,\n -2.4,\n -2.75,\n " - "-3.11,\n -3.46,\n -3.81,\n -4.15,\n -4.5,\n " - "-4.86,\n -5.22,\n -5.57,\n -5.9,\n -6.27,\n " - "-6.61,\n -6.97,\n -7.3,\n -7.67,\n -8.01,\n " - "-8.35,\n -8.69,\n -9.05,\n -9.38,\n -9.71,\n " - "-10.07,\n -10.42,\n -10.76,\n -11.09,\n -11.43,\n " - " -11.78,\n -12.12,\n -12.46,\n -12.78,\n " - "-13.15,\n -13.46,\n -13.8,\n -14.12,\n -14.48,\n " - " -14.79,\n -15.11,\n -15.46,\n -15.79,\n " - "-16.12,\n -16.45,\n -16.76,\n -17.11,\n -17.44,\n " - " -17.74,\n -18.06,\n -18.39,\n -18.72,\n " - "-19.02,\n -19.32,\n -19.67,\n -19.99,\n -20.27,\n " - " -20.57,\n -20.92,\n -21.22,\n -21.54,\n " - "-21.82\n ],\n \"beam_azimuth_angles\": \n [\n 4.21,\n " - " 1.41,\n -1.4,\n -4.22,\n 4.22,\n 1.41,\n " - "-1.4,\n -4.23,\n 4.21,\n 1.4,\n -1.42,\n " - "-4.2,\n 4.22,\n 1.41,\n -1.4,\n -4.23,\n " - "4.21,\n 1.41,\n -1.41,\n -4.21,\n 4.22,\n " - "1.4,\n -1.41,\n -4.2,\n 4.22,\n 1.42,\n " - "-1.4,\n -4.2,\n 4.22,\n 1.41,\n -1.42,\n " - "-4.21,\n 4.22,\n 1.41,\n -1.4,\n -4.21,\n " - "4.2,\n 1.4,\n -1.4,\n -4.22,\n 4.21,\n " - "1.41,\n -1.41,\n -4.21,\n 4.22,\n 1.41,\n " - "-1.4,\n -4.21,\n 4.21,\n 1.41,\n -1.4,\n " - "-4.21,\n 4.2,\n 1.41,\n -1.4,\n -4.21,\n " - "4.2,\n 1.4,\n -1.41,\n -4.21,\n 4.22,\n " - "1.4,\n -1.4,\n -4.21,\n 4.22,\n 1.42,\n " - "-1.4,\n -4.2,\n 4.2,\n 1.42,\n -1.4,\n " - "-4.22,\n 4.22,\n 1.41,\n -1.4,\n -4.2,\n " - "4.23,\n 1.41,\n -1.4,\n -4.2,\n 4.21,\n " - "1.41,\n -1.4,\n -4.21,\n 4.21,\n 1.41,\n " - "-1.4,\n -4.21,\n 4.22,\n 1.41,\n -1.39,\n " - "-4.21,\n 4.23,\n 1.41,\n -1.39,\n -4.22,\n " - "4.23,\n 1.4,\n -1.4,\n -4.2,\n 4.21,\n " - "1.41,\n -1.41,\n -4.2,\n 4.22,\n 1.42,\n " - "-1.39,\n -4.22,\n 4.24,\n 1.41,\n -1.41,\n " - "-4.22,\n 4.23,\n 1.41,\n -1.39,\n -4.21,\n " - "4.23,\n 1.41,\n -1.39,\n -4.2,\n 4.23,\n " - "1.4,\n -1.39,\n -4.2,\n 4.22,\n 1.42,\n " - "-1.39,\n -4.2\n ],\n \"build_date\": " - "\"2022-04-14T21:11:47Z\",\n \"build_rev\": \"v2.3.0\",\n " - "\"client_version\": \"ouster_client 0.3.0\",\n \"data_format\": \n " - " {\n \"column_window\": \n [\n 0,\n 1023\n " - " ],\n \"columns_per_frame\": 1024,\n " - "\"columns_per_packet\": 16,\n \"pixel_shift_by_row\": \n " - "[\n 24,\n 16,\n 8,\n 0,\n 24,\n " - " 16,\n 8,\n 0,\n 24,\n 16,\n " + "{\n \"sensor_info\": \n {\n \"beam_intrinsics\": \n {\n " + "\"beam_altitude_angles\": \n [\n 20.95,\n 20.67,\n " + " 20.36,\n 20.03,\n 19.73,\n 19.41,\n " + " 19.11,\n 18.76,\n 18.47,\n 18.14,\n " + "17.82,\n 17.5,\n 17.19,\n 16.86,\n " + "16.53,\n 16.2,\n 15.89,\n 15.56,\n " + "15.23,\n 14.9,\n 14.57,\n 14.23,\n " + "13.9,\n 13.57,\n 13.25,\n 12.91,\n " + "12.57,\n 12.22,\n 11.9,\n 11.55,\n " + "11.2,\n 10.87,\n 10.54,\n 10.18,\n " + "9.84,\n 9.51,\n 9.15,\n 8.81,\n 8.47,\n " + " 8.11,\n 7.78,\n 7.43,\n 7.08,\n " + "6.74,\n 6.39,\n 6.04,\n 5.7,\n 5.34,\n " + " 4.98,\n 4.64,\n 4.29,\n 3.93,\n " + "3.58,\n 3.24,\n 2.88,\n 2.53,\n 2.17,\n " + " 1.82,\n 1.47,\n 1.12,\n 0.78,\n " + "0.41,\n 0.07,\n -0.28,\n -0.64,\n " + "-0.99,\n -1.35,\n -1.7,\n -2.07,\n " + "-2.4,\n -2.75,\n -3.11,\n -3.46,\n " + "-3.81,\n -4.15,\n -4.5,\n -4.86,\n " + "-5.22,\n -5.57,\n -5.9,\n -6.27,\n " + "-6.61,\n -6.97,\n -7.3,\n -7.67,\n " + "-8.01,\n -8.35,\n -8.69,\n -9.05,\n " + "-9.38,\n -9.71,\n -10.07,\n -10.42,\n " + "-10.76,\n -11.09,\n -11.43,\n -11.78,\n " + "-12.12,\n -12.46,\n -12.78,\n -13.15,\n " + "-13.46,\n -13.8,\n -14.12,\n -14.48,\n " + "-14.79,\n -15.11,\n -15.46,\n -15.79,\n " + "-16.12,\n -16.45,\n -16.76,\n -17.11,\n " + "-17.44,\n -17.74,\n -18.06,\n -18.39,\n " + "-18.72,\n -19.02,\n -19.32,\n -19.67,\n " + "-19.99,\n -20.27,\n -20.57,\n -20.92,\n " + "-21.22,\n -21.54,\n -21.82\n ],\n " + "\"beam_azimuth_angles\": \n [\n 4.21,\n 1.41,\n " + " -1.4,\n -4.22,\n 4.22,\n 1.41,\n " + "-1.4,\n -4.23,\n 4.21,\n 1.4,\n -1.42,\n " + " -4.2,\n 4.22,\n 1.41,\n -1.4,\n " + "-4.23,\n 4.21,\n 1.41,\n -1.41,\n " + "-4.21,\n 4.22,\n 1.4,\n -1.41,\n -4.2,\n " + " 4.22,\n 1.42,\n -1.4,\n -4.2,\n " + "4.22,\n 1.41,\n -1.42,\n -4.21,\n 4.22,\n " + " 1.41,\n -1.4,\n -4.21,\n 4.2,\n " + "1.4,\n -1.4,\n -4.22,\n 4.21,\n 1.41,\n " + " -1.41,\n -4.21,\n 4.22,\n 1.41,\n " + "-1.4,\n -4.21,\n 4.21,\n 1.41,\n -1.4,\n " + " -4.21,\n 4.2,\n 1.41,\n -1.4,\n " + "-4.21,\n 4.2,\n 1.4,\n -1.41,\n -4.21,\n " + " 4.22,\n 1.4,\n -1.4,\n -4.21,\n " + "4.22,\n 1.42,\n -1.4,\n -4.2,\n 4.2,\n " + " 1.42,\n -1.4,\n -4.22,\n 4.22,\n " + "1.41,\n -1.4,\n -4.2,\n 4.23,\n 1.41,\n " + " -1.4,\n -4.2,\n 4.21,\n 1.41,\n " + "-1.4,\n -4.21,\n 4.21,\n 1.41,\n -1.4,\n " + " -4.21,\n 4.22,\n 1.41,\n -1.39,\n " + "-4.21,\n 4.23,\n 1.41,\n -1.39,\n " + "-4.22,\n 4.23,\n 1.4,\n -1.4,\n -4.2,\n " + " 4.21,\n 1.41,\n -1.41,\n -4.2,\n " + "4.22,\n 1.42,\n -1.39,\n -4.22,\n 4.24,\n " + " 1.41,\n -1.41,\n -4.22,\n 4.23,\n " + "1.41,\n -1.39,\n -4.21,\n 4.23,\n 1.41,\n " + " -1.39,\n -4.2,\n 4.23,\n 1.4,\n " + "-1.39,\n -4.2,\n 4.22,\n 1.42,\n -1.39,\n " + " -4.2\n ],\n \"beam_to_lidar_transform\": \n [\n " + " 1.0,\n 0.0,\n 0.0,\n 15.806,\n " + "0.0,\n 1.0,\n 0.0,\n 0.0,\n 0.0,\n " + "0.0,\n 1.0,\n 0.0,\n 0.0,\n 0.0,\n " + "0.0,\n 1.0\n ],\n " + "\"lidar_origin_to_beam_origin_mm\": 15.806\n },\n " + "\"calibration_status\": {},\n \"config_params\": \n {\n " + "\"lidar_mode\": \"1024x10\",\n \"udp_port_imu\": 7503,\n " + "\"udp_port_lidar\": 7502\n },\n \"imu_intrinsics\": \n {\n " + " \"imu_to_sensor_transform\": \n [\n 1.0,\n " + "0.0,\n 0.0,\n 6.253,\n 0.0,\n 1.0,\n " + " 0.0,\n -11.775,\n 0.0,\n 0.0,\n 1.0,\n " + " 7.645,\n 0.0,\n 0.0,\n 0.0,\n 1.0\n " + " ]\n },\n \"lidar_data_format\": \n {\n " + "\"column_window\": \n [\n 0,\n 1023\n ],\n " + " \"columns_per_frame\": 1024,\n \"columns_per_packet\": 16,\n " + " \"fps\": 10,\n \"pixel_shift_by_row\": \n [\n " + "24,\n 16,\n 8,\n 0,\n 24,\n 16,\n " + " 8,\n 0,\n 24,\n 16,\n 8,\n " + "0,\n 24,\n 16,\n 8,\n 0,\n 24,\n " + " 16,\n 8,\n 0,\n 24,\n 16,\n " "8,\n 0,\n 24,\n 16,\n 8,\n 0,\n " " 24,\n 16,\n 8,\n 0,\n 24,\n " "16,\n 8,\n 0,\n 24,\n 16,\n 8,\n " @@ -118,25 +139,26 @@ TEST_F(ReaderTest, Basics) { "8,\n 0,\n 24,\n 16,\n 8,\n 0,\n " " 24,\n 16,\n 8,\n 0,\n 24,\n " "16,\n 8,\n 0,\n 24,\n 16,\n 8,\n " - " 0,\n 24,\n 16,\n 8,\n 0,\n " - "24,\n 16,\n 8,\n 0,\n 24,\n 16,\n " - " 8,\n 0\n ],\n \"pixels_per_column\": 128,\n " - " \"udp_profile_imu\": \"LEGACY\",\n \"udp_profile_lidar\": " - "\"RNG15_RFL8_NIR8\"\n },\n \"hostname\": \"\",\n " + " 0\n ],\n \"pixels_per_column\": 128,\n " + "\"udp_profile_imu\": \"LEGACY\",\n \"udp_profile_lidar\": " + "\"RNG15_RFL8_NIR8\"\n },\n \"lidar_intrinsics\": \n {\n " + "\"lidar_to_sensor_transform\": \n [\n -1.0,\n " + "0.0,\n 0.0,\n 0.0,\n 0.0,\n -1.0,\n " + " 0.0,\n 0.0,\n 0.0,\n 0.0,\n 1.0,\n " + " 36.18,\n 0.0,\n 0.0,\n 0.0,\n 1.0\n " + "]\n },\n \"ouster-sdk\": \n {\n \"client_version\": " + "\"ouster_client 0.13.0\",\n \"extrinsic\": \n [\n " + "1.0,\n 0.0,\n 0.0,\n 0.0,\n 0.0,\n " + "1.0,\n 0.0,\n 0.0,\n 0.0,\n 0.0,\n " + "1.0,\n 0.0,\n 0.0,\n 0.0,\n 0.0,\n " + "1.0\n ],\n \"output_source\": \"sensor_info_to_string\"\n " + " },\n \"sensor_info\": \n {\n \"build_date\": " + "\"2022-04-14T21:11:47Z\",\n \"build_rev\": \"v2.3.0\",\n " "\"image_rev\": \"ousteros-image-prod-aries-v2.3.0+20220415163956\",\n " - " \"imu_to_sensor_transform\": \n [\n 1,\n 0,\n " - "0,\n 6.253,\n 0,\n 1,\n 0,\n -11.775,\n " - "0,\n 0,\n 1,\n 7.645,\n 0,\n 0,\n 0,\n " - " 1\n ],\n \"initialization_id\": 7109750,\n " - "\"json_calibration_version\": 4,\n \"lidar_mode\": \"1024x10\",\n " - " \"lidar_origin_to_beam_origin_mm\": 15.806,\n " - "\"lidar_to_sensor_transform\": \n [\n -1,\n 0,\n " - "0,\n 0,\n 0,\n -1,\n 0,\n 0,\n 0,\n " - " 0,\n 1,\n 36.18,\n 0,\n 0,\n 0,\n 1\n " - " ],\n \"prod_line\": \"OS-1-128\",\n \"prod_pn\": " - "\"840-103575-06\",\n \"prod_sn\": \"122201000998\",\n " - "\"proto_rev\": \"\",\n \"status\": \"RUNNING\",\n " - "\"udp_port_imu\": 7503,\n \"udp_port_lidar\": 7502\n }\n}"); + " \"initialization_id\": 7109750,\n \"prod_line\": " + "\"OS-1-128\",\n \"prod_pn\": \"840-103575-06\",\n " + "\"prod_sn\": \"122201000998\",\n \"status\": \"RUNNING\"\n " + "},\n \"user_data\": \"\"\n }\n}"); EXPECT_EQ(1, reader.meta_store().count()); EXPECT_EQ( @@ -154,8 +176,8 @@ TEST_F(ReaderTest, ChunksReading) { auto chunks = reader.chunks(); - EXPECT_EQ(chunks.to_string(), "ChunksRange: [ba = 0, ea = 1013976]"); - EXPECT_EQ(chunks.begin().to_string(), "ChunksIter: [ca = 0, ea = 1013976]"); + EXPECT_EQ(chunks.to_string(), "ChunksRange: [ba = 0, ea = 1015824]"); + EXPECT_EQ(chunks.begin().to_string(), "ChunksIter: [ca = 0, ea = 1015824]"); std::cout << chunks.begin()->end_ts().count() << std::endl; EXPECT_EQ(chunks.begin()->start_ts(), ts_t(991587364520L)); EXPECT_EQ(chunks.begin()->end_ts(), ts_t(991787323080L)); @@ -165,14 +187,12 @@ TEST_F(ReaderTest, ChunksReading) { " start_ts = 991587364520, end_ts = 991787323080," " status = 1}), chunk_buf_ = nullptr]"); EXPECT_EQ(((ChunkRef)*chunks.begin())[0].to_string(), - "MessageRef: [id = 2, ts = 991587364520, buffer = 0c" - " 2b 05 00 14 00 00 00 10 00 1c 00 04 00 08 00 0c 00" - " 10 00 14 00 18 00 10 00 00 00 34 38 00 00 24 38 00" - " 00 18 18 00 00 10 10 00 00 08 00 00 00 03 07 00 00" - " 00 04 00 00 01 00 00 00 01 00 00 00 01 00 00 00 01" - " 00 00 00 01 00 00 00 01 00 00 00 01 00 00 00 01 00" - " 00 00 01 00 00 00 01 00 00 00 01 00 00 00 ... and" - " 338604 more ...]"); + "MessageRef: [id = 2, ts = 991587364520, buffer = 74 2d 05 00 28 " + "00 00 00 00 00 00 00 00 00 1e 00 24 00 04 00 08 00 0c 00 10 00 " + "14 00 18 00 00 00 1c 00 00 00 00 00 00 00 00 00 20 00 1e 00 00 " + "00 88 3a 00 00 78 3a 00 00 6c 1a 00 00 64 12 00 00 5c 02 00 00 " + "03 07 00 00 4c 00 00 00 04 00 00 00 40 00 00 00 00 00 00 00 00 " + "00 00 00 00 00 00 00 00 00 00 00 ... and 339220 more ...]"); EXPECT_EQ(chunks.begin()->begin().to_string(), "MessagesChunkIter: [chunk_ref = ChunkRef:" " [msgs_size = 3, state = ({offset = 0," @@ -245,15 +265,14 @@ TEST_F(ReaderTest, MessagesReadingStreaming) { int sit_cnt = 0; ts_t sit_prev{0}; bool sit_ordered = true; - EXPECT_EQ((*reader.messages().begin()).to_string(), - "MessageRef: [id = 2, ts = 991587364520, buffer =" - " 0c 2b 05 00 14 00 00 00 10 00 1c 00 04 00 08 00" - " 0c 00 10 00 14 00 18 00 10 00 00 00 34 38 00 00" - " 24 38 00 00 18 18 00 00 10 10 00 00 08 00 00 00" - " 03 07 00 00 00 04 00 00 01 00 00 00 01 00 00 00" - " 01 00 00 00 01 00 00 00 01 00 00 00 01 00 00 00" - " 01 00 00 00 01 00 00 00 01 00 00 00 01 00 00 00" - " 01 00 00 00 ... and 338604 more ...]"); + EXPECT_EQ( + (*reader.messages().begin()).to_string(), + "MessageRef: [id = 2, ts = 991587364520, buffer = 74 2d 05 00 28 00 00 " + "00 00 00 00 00 00 00 1e 00 24 00 04 00 08 00 0c 00 10 00 14 00 18 00 " + "00 00 1c 00 00 00 00 00 00 00 00 00 20 00 1e 00 00 00 88 3a 00 00 78 " + "3a 00 00 6c 1a 00 00 64 12 00 00 5c 02 00 00 03 07 00 00 4c 00 00 00 " + "04 00 00 00 40 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 " + "00 ... and 339220 more ...]"); EXPECT_EQ(reader.messages().begin().to_string(), "MessagesStreamingIter: [curr_ts = 991587364520," " end_ts = 991787323081, curr_chunks_.size = 1," diff --git a/ouster_osf/tests/writer_custom_test.cpp b/ouster_osf/tests/writer_custom_test.cpp index e14261a1..f4349428 100644 --- a/ouster_osf/tests/writer_custom_test.cpp +++ b/ouster_osf/tests/writer_custom_test.cpp @@ -80,7 +80,7 @@ class YoStream : public MessageStream { // 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_.save_message(meta_.id(), ts, msg_buf); + writer_.save_message(meta_.id(), ts, ts, msg_buf); } // Pack yo message into buffer diff --git a/ouster_osf/tests/writer_test.cpp b/ouster_osf/tests/writer_test.cpp index ef3b6ed2..e54b3e5a 100644 --- a/ouster_osf/tests/writer_test.cpp +++ b/ouster_osf/tests/writer_test.cpp @@ -260,7 +260,7 @@ TEST_F(WriterTest, WriteSlicedLegacyLidarScan) { field_types.emplace_back(sensor::ChanField::SIGNAL, sensor::ChanFieldType::UINT16); field_types.emplace_back(sensor::ChanField::REFLECTIVITY, - sensor::ChanFieldType::UINT16); + sensor::ChanFieldType::UINT8); std::cout << "LidarScan field_types: " << ouster::to_string(field_types) << std::endl; diff --git a/ouster_osf/tests/writerv2_test.cpp b/ouster_osf/tests/writerv2_test.cpp index 69764e8a..fed247e0 100644 --- a/ouster_osf/tests/writerv2_test.cpp +++ b/ouster_osf/tests/writerv2_test.cpp @@ -151,17 +151,34 @@ TEST_F(WriterV2Test, WriterV2SingleVectorTest) { void test_multi_file(std::string& output_osf_filename, LidarScan& ls, LidarScan& ls2) { + bool got_1 = false; + bool got_2 = false; Reader reader(output_osf_filename); auto msg_it = reader.messages().begin(); EXPECT_NE(msg_it, reader.messages().end()); + msg_it->id(); auto ls_recovered = msg_it->decode_msg(); EXPECT_TRUE(ls_recovered); - EXPECT_EQ(*ls_recovered, ls); + if (msg_it->id() == 3) { + got_1 = true; + EXPECT_EQ(*ls_recovered, ls); + } else { + got_2 = true; + EXPECT_EQ(*ls_recovered, ls2); + } EXPECT_NE(++msg_it, reader.messages().end()); auto ls_recovered2 = msg_it->decode_msg(); EXPECT_TRUE(ls_recovered2); - EXPECT_EQ(*ls_recovered2, ls2); + if (msg_it->id() == 3) { + got_1 = true; + EXPECT_EQ(*ls_recovered2, ls); + } else { + got_2 = true; + EXPECT_EQ(*ls_recovered2, ls2); + } EXPECT_EQ(++msg_it, reader.messages().end()); + EXPECT_TRUE(got_1); + EXPECT_TRUE(got_2); } TEST_F(WriterV2Test, WriterV2MultiIndexedTest) { @@ -178,7 +195,7 @@ TEST_F(WriterV2Test, WriterV2MultiIndexedTest) { writer.save(0, ls); writer.save(1, ls2); } - test_multi_file(output_osf_filename, ls2, ls); + test_multi_file(output_osf_filename, ls, ls2); } TEST_F(WriterV2Test, WriterV2MultiVectorTest) { @@ -194,7 +211,7 @@ TEST_F(WriterV2Test, WriterV2MultiVectorTest) { Writer writer(output_osf_filename, {info, info2}); writer.save({ls, ls2}); } - test_multi_file(output_osf_filename, ls2, ls); + test_multi_file(output_osf_filename, ls, ls2); } } // namespace } // namespace osf diff --git a/ouster_pcap/include/ouster/indexed_pcap_reader.h b/ouster_pcap/include/ouster/indexed_pcap_reader.h index 8fa5137e..9bc2386b 100644 --- a/ouster_pcap/include/ouster/indexed_pcap_reader.h +++ b/ouster_pcap/include/ouster/indexed_pcap_reader.h @@ -4,6 +4,9 @@ #pragma once +#include +#include + #include "ouster/os_pcap.h" #include "ouster/pcap.h" #include "ouster/types.h" @@ -11,7 +14,8 @@ namespace ouster { namespace sensor_utils { -struct PcapIndex { +class PcapIndex { + public: using frame_index = std::vector; ///< Maps a frame number to a file offset @@ -49,16 +53,21 @@ struct PcapIndex { */ size_t frame_count(size_t sensor_index) const; - /** - * Seeks the given reader to the given frame number for the given sensor - * index - */ // TODO[UN]: in my opinion we are better off removing this method from this // class It is better if we keep this class as a simple POD object. Another // problem with this method specifically is that it creates a cyclic and // this is the reason why we are passing PcapReader instead of // IndexedPcapReader to avoid this cyclic relation. If it mounts to anything - // this method should be part of the IndexedPcapReader. + // this method should be part of the IndexedPcapReader + /** + * Seeks the given reader to the given frame number for the given sensor + * index + * + * @param[in,out] reader The reader to use for seeking. + * @param[in] sensor_index The position of the sensor for which to + * seek for. + * @param[in] frame_number The frame number to seek to. + */ void seek_to_frame(PcapReader& reader, size_t sensor_index, unsigned int frame_number); }; @@ -69,13 +78,19 @@ struct PcapIndex { * The index must be computed by iterating through all packets and calling * `update_index_for_current_packet()` for each one. */ -struct IndexedPcapReader : public PcapReader { +class IndexedPcapReader : public PcapReader { + public: /** * @param[in] pcap_filename A file path of the pcap to read - * @param[in] metadata_filenames A vector of sensor metadata file paths + * @param[in] metadata_filenames A vector of sensor metadata filepaths */ - IndexedPcapReader(const std::string& pcap_filename, - const std::vector& metadata_filenames); + IndexedPcapReader( + const std::string& + pcap_filename, ///< [in] - A file path of the pcap to read + const std::vector& + metadata_filenames ///< [in] - A vector of sensor metadata file + ///< paths + ); /** * @param[in] pcap_filename A file path of the pcap to read @@ -116,11 +131,14 @@ struct IndexedPcapReader : public PcapReader { /** * Updates the frame index for the current packet + * + * @todo I recommend take this a private method, + * the problem with exposing this method is that + * it only yields right results if invoked sequentially + * ; the results are dependent on the internal state. + * * @return the progress of indexing as an int from [0, 100] */ - // TODO: I recommend take this a private method, the problem with exposing - // this method is that it only yields right results if invoked sequentially - // ; the results are dependent on the internal state. int update_index_for_current_packet(); /** @@ -128,18 +146,24 @@ struct IndexedPcapReader : public PcapReader { * hopefully avoiding spurious result that could occur from out of order or * dropped packets. * + * @param[in] previous The previous frame id. + * @param[in] current The current frame id. * @return true if the frame id has rolled over. */ static bool frame_id_rolled_over(uint16_t previous, uint16_t current); + protected: + void init_(); std::vector sensor_infos_; ///< A vector of sensor_info that correspond to the ///< provided metadata files + std::vector packet_formats_; PcapIndex index_; // TODO: remove, this should be a transient variable std::vector> previous_frame_ids_; ///< previous frame id for each sensor + std::unordered_map> port_map_; }; } // namespace sensor_utils diff --git a/ouster_pcap/include/ouster/os_pcap.h b/ouster_pcap/include/ouster/os_pcap.h index 38d2e641..f4f57a6f 100644 --- a/ouster_pcap/include/ouster/os_pcap.h +++ b/ouster_pcap/include/ouster/os_pcap.h @@ -191,6 +191,7 @@ size_t read_packet(playback_handle& handle, uint8_t* buf, size_t buffer_size); * @param[in] file The file path to the target pcap to record to. * @param[in] frag_size The size of the fragments for packet fragmentation. * @param[in] use_sll_encapsulation Whether to use sll encapsulation. + * @return record_handle A handle to the initialized record. */ std::shared_ptr record_initialize( const std::string& file, int frag_size, bool use_sll_encapsulation = false); diff --git a/ouster_pcap/include/ouster/pcap.h b/ouster_pcap/include/ouster/pcap.h index f959c816..5577c8b8 100644 --- a/ouster_pcap/include/ouster/pcap.h +++ b/ouster_pcap/include/ouster/pcap.h @@ -43,6 +43,7 @@ struct packet_info { * Class for dealing with reading pcap files */ class PcapReader { + protected: std::unique_ptr impl; ///< Private implementation pointer packet_info info; ///< Cached packet info std::map fragment_count; ///< Map to count fragments per packet diff --git a/ouster_pcap/src/indexed_pcap_reader.cpp b/ouster_pcap/src/indexed_pcap_reader.cpp index 50344f25..61734fc6 100644 --- a/ouster_pcap/src/indexed_pcap_reader.cpp +++ b/ouster_pcap/src/indexed_pcap_reader.cpp @@ -1,5 +1,10 @@ #include "ouster/indexed_pcap_reader.h" +#include +#include + +#include "ouster/types.h" + namespace ouster { namespace sensor_utils { @@ -10,9 +15,10 @@ IndexedPcapReader::IndexedPcapReader( index_(metadata_filenames.size()), previous_frame_ids_(metadata_filenames.size()) { for (const std::string& metadata_filename : metadata_filenames) { - sensor_infos_.push_back( - ouster::sensor::metadata_from_json(metadata_filename)); + auto temp_info = ouster::sensor::metadata_from_json(metadata_filename); + sensor_infos_.push_back(temp_info); } + init_(); } IndexedPcapReader::IndexedPcapReader( @@ -21,19 +27,64 @@ IndexedPcapReader::IndexedPcapReader( : PcapReader(pcap_filename), sensor_infos_(sensor_infos), index_(sensor_infos.size()), - previous_frame_ids_(sensor_infos.size()) {} + previous_frame_ids_(sensor_infos.size()) { + init_(); +} + +void IndexedPcapReader::init_() { + uint64_t index = 0; + for (auto it : sensor_infos_) { + std::string sn_lidar = it.sn; + std::string sn_imu = "LEGACY_IMU"; + if (it.config.udp_profile_lidar == + ouster::sensor::UDPProfileLidar::PROFILE_LIDAR_LEGACY) { + sn_lidar = "LEGACY_LIDAR"; + } + packet_formats_.push_back(ouster::sensor::packet_format(it)); + + if (port_map_[*it.config.udp_port_lidar].find(sn_lidar) != + port_map_[*it.config.udp_port_lidar].end()) { + std::cout << "Duplicate lidar port/sn found for indexing pcap: " + + sn_lidar + ":" + + std::to_string(*it.config.udp_port_lidar) + << std::endl; + throw std::runtime_error( + "Duplicate lidar port/sn found for indexing pcap: " + sn_lidar + + ":" + std::to_string(*it.config.udp_port_lidar)); + } + port_map_[*it.config.udp_port_lidar][sn_lidar] = index; + if (port_map_[*it.config.udp_port_imu].find(sn_imu) != + port_map_[*it.config.udp_port_imu].end()) { + std::cout << "Duplicate imu port/sn found for indexing pcap: " + + sn_imu + ":" + + std::to_string(*it.config.udp_port_imu) + << std::endl; + throw std::runtime_error( + "Duplicate imu port/sn found for indexing pcap: " + sn_imu + + ":" + std::to_string(*it.config.udp_port_imu)); + } + port_map_[*it.config.udp_port_imu][sn_imu] = index; + + index++; + } +} nonstd::optional IndexedPcapReader::sensor_idx_for_current_packet() const { const auto& pkt_info = current_info(); - for (size_t i = 0; i < sensor_infos_.size(); i++) { - if (pkt_info.dst_port == sensor_infos_[i].config.udp_port_lidar) { - // TODO use the packet format and match serial number if it's - // available this will allow us to have multiple sensors on the same - // port - return i; + auto temp_match = port_map_.find(pkt_info.dst_port); + if (temp_match != port_map_.end()) { + for (auto it : temp_match->second) { + auto res = validate_packet( + sensor_infos_[it.second], packet_formats_[it.second], data, + pkt_info.payload_size, + ouster::sensor::PacketValidationType::LIDAR); + if (res == ouster::sensor::PacketValidationFailure::NONE) { + return it.second; + } } } + return nonstd::nullopt; } diff --git a/ouster_pcap/src/pcap.cpp b/ouster_pcap/src/pcap.cpp index 53b2b5f5..5c5a7b11 100644 --- a/ouster_pcap/src/pcap.cpp +++ b/ouster_pcap/src/pcap.cpp @@ -17,8 +17,8 @@ #define FTELL _ftelli64 #define FSEEK _fseeki64 #elif defined __EMSCRIPTEN__ -#define FTELL ftell -#define FSEEK fseek +#define FTELL ftello +#define FSEEK fseeko #else #include // inet_ntop #include // timeval diff --git a/ouster_viz/include/ouster/point_viz.h b/ouster_viz/include/ouster/point_viz.h index 320db0dc..e9dcd9e6 100644 --- a/ouster_viz/include/ouster/point_viz.h +++ b/ouster_viz/include/ouster/point_viz.h @@ -13,18 +13,21 @@ #include #include #include +#include #include +#include "nonstd/optional.hpp" + namespace ouster { namespace viz { /** - * @todo document me + * 4x4 matrix of doubles to represent transformations */ using mat4d = std::array; /** - * @todo document me + * 4x4 identity matrix */ constexpr mat4d identity4d = {1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1}; @@ -42,6 +45,49 @@ class GLRings; struct CameraData; } // namespace impl +/** + * @brief A mouse button enum, for use in mouse button event handlers + */ +enum class MouseButton : int { + // Note, this purposefully duplicates GLFW's enum + // so that we can provide our own symbols without + // having to include glfw.h here. + // https://www.glfw.org/docs/latest/group__buttons.html + MOUSE_BUTTON_1 = 0, + MOUSE_BUTTON_2 = 1, + MOUSE_BUTTON_3 = 2, + MOUSE_BUTTON_4 = 3, + MOUSE_BUTTON_5 = 4, + MOUSE_BUTTON_6 = 5, + MOUSE_BUTTON_7 = 6, + MOUSE_BUTTON_8 = 7, + MOUSE_BUTTON_LAST = MOUSE_BUTTON_8, + MOUSE_BUTTON_LEFT = MOUSE_BUTTON_1, + MOUSE_BUTTON_RIGHT = MOUSE_BUTTON_2, + MOUSE_BUTTON_MIDDLE = MOUSE_BUTTON_3, +}; + +/** + * @brief A mouse button event enum, for use in mouse button event handlers + */ +enum class MouseButtonEvent : int { + MOUSE_BUTTON_RELEASED = 0, + MOUSE_BUTTON_PRESSED = 1, +}; + +/** + * @brief An enum for modifier keys, for use in mouse button event handlers + */ +enum class EventModifierKeys : int { + MOD_NONE = 0, + MOD_SHIFT = 0x0001, + MOD_CONTROL = 0x0002, + MOD_ALT = 0x0004, + MOD_SUPER = 0x0008, + MOD_CAPS_LOCK = 0x0010, + MOD_NUM_LOCK = 0x0020, +}; + struct WindowCtx; class Camera; class Cloud; @@ -51,12 +97,12 @@ class Label; class TargetDisplay; /** - * @todo document me + * Sets the default_window_width to 800 */ constexpr int default_window_width = 800; /** - * @todo document me + * Sets the default_window_height to 600 */ constexpr int default_window_height = 600; @@ -72,6 +118,15 @@ constexpr int default_window_height = 600; * rendering (e.g. when streaming data from a running sensor). */ class PointViz { + friend void add_default_controls(viz::PointViz& viz, std::mutex* mx); + + /** + * Get a reference to the current camera controls + * + * @return Handle to the current camera + */ + Camera& current_camera(); + public: struct Impl; @@ -79,9 +134,11 @@ class PointViz { * Creates a window and initializes the rendering context * * @param[in] name name of the visualizer, shown in the title bar - * @param[in] fix_aspect @todo document me - * @param[in] window_width @todo document me - * @param[in] window_height @todo document me + * @param[in] fix_aspect Window aspect to set + * @param[in] window_width Window width to set, + * else uses the default_window_width + * @param[in] window_height Window height to set, + * else uses the default_window_height */ PointViz(const std::string& name, bool fix_aspect = false, int window_width = default_window_width, @@ -127,20 +184,6 @@ class PointViz { */ void visible(bool state); - /** - * Check if viz update_on_input state - * - * @return true if the viz will update on input - */ - bool update_on_input(); - - /** - * Set viz update_on_input flag. - * - * @param[in] state new value of the flag - */ - void update_on_input(bool state); - /** * Update visualization state * @@ -157,32 +200,56 @@ class PointViz { * * @param[in] f the callback. The second argument is the ascii value of the * key pressed. Third argument is a bitmask of the modifier keys + * The callback's return value determines whether + * the remaining key callbacks should be called. */ void push_key_handler(std::function&& f); /** * Add a callback for handling mouse button input * - * @param[in] f @todo document me + * @param[in] f the callback. The callback's arguments are + * ctx: the context containing information about the buttons + * pressed, the mouse position, and the viewport; + * button: the mouse button pressed; + * mods: representing which modifier keys are pressed + * during the mouse click. + * The callback's return value determines whether + * the remaining mouse button callbacks should be called. */ void push_mouse_button_handler( - std::function&& f); + std::function&& f); /** * Add a callback for handling mouse scrolling input * - * @param[in] f @todo document me + * @param[in] f the callback. The callback's arguments are + * ctx: the context containing information about the buttons + * pressed, the mouse position, and the viewport; + * x: the amount of scrolling in the x direction; + * y: the amount of scrolling in the y direction. + * The callback's return value determines whether + * the remaining mouse scroll callbacks should be called. */ void push_scroll_handler( - std::function&& f); + std::function&& f); /** * Add a callback for handling mouse movement * - * @param[in] f @todo document me + * @param[in] f the callback. The callback's arguments are + * ctx: the context containing information about the buttons + * pressed, the mouse position, and the viewport; + * x: the mouse position in the x direction; + * y: the mouse position in the y direction. + * The callback's return value determines whether + * the remaining mouse position callbacks should be called. */ void push_mouse_pos_handler( - std::function&& f); + std::function&& f); /** * Add a callback for processing every new draw frame buffer. @@ -192,102 +259,126 @@ class PointViz { * for further processing. * * @param[in] f function callback of a form f(fb_data, fb_width, fb_height) + * The callback's return value determines whether + * the remaining frame buffer callbacks should be called. */ void push_frame_buffer_handler( std::function&, int, int)>&& f); /** - * Remove the last added callback for handling keyboard input + * Remove the last added callback for handling keyboard events */ void pop_key_handler(); /** - * @copydoc pop_key_handler() + * Remove the last added callback for handling mouse button events */ void pop_mouse_button_handler(); /** - * @copydoc pop_key_handler() + * Remove the last added callback for handling mouse scroll events */ void pop_scroll_handler(); /** - * @copydoc pop_key_handler() + * Remove the last added callback for handling mouse position events */ void pop_mouse_pos_handler(); /** - * @copydoc pop_key_handler() + * Remove the last added callback for handling frame buffer events */ void pop_frame_buffer_handler(); + /** + * Add a callback for handling frame buffer resize events. + * @param[in] f function callback of the form f(const WindowCtx&). The + * callback's return value determines whether the remaining frame buffer + * resize callbacks should be called. + */ + void push_frame_buffer_resize_handler( + std::function&& f); + + /** + * Remove the last added callback for handling frame buffer resize events. + */ + void pop_frame_buffer_resize_handler(); + /** * Get a reference to the camera controls * - * @return @todo document me + * @return Handler to the camera object */ Camera& camera(); /** * Get a reference to the target display controls * - * @return @todo document me + * @return Handler to the target display controls */ TargetDisplay& target_display(); /** * Add an object to the scene * - * @param[in] cloud @todo document me + * @param[in] cloud Adds a point cloud to the scene */ void add(const std::shared_ptr& cloud); /** * Add an object to the scene * - * @param[in] image @todo document me + * @param[in] image Adds an image to the scene */ void add(const std::shared_ptr& image); /** * Add an object to the scene * - * @param[in] cuboid @todo document me + * @param[in] cuboid Adds a cuboid to the scene */ void add(const std::shared_ptr& cuboid); /** * Add an object to the scene * - * @param[in] label @todo document me + * @param[in] label Adds a label to the scene */ void add(const std::shared_ptr