diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 158c63d4..cb8cd30d 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,6 +2,60 @@ Changelog ========= +[unreleased] +============ + +ouster_client +-------------- +* breaking change: signal multiplier type changed to double to support new FW values of signal + multiplier. +* breaking change: make_xyz_lut takes mat4d beam_to_lidar_transform instead of + lidar_origin_to_beam_origin_mm double to accomodate new FWs. Old reference Python implementation + was kept, but new reference was also added. +* address an issue that could cause the processed frame being dropped in favor or the previous + frame when the frame_id wraps-around. +* added a new flag ``CONFIG_FORCE_REINIT`` for ``set_config()`` method, to force the sensor to reinit + even when config params have not changed. +* breaking change: drop defaults parameters from the shortform ``init_client()`` method. +* added a new method ``init_logger()`` to provide control over the logs emitted by ``ouster_client``. +* add parsing for new FW 3.0 thermal features shot_limiting and thermal_shutdown statuses and countdowns +* add frame_status to LidarScan + +python +------ +* breaking change: drop defaults parameters of ``client.Sensor`` constructor. +* breaking change: change Scans interface Timeout to default to 1 second instead of None +* added a new method ``init_logger()`` to provide control over the logs emitted by ``client.Sensor``. +* add ``client.LidarScan`` methods ``__repr__()`` and ``__str__()``. + +ouster_viz +---------- +* add ``SimpleViz.screenshot()`` function and a key handler ``SHIFT-Z`` to + save a screenshot. Can be called from a client thread, and executes + asyncronously (i.e. returns immediately and pushes a one off callback + to frame buffer handlers) +* add ``PointViz.viewport_width()`` and ``PointViz.viewport_height()`` functions +* add ``PointViz.push/pop_frame_buffer_handler()`` to attach a callbacks on + every frame draw update that calls from the main rendering loop. +* add ``SHIFT-X`` key to SimpleViz to start continuous saving of screenshots + on every draw operation. (good for making videos) +* expose ``Camera.set_target`` function through pybind + +ouster-sdk +---------- +* Moved ouster_ros to its own repo +* pin ``openssl`` Conan package dependency to ``openssl/1.1.1s`` due to + ``libtins`` and ``libcurl`` conflicting ``openssl`` versions + + +[20220927] +========== + +ouster_client +-------------- +* fix a bug in longform ``init_client()`` which was not setting timestamp_mode and lidar_mode correctly + + [20220826] ========== @@ -43,7 +97,8 @@ ouster_ros * drop FW 1.13 compatibility for sensors and recorded bags * remove setting of EIGEN_MAX_ALIGN_BYTES * add two new ros services /ouster/get_config and /ouster/set_config (experimental) -* Add new timestamp_mode TIME_FROM_ROS_TIME +* add new timestamp_mode TIME_FROM_ROS_TIME +* declare PCL_NO_PRECOMPILE ahead of all PCL library includes [20220608] diff --git a/CMakeLists.txt b/CMakeLists.txt index 763f75ee..7ecf8aae 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,16 +1,21 @@ cmake_minimum_required(VERSION 3.10...3.22) list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake) + +# for installation, install only custom find scripts which are not shipped by package +# managers or cmake itself. This does not apply e.g. to eigen +set(DEPENDENCY_MODULES ${CMAKE_CURRENT_LIST_DIR}/cmake/Findjsoncpp.cmake) + include(DefaultBuildType) # configure vcpkg from environment variables, if present include(VcpkgEnv) # ==== Project Name ==== -project(ouster_example VERSION 20220826) +project(ouster_example VERSION 20220927) # generate version header -set(OusterSDK_VERSION_STRING 0.5.1) +set(OusterSDK_VERSION_STRING 0.7.1.b1) include(VersionGen) # ==== Options ==== @@ -88,7 +93,7 @@ set(CPACK_SOURCE_IGNORE_FILES /.git /dist) set(CPACK_DEBIAN_PACKAGE_NAME ouster-sdk) set(CPACK_DEBIAN_FILE_NAME DEB-DEFAULT) set(CPACK_DEBIAN_PACKAGE_DEPENDS - "libjsoncpp-dev, libeigen3-dev, libtins-dev, libglfw3-dev, libglew-dev") + "libjsoncpp-dev, libeigen3-dev, libtins-dev, libglfw3-dev, libglew-dev, libspdlog-dev") include(CPack) @@ -109,6 +114,18 @@ install(FILES "${CMAKE_CURRENT_BINARY_DIR}/OusterSDKConfig.cmake" "${CMAKE_CURRENT_BINARY_DIR}/OusterSDKConfigVersion.cmake" DESTINATION lib/cmake/OusterSDK) -install(FILES LICENSE - DESTINATION share/doc/${CPACK_DEBIAN_PACKAGE_NAME} - RENAME copyright) +# we install any custom dependency modules here +install(FILES ${DEPENDENCY_MODULES} + DESTINATION lib/cmake/OusterSDK/Modules) +install(FILES LICENSE LICENSE-bin + DESTINATION share) + +# make uninstall +add_custom_target("uninstall" COMMENT "Uninstall installed files") +add_custom_command( + TARGET "uninstall" + POST_BUILD + COMMENT "Uninstall files with install_manifest.txt" + COMMAND xargs rm -vf < install_manifest.txt || echo Nothing in + install_manifest.txt to be uninstalled! +) diff --git a/LICENSE-bin b/LICENSE-bin index d86c379a..13208146 100644 --- a/LICENSE-bin +++ b/LICENSE-bin @@ -253,3 +253,31 @@ License: Curl License Except as contained in this notice, the name of a copyright holder shall not be used in advertising or otherwise to promote the sale, use or other dealings in this Software without prior written authorization of the copyright holder. + +Name: spdlog +Description: statically linked +Availability: https://github.com/gabime/spdlog +License: MIT + Copyright (c) 2016 Gabi Melman. + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. + + -- NOTE: Third party dependency used by this software -- + This software depends on the fmt lib (MIT License), + and users must comply to its license: https://github.com/fmtlib/fmt/blob/master/LICENSE.rst \ No newline at end of file diff --git a/README.rst b/README.rst index fea34d45..71b964d0 100644 --- a/README.rst +++ b/README.rst @@ -21,6 +21,20 @@ reading and visualizing data. * `ouster_viz `_ contains a customizable point cloud visualizer * `python `_ contains the code for the ouster sensor python SDK (``ouster-sdk`` Python package) +.. note:: + Ouster ROS driver code has been moved out to a separate GitHub repository. To get started using the + driver follow the instructions provided on the repository's main page: https://github.com/ouster-lidar/ouster-ros + + +Contact +======= + +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 `_. + +For suspected security problems, please contact us at security@ouster.io. + License ======= diff --git a/cmake/OusterSDKConfig.cmake.in b/cmake/OusterSDKConfig.cmake.in index d6e97567..383b160c 100644 --- a/cmake/OusterSDKConfig.cmake.in +++ b/cmake/OusterSDKConfig.cmake.in @@ -2,16 +2,18 @@ message(STATUS "Found OusterSDK: ${CMAKE_CURRENT_LIST_FILE}") include(CMakeFindDependencyMacro) +set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_LIST_DIR}/Modules/") + # ouster_client dependencies find_dependency(Eigen3) find_dependency(jsoncpp) find_dependency(CURL) +find_dependency(spdlog) # viz dependencies if(@BUILD_VIZ@) set(OpenGL_GL_PREFERENCE GLVND) find_dependency(OpenGL) - find_dependency(Threads) find_dependency(glfw3) if(@OUSTER_VIZ_USE_GLAD@) diff --git a/conanfile.py b/conanfile.py index fb5d64ef..008608e4 100644 --- a/conanfile.py +++ b/conanfile.py @@ -56,8 +56,15 @@ def config_options(self): del self.options.fPIC def requirements(self): + # not required directly here but because libtins and libcurl pulling + # slightly different versions of zlib and openssl we need to set it + # here explicitly + self.requires("zlib/1.2.13") + self.requires("openssl/1.1.1s") + self.requires("eigen/3.4.0") self.requires("jsoncpp/1.9.5") + self.requires("spdlog/1.10.0") self.requires("libcurl/7.82.0") if self.options.build_pcap: diff --git a/docs/cpp/building.rst b/docs/cpp/building.rst index cecc44d1..4e9c3721 100644 --- a/docs/cpp/building.rst +++ b/docs/cpp/building.rst @@ -4,7 +4,7 @@ Building the C++ Client from Source =================================== -Building the example code requires a compiler supporting C++11 and CMake 3.1 or newer and the +Building the example code requires a compiler supporting C++14 and CMake 3.1 or newer and the jsoncpp, Eigen3, and tins libraries with headers installed on the system. The sample visualizer also requires the GLFW3 and GLEW libraries. @@ -19,7 +19,7 @@ To install build dependencies on Ubuntu, run: .. code:: console $ sudo apt install build-essential cmake libjsoncpp-dev libeigen3-dev libcurl4-openssl-dev \ - libtins-dev libpcap-dev libglfw3-dev libglew-dev + libtins-dev libpcap-dev libglfw3-dev libglew-dev libspdlog-dev You may also install curl with a different ssl backend, for example libcurl4-gnutls-dev or libcurl4-nss-dev. diff --git a/docs/index.rst b/docs/index.rst index 69d74b94..94a75662 100644 --- a/docs/index.rst +++ b/docs/index.rst @@ -27,7 +27,7 @@ :caption: ROS1 Guide :hidden: - Build and Use + Build and Use .. toctree:: :caption: SDK Reference diff --git a/docs/installation.rst b/docs/installation.rst index 406c7305..676e215c 100644 --- a/docs/installation.rst +++ b/docs/installation.rst @@ -119,3 +119,9 @@ C++ The Ouster C++ SDK currently must be built and installed from sources. Please refer to :doc:`/cpp/building`. + + +ROS +=== +Ouster ROS driver code has been moved out to a separate GitHub repository. To get started using the +driver follow the instructions provided on the repository's main page: https://github.com/ouster-lidar/ouster-ros diff --git a/docs/python/devel.rst b/docs/python/devel.rst index 70d950c3..baf0f168 100644 --- a/docs/python/devel.rst +++ b/docs/python/devel.rst @@ -18,6 +18,7 @@ Building the Python SDK from source requires several dependencies: - `libpcap `_ - `libglfw3 `_ >= 3.2 - `libglew `_ >= 2.1 or `glad `_ +- `spdlog `_ >= 1.9 - `Python `_ >= 3.7 (with headers and development libraries) - `pybind11 `_ >= 2.0 @@ -33,7 +34,7 @@ On supported Debian-based linux systems, you can install all build dependencies $ sudo apt install build-essential cmake \ libeigen3-dev libjsoncpp-dev libtins-dev libpcap-dev \ python3-dev python3-pip pybind11-dev libcurl4-openssl-dev \ - libglfw3-dev libglew-dev + libglfw3-dev libglew-dev libspdlog-dev On macos >= 10.13, using homebrew, you should be able to run: diff --git a/docs/python/viz/viz-run.rst b/docs/python/viz/viz-run.rst index 65d50149..43b4c1ed 100644 --- a/docs/python/viz/viz-run.rst +++ b/docs/python/viz/viz-run.rst @@ -56,6 +56,8 @@ Keyboard controls: ``ctrl + ./,`` Step 10 frames forward/back ``>/<`` Increase/decrease playback rate (during replay) ``shift`` Camera Translation with mouse drag + ``shift-z`` Save a screenshot of the current view + ``shift-x`` Toggle a continuous saving of screenshots ============== =============================================== .. diff --git a/examples/client_example.cpp b/examples/client_example.cpp index 36320c14..d28b419e 100644 --- a/examples/client_example.cpp +++ b/examples/client_example.cpp @@ -9,8 +9,8 @@ #include #include -#include "ouster/impl/build.h" #include "ouster/client.h" +#include "ouster/impl/build.h" #include "ouster/lidar_scan.h" #include "ouster/types.h" @@ -36,6 +36,11 @@ int main(int argc, char* argv[]) { return argc == 1 ? EXIT_SUCCESS : EXIT_FAILURE; } + + // Limit ouster_client log statements to "info" and direct the output to log + // file rather than the console (default). + sensor::init_logger("info", "ouster.log"); + std::cerr << "Ouster client example " << ouster::SDK_VERSION << std::endl; /* * The sensor client consists of the network client and a library for @@ -50,11 +55,11 @@ int main(int argc, char* argv[]) { const std::string sensor_hostname = argv[1]; const std::string data_destination = (argc == 3) ? argv[2] : ""; - std::cerr << "Connecting to \"" << sensor_hostname << "\"... "; + std::cerr << "Connecting to \"" << sensor_hostname << "\"...\n"; auto handle = sensor::init_client(sensor_hostname, data_destination); if (!handle) FATAL("Failed to connect"); - std::cerr << "ok" << std::endl; + std::cerr << "Connection to sensor succeeded" << std::endl; /* * Configuration and calibration parameters can be queried directly from the diff --git a/examples/representations_example.cpp b/examples/representations_example.cpp index c0501f66..d35813ae 100644 --- a/examples/representations_example.cpp +++ b/examples/representations_example.cpp @@ -13,8 +13,8 @@ #include #include "helpers.h" -#include "ouster/impl/build.h" #include "ouster/client.h" +#include "ouster/impl/build.h" #include "ouster/lidar_scan.h" #include "ouster/types.h" @@ -101,8 +101,8 @@ int main(int argc, char* argv[]) { // say a vehicle center //! [doc-stag-extrinsics-to-xyzlut] auto lut_extrinsics = make_xyz_lut( - w, h, sensor::range_unit, info.lidar_origin_to_beam_origin_mm, - transformation, info.beam_azimuth_angles, info.beam_altitude_angles); + w, h, sensor::range_unit, info.beam_to_lidar_transform, transformation, + info.beam_azimuth_angles, info.beam_altitude_angles); std::cerr << "Calculating 3d Points of with special transform provided.." << std::endl; diff --git a/ouster_client/CMakeLists.txt b/ouster_client/CMakeLists.txt index 9cce683e..049b4f48 100644 --- a/ouster_client/CMakeLists.txt +++ b/ouster_client/CMakeLists.txt @@ -2,18 +2,20 @@ find_package(Eigen3 REQUIRED) find_package(jsoncpp REQUIRED) find_package(CURL REQUIRED) +find_package(spdlog REQUIRED) # ==== Libraries ==== add_library(ouster_client src/client.cpp src/types.cpp src/netcompat.cpp src/lidar_scan.cpp src/image_processing.cpp src/buffered_udp_source.cpp src/parsing.cpp - src/sensor_http.cpp src/sensor_http_imp.cpp src/sensor_tcp_imp.cpp) + src/sensor_http.cpp src/sensor_http_imp.cpp src/sensor_tcp_imp.cpp src/logging.cpp) target_link_libraries(ouster_client PUBLIC Eigen3::Eigen $ PRIVATE CURL::libcurl - jsoncpp_lib) + jsoncpp_lib + spdlog::spdlog) target_compile_definitions(ouster_client PRIVATE EIGEN_MPL2_ONLY) add_library(OusterSDK::ouster_client ALIAS ouster_client) diff --git a/ouster_client/include/ouster/client.h b/ouster_client/include/ouster/client.h index 8bb5c844..26a9cdcc 100644 --- a/ouster_client/include/ouster/client.h +++ b/ouster_client/include/ouster/client.h @@ -32,6 +32,33 @@ enum client_state { /** Minimum supported version. */ const util::version min_version = {1, 12, 0}; +/** + * Initializes and configures ouster_client logs. This method should be invoked + * only once before calling any other method from the library if the user wants + * to direct the library log statements to a different medium (other than + * console which is the default). + * + * @param[in] log_level Control the level of log messages outputed by the + * client. Valid options are (case-sensitive): "trace", "debug", "info", + * "warning", "error", "critical" and "off". + * @param[in] log_file_path Path to location where log files are stored. The + * path must be in a location that the process has write access to. If an empty + * string is provided then the logs will be directed to the console. When an + * empty string is passed then the rest of parameters are ignored. + * @param[in] rotating Configure the log file with rotation, rotation rules are + * specified through the two following parameters max_size_in_bytes and + * max_files. If rotating is set to false the following parameters are ignored. + * @param[in] max_size_in_bytes Maximum number of bytes to write to a rotating + * log file before starting a new file. ignored if rotating is set to false. + * @param[in] max_files Maximum number of rotating files to accumlate before + * re-using the first file. ignored if rotating is set to false. + * + * @return true on success, otherwise false. + */ +bool init_logger(const std::string& log_level, + const std::string& log_file_path = "", bool rotating = false, + int max_size_in_bytes = 0, int max_files = 0); + /** \defgroup ouster_client_init Ouster Client Client Initialization * @{ */ @@ -45,8 +72,8 @@ const util::version min_version = {1, 12, 0}; * * @return pointer owning the resources associated with the connection. */ -std::shared_ptr init_client(const std::string& hostname = "", - int lidar_port = 7502, int imu_port = 7503); +std::shared_ptr init_client(const std::string& hostname, int lidar_port, + int imu_port); /** * Connect to and configure the sensor and start listening for data. @@ -54,17 +81,19 @@ std::shared_ptr init_client(const std::string& hostname = "", * @param[in] hostname hostname or ip of the sensor. * @param[in] udp_dest_host hostname or ip where the sensor should send data * or "" for automatic detection of destination. - * @param[in] mode The lidar mode to use. + * @param[in] ld_mode The lidar mode to use. * @param[in] ts_mode The timestamp mode to use. - * @param[in] lidar_port port on which the sensor will send lidar data. - * @param[in] imu_port port on which the sensor will send imu data. + * @param[in] lidar_port port on which the sensor will send lidar data. When + * using zero the method will automatically acquire and assign any free port. + * @param[in] imu_port port on which the sensor will send imu data. When + * using zero the method will automatically acquire and assign any free port. * @param[in] timeout_sec how long to wait for the sensor to initialize. * * @return pointer owning the resources associated with the connection. */ std::shared_ptr init_client(const std::string& hostname, const std::string& udp_dest_host, - lidar_mode mode = MODE_UNSPEC, + lidar_mode ld_mode = MODE_UNSPEC, timestamp_mode ts_mode = TIME_FROM_UNSPEC, int lidar_port = 0, int imu_port = 0, int timeout_sec = 60); @@ -138,13 +167,18 @@ std::string get_metadata(client& cli, int timeout_sec = 60, bool get_config(const std::string& hostname, sensor_config& config, bool active = true); +// clang-format off /** * Flags for set_config() */ enum config_flags : uint8_t { - CONFIG_UDP_DEST_AUTO = (1 << 0), ///< Set udp_dest automatically - CONFIG_PERSIST = (1 << 1) ///< Make configuration persistent + CONFIG_UDP_DEST_AUTO = (1 << 0), ///< Set udp_dest automatically + CONFIG_PERSIST = (1 << 1), ///< Make configuration persistent + CONFIG_FORCE_REINIT = (1 << 2) ///< Forces the sensor to re-init during + ///< set_config even when config params + ///< have not changed }; +// clang-format on /** * Set sensor config on sensor. diff --git a/ouster_client/include/ouster/impl/lidar_scan_impl.h b/ouster_client/include/ouster/impl/lidar_scan_impl.h index d9a55531..03740a78 100644 --- a/ouster_client/include/ouster/impl/lidar_scan_impl.h +++ b/ouster_client/include/ouster/impl/lidar_scan_impl.h @@ -281,6 +281,36 @@ void foreach_field(SCAN&& ls, OP&& op, Args&&... args) { ft.first, std::forward(args)...); } +// Read LidarScan field and cast to the destination +struct read_and_cast { + template + void operator()(Eigen::Ref> src, Eigen::Ref> dest) { + dest = src.template cast(); + } + template + void operator()(Eigen::Ref> src, Eigen::Ref> dest) { + dest = src.template cast(); + } + template + void operator()(Eigen::Ref> src, img_t& dest) { + dest = src.template cast(); + } + template + void operator()(Eigen::Ref> src, img_t& dest) { + dest = src.template cast(); + } +}; + +// Copy fields from `ls_source` LidarScan to `field_dest` img with casting +// to the img_t type of `field_dest`. +struct copy_and_cast { + template + void operator()(Eigen::Ref> field_dest, const LidarScan& ls_source, + sensor::ChanField ls_source_field) { + visit_field(ls_source, ls_source_field, read_and_cast(), + field_dest); + } +}; } // namespace impl template diff --git a/ouster_client/include/ouster/lidar_scan.h b/ouster_client/include/ouster/lidar_scan.h index 870b6b47..9e655370 100644 --- a/ouster_client/include/ouster/lidar_scan.h +++ b/ouster_client/include/ouster/lidar_scan.h @@ -1,8 +1,5 @@ /** - * Copyright (c) 2018, Ouster, Inc. - * All rights reserved. - * - * @file + * Copyright (c) 2018, Ouster, Inc. All rights reserved. @file * @brief Holds lidar data by field in row-major order */ @@ -26,6 +23,12 @@ namespace impl { struct FieldSlot; } +/** + * Alias for the lidar scan field types + */ +using LidarScanFieldTypes = + std::vector>; + /** * Data structure for efficient operations on aggregated lidar data. * @@ -82,12 +85,9 @@ class LidarScan { Header measurement_id_; Header status_; std::map fields_; - std::vector> - field_types_; + LidarScanFieldTypes field_types_; - LidarScan(size_t w, size_t h, - std::vector> - field_types); + LidarScan(size_t w, size_t h, LidarScanFieldTypes field_types); public: /** @@ -116,6 +116,15 @@ class LidarScan { */ [[deprecated]] std::vector headers{}; + /** + * Frame status - information from the packet header which corresponds to a + * frame + * + * @warning Member variables: use with caution, some of these will become + * private. + */ + uint64_t frame_status{0}; + /** * The current frame ID. * @@ -190,6 +199,16 @@ class LidarScan { */ ~LidarScan(); + /** + * Get frame shot limiting status + */ + sensor::ShotLimitingStatus shot_limiting() const; + + /** + * Get frame thermal shutdown status + */ + sensor::ThermalShutdownStatus thermal_shutdown() const; + /** * Access timestamps as a vector. * @@ -291,6 +310,42 @@ class LidarScan { friend bool operator==(const LidarScan& a, const LidarScan& b); }; +/** + * Get string representation of lidar scan field types. + * + * @param[in] field_types The field types to get the string representation of. + * + * @return string representation of the lidar scan field types. + */ +std::string to_string(const LidarScanFieldTypes& field_types); + +/** + * Get the lidar scan field types from a lidar scan + * + * @param[in] ls The lidar scan to get the lidar scan field types from. + * + * @return The lidar scan field types + */ +LidarScanFieldTypes get_field_types(const LidarScan& ls); + +/** + * Get the lidar scan field types from sensor info + * + * @param[in] info The sensor info to get the lidar scan field types from. + * + * @return The lidar scan field types + */ +LidarScanFieldTypes get_field_types(const sensor::sensor_info& info); + +/** + * Get string representation of a lidar scan. + * + * @param[in] ls The lidar scan to get the string representation of. + * + * @return string representation of the lidar scan. + */ +std::string to_string(const LidarScan& ls); + /** \defgroup ouster_client_lidar_scan_operators Ouster Client lidar_scan.h * Operators * @{ @@ -354,8 +409,8 @@ struct XYZLut { * @param[in] h number of rows in the lidar scan. * @param[in] range_unit the unit, in meters, of the range, e.g. * sensor::range_unit. - * @param[in] lidar_origin_to_beam_origin_mm the radius to the beam origin point - * of the unit, in millimeters. + * @param[in] beam_to_lidar_transform, signifying transform between beams and + * lidar origin. Translation portion is in millimeters. * @param[in] transform additional transformation to apply to resulting points. * @param[in] azimuth_angles_deg azimuth offsets in degrees for each of h beams. * @param[in] altitude_angles_deg altitude in degrees for each of h beams. @@ -363,7 +418,7 @@ struct XYZLut { * @return xyz direction and offset vectors for each point in the lidar scan. */ XYZLut make_xyz_lut(size_t w, size_t h, double range_unit, - double lidar_origin_to_beam_origin_mm, + const mat4d& beam_to_lidar_transform, const mat4d& transform, const std::vector& azimuth_angles_deg, const std::vector& altitude_angles_deg); @@ -378,7 +433,7 @@ XYZLut make_xyz_lut(size_t w, size_t h, double range_unit, 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.lidar_origin_to_beam_origin_mm, + sensor::range_unit, sensor.beam_to_lidar_transform, sensor.lidar_to_sensor_transform, sensor.beam_azimuth_angles, sensor.beam_altitude_angles); } diff --git a/ouster_client/include/ouster/types.h b/ouster_client/include/ouster/types.h index e4df9a66..4a9344a3 100644 --- a/ouster_client/include/ouster/types.h +++ b/ouster_client/include/ouster/types.h @@ -180,14 +180,42 @@ enum UDPProfileIMU { PROFILE_IMU_LEGACY = 1 ///< Legacy IMU data }; +/** Thermal Shutdown status. */ +enum ThermalShutdownStatus { + THERMAL_SHUTDOWN_NORMAL = 0x00, ///< Normal operation + THERMAL_SHUTDOWN_IMMINENT = 0x01, ///< Thermal Shutdown imminent +}; + +/** Shot Limiting status. */ +enum ShotLimitingStatus { + SHOT_LIMITING_NORMAL = 0x00, ///< Normal operation + SHOT_LIMITING_IMMINENT = 0x01, ///< Shot limiting imminent + SHOT_LIMITING_REDUCTION_0_10 = + 0x02, //< Shot limiting reduction by 0 to 10% + SHOT_LIMITING_REDUCTION_10_20 = + 0x03, ///< Shot limiting reduction by 10 to 20% + SHOT_LIMITING_REDUCTION_20_30 = + 0x04, ///< Shot limiting reduction by 20 to 30% + SHOT_LIMITING_REDUCTION_30_40 = + 0x05, ///< Shot limiting reduction by 30 to 40% + SHOT_LIMITING_REDUCTION_40_50 = + 0x06, ///< Shot limiting reduction by 40 to 50% + SHOT_LIMITING_REDUCTION_50_60 = + 0x07, ///< Shot limiting reduction by 50 to 60% + SHOT_LIMITING_REDUCTION_60_70 = + 0x08, ///< Shot limiting reduction by 60 to 70% + SHOT_LIMITING_REDUCTION_70_75 = + 0x09, ///< Shot limiting reduction by 70 to 80% +}; + /** - * Convenience type alias for azimuth windows, the window over which the sensor - * fires in millidegrees. + * Convenience type alias for azimuth windows, the window over which the + * sensor fires in millidegrees. */ using AzimuthWindow = std::pair; /** - * Convenience type alias for column windows, the window over which the sensor - * fires in columns. + * Convenience type alias for column windows, the window over which the + * sensor fires in columns. */ using ColumnWindow = std::pair; @@ -197,10 +225,10 @@ using ColumnWindow = std::pair; struct sensor_config { optional udp_dest; ///< The destination address for the ///< lidar/imu data to be sent to - optional udp_port_lidar; ///< The destination port for the lidar data - ///< to be sent to - optional - udp_port_imu; ///< The destination port for the imu data to be sent to + optional udp_port_lidar; ///< The destination port for the lidar + ///< data to be sent to + optional udp_port_imu; ///< The destination port for the imu data + ///< to be sent to // TODO: replace ts_mode and ld_mode when timestamp_mode and // lidar_mode get changed to CapsCase @@ -235,10 +263,10 @@ struct sensor_config { optional azimuth_window; /** - * Multiplier for signal strength of sensor. See the sensor docs for more - * details on usage. + * Multiplier for signal strength of sensor. See the sensor docs for + * more details on usage. */ - optional signal_multiplier; + optional signal_multiplier; /** * The nmea polarity for the sensor to use. @@ -277,8 +305,8 @@ struct sensor_config { optional sync_pulse_out_polarity; /** - * Angle in degrees that sensor traverses between each SYNC_PULSE_OUT pulse. - * See senor docs for more details. + * Angle in degrees that sensor traverses between each SYNC_PULSE_OUT + * pulse. See senor docs for more details. */ optional sync_pulse_out_angle; @@ -354,8 +382,9 @@ struct sensor_info { beam_altitude_angles; ///< beam altitude angles for 3D projection double lidar_origin_to_beam_origin_mm; ///< distance between lidar origin ///< and beam origin in mm - mat4d imu_to_sensor_transform; ///< transform between sensor coordinate - ///< frame and imu + mat4d beam_to_lidar_transform; ///< transform between beam and lidar frame + mat4d imu_to_sensor_transform; ///< transform between sensor coordinate + ///< frame and imu mat4d lidar_to_sensor_transform; ///< transform between lidar and sensor ///< coordinate frames mat4d extrinsic; ///< extrinsic matrix @@ -606,6 +635,26 @@ std::string to_string(UDPProfileIMU profile); */ optional udp_profile_imu_of_string(const std::string& s); +/** + * Get string representation of a Shot Limiting Status. + * + * @param[in] shot_limiting_status The shot limiting status to get the string + * representation of. + * + * @return string representation of the shot limiting status. + */ +std::string to_string(ShotLimitingStatus shot_limiting_status); + +/** + * Get string representation of Thermal Shutdown Status. + * + * @param[in] thermal_shutdown_status The thermal shutdown status to get the + * string representation of. + * + * @return string representation of thermal shutdown status. + */ +std::string to_string(ThermalShutdownStatus thermal_shutdown_status); + /** * Parse metadata text blob from the sensor into a sensor_info struct. * @@ -730,6 +779,15 @@ std::string to_string(ChanField field); */ enum ChanFieldType { VOID = 0, UINT8, UINT16, UINT32, UINT64 }; +/** + * Get string representation of a channel field. + * + * @param[in] ft The field type to get the string representation of. + * + * @return string representation of the channel field type. + */ +std::string to_string(ChanFieldType ft); + /** * Table of accessors for extracting data from imu and lidar packets. * @@ -805,6 +863,42 @@ class packet_format final { */ uint64_t prod_sn(const uint8_t* lidar_buf) const; + /** + * Read the packet thermal shutdown countdown + * + * @param[in] lidar_buf the lidar buf. + * + * @return the thermal shutdown countdown. + */ + uint16_t countdown_thermal_shutdown(const uint8_t* lidar_buf) const; + + /** + * Read the packet shot limiting countdown + * + * @param[in] liar_buf the lidar buf. + * + * @return the shot limiting countdown. + */ + uint16_t countdown_shot_limiting(const uint8_t* lidar_buf) const; + + /** + * Read the packet thermal shutdown header. + * + * @param[in] lidar_buf the lidar buf. + * + * @return the thermal shutdown status + */ + uint8_t thermal_shutdown(const uint8_t* lidar_buf) const; + + /** + * Read the packet shot limiting header. + * + * @param[in] lidar_buf the lidar buf. + * + * @return the shot limiting status + */ + uint8_t shot_limiting(const uint8_t* lidar_buf) const; + /** * Get the bit width of the specified channel field. * diff --git a/ouster_client/src/buffered_udp_source.cpp b/ouster_client/src/buffered_udp_source.cpp index 16ac2af3..a6b8bcd8 100644 --- a/ouster_client/src/buffered_udp_source.cpp +++ b/ouster_client/src/buffered_udp_source.cpp @@ -39,9 +39,8 @@ constexpr size_t packet_size = 65536; BufferedUDPSource::BufferedUDPSource(size_t buf_size) : capacity_{buf_size + 1} { std::generate_n(std::back_inserter(bufs_), capacity_, [&] { - return std::make_pair( - client_state::CLIENT_ERROR, - std::make_unique(packet_size)); + return std::make_pair(client_state::CLIENT_ERROR, + std::make_unique(packet_size)); }); } diff --git a/ouster_client/src/client.cpp b/ouster_client/src/client.cpp index 82fb47de..6c35c62a 100644 --- a/ouster_client/src/client.cpp +++ b/ouster_client/src/client.cpp @@ -14,7 +14,6 @@ #include #include #include -#include #include #include #include @@ -23,12 +22,14 @@ #include #include +#include "logging.h" #include "netcompat.h" #include "ouster/types.h" #include "sensor_http.h" using namespace std::chrono_literals; namespace chrono = std::chrono; +using ouster::sensor::impl::Logger; using ouster::sensor::util::SensorHttp; namespace ouster { @@ -59,8 +60,7 @@ int32_t get_sock_port(SOCKET sock_fd) { if (!impl::socket_valid( getsockname(sock_fd, (struct sockaddr*)&ss, &addrlen))) { - std::cerr << "udp getsockname(): " << impl::socket_get_error() - << std::endl; + logger().error("udp getsockname(): {}", impl::socket_get_error()); return SOCKET_ERROR; } @@ -84,11 +84,11 @@ SOCKET udp_data_socket(int port) { int ret = getaddrinfo(NULL, port_s.c_str(), &hints, &info_start); if (ret != 0) { - std::cerr << "udp getaddrinfo(): " << gai_strerror(ret) << std::endl; + logger().error("udp getaddrinfo(): {}", gai_strerror(ret)); return SOCKET_ERROR; } if (info_start == NULL) { - std::cerr << "udp getaddrinfo(): empty result" << std::endl; + logger().error("udp getaddrinfo(): empty result"); return SOCKET_ERROR; } @@ -104,8 +104,7 @@ SOCKET udp_data_socket(int port) { SOCKET sock_fd = socket(ai->ai_family, ai->ai_socktype, ai->ai_protocol); if (!impl::socket_valid(sock_fd)) { - std::cerr << "udp socket(): " << impl::socket_get_error() - << std::endl; + logger().warn("udp socket(): {}", impl::socket_get_error()); continue; } @@ -113,36 +112,32 @@ SOCKET udp_data_socket(int port) { if (ai->ai_family == AF_INET6 && setsockopt(sock_fd, IPPROTO_IPV6, IPV6_V6ONLY, (char*)&off, sizeof(off))) { - std::cerr << "udp setsockopt(): " << impl::socket_get_error() - << std::endl; + logger().warn("udp setsockopt(): {}", impl::socket_get_error()); impl::socket_close(sock_fd); continue; } if (impl::socket_set_reuse(sock_fd)) { - std::cerr << "udp socket_set_reuse(): " - << impl::socket_get_error() << std::endl; + logger().warn("udp socket_set_reuse(): {}", + impl::socket_get_error()); } if (::bind(sock_fd, ai->ai_addr, (socklen_t)ai->ai_addrlen)) { - std::cerr << "udp bind(): " << impl::socket_get_error() - << std::endl; + 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)) { - std::cerr << "udp fcntl(): " << impl::socket_get_error() - << std::endl; + 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))) { - std::cerr << "udp setsockopt(): " << impl::socket_get_error() - << std::endl; + logger().warn("udp setsockopt(): {}", impl::socket_get_error()); impl::socket_close(sock_fd); continue; } @@ -154,18 +149,21 @@ SOCKET udp_data_socket(int port) { // could not bind() a UDP server socket freeaddrinfo(info_start); + logger().error("failed to bind udp socket"); return SOCKET_ERROR; } -bool collect_metadata(client& cli, SensorHttp& sensor_http, - chrono::seconds timeout) { - auto timeout_time = chrono::steady_clock::now() + timeout; +Json::Value collect_metadata(const std::string& hostname, int timeout_sec) { + auto sensor_http = SensorHttp::create(hostname); + auto timeout_time = + chrono::steady_clock::now() + chrono::seconds{timeout_sec}; std::string status; + // TODO: can remove this loop when we drop support for FW 2.4 do { if (chrono::steady_clock::now() >= timeout_time) return false; std::this_thread::sleep_for(1s); - status = sensor_http.sensor_info()["status"].asString(); + status = sensor_http->sensor_info()["status"].asString(); } while (status == "INITIALIZING"); // not all metadata available when sensor isn't RUNNING @@ -176,12 +174,10 @@ bool collect_metadata(client& cli, SensorHttp& sensor_http, "WARMUP, or ERROR state"); } - cli.meta = sensor_http.metadata(); - + auto metadata = sensor_http->metadata(); // merge extra info into metadata - cli.meta["client_version"] = client_version(); - - return true; + metadata["client_version"] = client_version(); + return metadata; } } // namespace @@ -199,26 +195,35 @@ bool set_config(const std::string& hostname, const sensor_config& config, auto sensor_http = SensorHttp::create(hostname); // reset staged config to avoid spurious errors - auto active_params = sensor_http->get_config_params(true); - - Json::CharReaderBuilder builder; - auto reader = std::unique_ptr{builder.newCharReader()}; - Json::Value root; - auto parse_success = reader->parse( - active_params.c_str(), active_params.c_str() + active_params.size(), - &root, nullptr); - - if (!parse_success) - throw std::runtime_error("Error while parsing current sensor config."); + auto config_params = sensor_http->active_config_params(); + Json::Value config_params_copy = config_params; // set all desired config parameters Json::Value config_json = to_json(config); for (const auto& key : config_json.getMemberNames()) { - root[key] = config_json[key]; + config_params[key] = config_json[key]; + } + + if (config_json.isMember("operating_mode") && + config_params.isMember("auto_start_flag")) { + // we're setting operating mode and this sensor has a FW with + // auto_start_flag + config_params["auto_start_flag"] = + config_json["operating_mode"] == "NORMAL" ? 1 : 0; } - active_params = Json::FastWriter().write(root); - sensor_http->set_config_param(".", active_params); + // Signal multiplier changed from int to double for FW 3.0/2.5+, with + // corresponding change to config.signal_multiplier. + // Change values 1, 2, 3 back to ints to support older FWs + if (config_params["signal_multiplier"].asDouble() != 0.25 && + config_params["signal_multiplier"].asDouble() != 0.5) { + config_params["signal_multiplier"] = + config_params["signal_multiplier"].asInt(); + // TODO: figure out what to do when people enter invalid signal + // multiplier values between 1 and 3.99. Invalid decimal values <1 and + // >4 will be invalid in any case and the error message won't say so + // those are fine + } // set automatic udp dest, if flag specified if (config_flags & CONFIG_UDP_DEST_AUTO) { @@ -226,10 +231,34 @@ bool set_config(const std::string& hostname, const sensor_config& config, throw std::invalid_argument( "UDP_DEST_AUTO flag set but provided config has udp_dest"); sensor_http->set_udp_dest_auto(); + + auto staged = sensor_http->staged_config_params(); + + // now we set config_params according to the staged udp_dest from the + // sensor + if (staged.isMember("udp_ip")) { // means the FW version carries udp_ip + config_params["udp_ip"] = staged["udp_ip"]; + config_params["udp_dest"] = staged["udp_ip"]; + } else { // don't need to worry about udp_ip + config_params["udp_dest"] = staged["udp_dest"]; + } + } + + // if configuration didn't change then skip applying the params + // note: comparison will fail if config_params contains newer config params + // introduced after the verison of FW the sensor is on + if (config_flags & CONFIG_FORCE_REINIT || + config_params_copy != config_params) { + Json::StreamWriterBuilder builder; + builder["indentation"] = ""; + // 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); + // reinitialize to make all staged parameters effective + sensor_http->reinitialize(); } - // reinitialize to make all staged parameters effective - sensor_http->reinitialize(); // save if indicated if (config_flags & CONFIG_PERSIST) { sensor_http->save_config_params(); @@ -239,10 +268,10 @@ bool set_config(const std::string& hostname, const sensor_config& config, } std::string get_metadata(client& cli, int timeout_sec, bool legacy_format) { - if (!cli.meta) { - auto sensor_http = SensorHttp::create(cli.hostname); - if (!collect_metadata(cli, *sensor_http, chrono::seconds{timeout_sec})) - return ""; + try { + cli.meta = collect_metadata(cli.hostname, timeout_sec); + } catch (const std::exception&) { + return ""; } Json::StreamWriterBuilder builder; @@ -253,10 +282,24 @@ std::string get_metadata(client& cli, int timeout_sec, bool legacy_format) { return legacy_format ? convert_to_legacy(metadata_string) : metadata_string; } +bool init_logger(const std::string& log_level, const std::string& log_file_path, + bool rotating, int max_size_in_bytes, int max_files) { + if (log_file_path.empty()) { + return Logger::instance().configure_stdout_sink(log_level); + } else { + return Logger::instance().configure_file_sink( + log_level, log_file_path, rotating, max_size_in_bytes, max_files); + } +} + std::shared_ptr init_client(const std::string& hostname, int lidar_port, int imu_port) { + logger().info("initializing sensor: {} with ports: {}/{}", hostname, + lidar_port, imu_port); + auto cli = std::make_shared(); cli->hostname = hostname; + cli->lidar_fd = udp_data_socket(lidar_port); cli->imu_fd = udp_data_socket(imu_port); @@ -268,7 +311,7 @@ std::shared_ptr init_client(const std::string& hostname, int lidar_port, std::shared_ptr init_client(const std::string& hostname, const std::string& udp_dest_host, - lidar_mode mode, timestamp_mode ts_mode, + lidar_mode ld_mode, timestamp_mode ts_mode, int lidar_port, int imu_port, int timeout_sec) { auto cli = init_client(hostname, lidar_port, imu_port); @@ -280,50 +323,33 @@ std::shared_ptr init_client(const std::string& hostname, if (!impl::socket_valid(lidar_port) || !impl::socket_valid(imu_port)) return std::shared_ptr(); - bool success = true; - try { - // fail fast if we can't reach the sensor via HTTP - auto sensor_http = SensorHttp::create(hostname); - - // if dest address is not specified, have the sensor to set it - // automatically - if (udp_dest_host.empty()) { - sensor_http->set_udp_dest_auto(); - } else { - sensor_http->set_config_param("udp_dest", udp_dest_host); - } + sensor::sensor_config config; + uint8_t config_flags = 0; + if (udp_dest_host.empty()) + config_flags |= CONFIG_UDP_DEST_AUTO; + else + config.udp_dest = udp_dest_host; + if (ld_mode) config.ld_mode = ld_mode; + if (ts_mode) config.ts_mode = ts_mode; + if (lidar_port) config.udp_port_lidar = lidar_port; + if (imu_port) config.udp_port_imu = imu_port; + config.operating_mode = OPERATING_NORMAL; + set_config(hostname, config, config_flags); - sensor_http->set_config_param("udp_port_lidar", - std::to_string(lidar_port)); - sensor_http->set_config_param("udp_port_imu", std::to_string(imu_port)); - - // if specified (not UNSPEC), set the lidar and timestamp modes - if (mode) { - sensor_http->set_config_param("lidar_mode", std::to_string(mode)); - } - - if (ts_mode) { - sensor_http->set_config_param("timestamp_mode", - std::to_string(ts_mode)); - } - - // wake up from STANDBY, if necessary - sensor_http->set_config_param("operating_mode", "NORMAL"); - sensor_http->reinitialize(); // will block until no longer INITIALIZING - success &= - collect_metadata(*cli, *sensor_http, chrono::seconds{timeout_sec}); + cli->meta = collect_metadata(hostname, timeout_sec); // check for sensor error states auto status = cli->meta["sensor_info"]["status"].asString(); - success &= (status != "ERROR" && status != "UNCONFIGURED"); + if (status == "ERROR" || status == "UNCONFIGURED") + return std::shared_ptr(); } catch (const std::runtime_error& e) { // log error message - std::cerr << "init_client error: " << e.what() << std::endl; + logger().error("init_client(): {}", e.what()); return std::shared_ptr(); } - return success ? cli : std::shared_ptr(); + return cli; } client_state poll_client(const client& c, const int timeout_sec) { @@ -345,7 +371,7 @@ client_state poll_client(const client& c, const int timeout_sec) { if (!impl::socket_valid(retval) && impl::socket_exit()) { res = EXIT; } else if (!impl::socket_valid(retval)) { - std::cerr << "select: " << impl::socket_get_error() << std::endl; + logger().error("select: {}", impl::socket_get_error()); res = client_state(res | CLIENT_ERROR); } else if (retval) { if (FD_ISSET(c.lidar_fd, &rfds)) res = client_state(res | LIDAR_DATA); @@ -356,15 +382,16 @@ client_state poll_client(const client& c, const int timeout_sec) { } static bool recv_fixed(SOCKET fd, void* buf, int64_t len) { + // Have to read longer than len because you need to know if the packet is + // too large int64_t bytes_read = recv(fd, (char*)buf, len + 1, 0); if (bytes_read == len) { return true; } else if (bytes_read == -1) { - std::cerr << "recvfrom: " << impl::socket_get_error() << std::endl; + logger().error("recvfrom: {}", impl::socket_get_error()); } else { - std::cerr << "Unexpected udp packet length of: " << bytes_read - << " bytes. Expected: " << len << " bytes." << std::endl; + logger().warn("Unexpected udp packet length: {}", bytes_read); } return false; } diff --git a/ouster_client/src/image_processing.cpp b/ouster_client/src/image_processing.cpp index 7707aefe..8f3739cc 100644 --- a/ouster_client/src/image_processing.cpp +++ b/ouster_client/src/image_processing.cpp @@ -10,7 +10,6 @@ #include #include #include -#include #include namespace ouster { diff --git a/ouster_client/src/lidar_scan.cpp b/ouster_client/src/lidar_scan.cpp index 075afaf1..e9ff336d 100644 --- a/ouster_client/src/lidar_scan.cpp +++ b/ouster_client/src/lidar_scan.cpp @@ -9,6 +9,7 @@ #include #include #include +#include #include #include @@ -17,6 +18,21 @@ namespace ouster { +// clang-format off +/** + * Flags for frame_status + */ +enum frame_status_masks : uint64_t { + FRAME_STATUS_THERMAL_SHUTDOWN_MASK = 0x0f, ///< Mask to get thermal shutdown status + FRAME_STATUS_SHOT_LIMITING_MASK = 0xf0 ///< Mask to get shot limting status +}; + +enum frame_status_shifts: uint64_t { + FRAME_STATUS_THERMAL_SHUTDOWN_SHIFT = 0, ///< No shift for thermal shutdown + FRAME_STATUS_SHOT_LIMITING_SHIFT = 4 /// shift 4 for shot limiting +}; + +// clang-format on using sensor::ChanField; using sensor::ChanFieldType; using sensor::UDPProfileLidar; @@ -121,6 +137,19 @@ LidarScan::LidarScan(size_t w, size_t h, sensor::UDPProfileLidar profile) LidarScan::LidarScan(size_t w, size_t h) : LidarScan{w, h, UDPProfileLidar::PROFILE_LIDAR_LEGACY} {} +sensor::ShotLimitingStatus LidarScan::shot_limiting() const { + return static_cast( + (frame_status & frame_status_masks::FRAME_STATUS_SHOT_LIMITING_MASK) >> + frame_status_shifts::FRAME_STATUS_SHOT_LIMITING_SHIFT); +} + +sensor::ThermalShutdownStatus LidarScan::thermal_shutdown() const { + return static_cast( + (frame_status & + frame_status_masks::FRAME_STATUS_THERMAL_SHUTDOWN_MASK) >> + frame_status_shifts::FRAME_STATUS_THERMAL_SHUTDOWN_SHIFT); +} + std::vector LidarScan::timestamps() const { std::vector res; res.reserve(headers.size()); @@ -213,14 +242,75 @@ bool operator==(const LidarScan::BlockHeader& a, bool operator==(const LidarScan& a, const LidarScan& b) { return a.frame_id == b.frame_id && a.w == b.w && a.h == b.h && - a.fields_ == b.fields_ && a.field_types_ == b.field_types_ && + a.frame_status == b.frame_status && a.fields_ == b.fields_ && + a.field_types_ == b.field_types_ && (a.timestamp() == b.timestamp()).all() && (a.measurement_id() == b.measurement_id()).all() && (a.status() == b.status()).all(); } +LidarScanFieldTypes get_field_types(const LidarScan& ls) { + return {ls.begin(), ls.end()}; +} + +LidarScanFieldTypes get_field_types(const sensor::sensor_info& info) { + // Get typical LidarScan to obtain field types + return impl::lookup_scan_fields(info.format.udp_profile_lidar); +} + +std::string to_string(const LidarScanFieldTypes& field_types) { + std::stringstream ss; + ss << "("; + for (size_t i = 0; i < field_types.size(); ++i) { + if (i > 0) ss << ", "; + ss << sensor::to_string(field_types[i].first) << ":" + << sensor::to_string(field_types[i].second); + } + ss << ")"; + return ss.str(); +} + +std::string to_string(const LidarScan& ls) { + std::stringstream ss; + LidarScanFieldTypes field_types(ls.begin(), ls.end()); + ss << "LidarScan: {h = " << ls.h << ", w = " << ls.w + << ", fid = " << ls.frame_id << "," << std::endl + << " frame status = " << std::hex << ls.frame_status << std::dec + << ", thermal_shutdown status = " << to_string(ls.thermal_shutdown()) + << ", shot_limiting status = " << to_string(ls.shot_limiting()) << "," + << std::endl + << " field_types = " << to_string(field_types) << "," << std::endl; + + if (!field_types.empty()) { + ss << " fields = [" << std::endl; + img_t key{ls.h, ls.w}; + for (const auto& ft : ls) { + impl::visit_field(ls, ft.first, impl::read_and_cast(), key); + ss << " " << to_string(ft.first) << ":" << to_string(ft.second) + << " = ("; + ss << key.minCoeff() << "; " << key.mean() << "; " + << key.maxCoeff(); + ss << ")," << std::endl; + } + ss << " ]," << std::endl; + } + + auto ts = ls.timestamp().cast(); + ss << " timestamp = (" << ts.minCoeff() << "; " << ts.mean() << "; " + << ts.maxCoeff() << ")," << std::endl; + auto mid = ls.measurement_id().cast(); + ss << " measurement_id = (" << mid.minCoeff() << "; " << mid.mean() << "; " + << mid.maxCoeff() << ")," << std::endl; + auto st = ls.status().cast(); + ss << " status = (" << st.minCoeff() << "; " << st.mean() << "; " + << st.maxCoeff() << ")" << std::endl; + + ss << "}"; + return ss.str(); +} + XYZLut make_xyz_lut(size_t w, size_t h, double range_unit, - double lidar_origin_to_beam_origin_mm, + const mat4d& beam_to_lidar_transform, const mat4d& transform, const std::vector& azimuth_angles_deg, const std::vector& altitude_angles_deg) { @@ -229,6 +319,13 @@ XYZLut make_xyz_lut(size_t w, size_t h, double range_unit, if (azimuth_angles_deg.size() != h || altitude_angles_deg.size() != h) throw std::invalid_argument("unexpected scan dimensions"); + double beam_to_lidar_euclidean_distance_mm = beam_to_lidar_transform(0, 3); + if (beam_to_lidar_transform(2, 3) != 0) { + beam_to_lidar_euclidean_distance_mm = + std::sqrt(std::pow(beam_to_lidar_transform(0, 3), 2) + + std::pow(beam_to_lidar_transform(2, 3), 2)); + } + Eigen::ArrayXd encoder(w * h); // theta_e Eigen::ArrayXd azimuth(w * h); // theta_a Eigen::ArrayXd altitude(w * h); // phi @@ -255,10 +352,15 @@ XYZLut make_xyz_lut(size_t w, size_t h, double range_unit, // offsets due to beam origin lut.offset = LidarScan::Points{w * h, 3}; - lut.offset.col(0) = encoder.cos() - lut.direction.col(0); - lut.offset.col(1) = encoder.sin() - lut.direction.col(1); - lut.offset.col(2) = -lut.direction.col(2); - lut.offset *= lidar_origin_to_beam_origin_mm; + lut.offset.col(0) = + encoder.cos() * beam_to_lidar_transform(0, 3) - + lut.direction.col(0) * beam_to_lidar_euclidean_distance_mm; + lut.offset.col(1) = + encoder.sin() * beam_to_lidar_transform(0, 3) - + lut.direction.col(1) * beam_to_lidar_euclidean_distance_mm; + lut.offset.col(2) = + -lut.direction.col(2) * beam_to_lidar_euclidean_distance_mm + + beam_to_lidar_transform(2, 3); // apply the supplied transform auto rot = transform.topLeftCorner(3, 3).transpose(); @@ -337,6 +439,19 @@ struct parse_field_col { } }; +uint64_t frame_status(const uint8_t thermal_shutdown, + const uint8_t shot_limiting) { + uint64_t res = 0; + res |= (thermal_shutdown & 0x0f) + << FRAME_STATUS_THERMAL_SHUTDOWN_SHIFT; // right nibble is thermal + // shutdown status, apply mask for + // safety, then shift + res |= (shot_limiting & 0x0f) + << FRAME_STATUS_SHOT_LIMITING_SHIFT; // right nibble is shot limiting, apply mask for + // safety, then shift + return res; +} + } // namespace bool ScanBatcher::operator()(const uint8_t* packet_buf, LidarScan& ls) { @@ -356,7 +471,12 @@ bool ScanBatcher::operator()(const uint8_t* packet_buf, LidarScan& ls) { // expecting to start batching a new scan next_m_id = 0; ls.frame_id = f_id; - } else if (ls.frame_id == f_id + 1) { + + const uint8_t f_thermal_shutdown = pf.thermal_shutdown(packet_buf); + const uint8_t f_shot_limiting = pf.shot_limiting(packet_buf); + ls.frame_status = frame_status(f_thermal_shutdown, f_shot_limiting); + + } else if (ls.frame_id == static_cast(f_id + 1)) { // drop reordered packets from the previous frame return false; } else if (ls.frame_id != f_id) { @@ -365,6 +485,7 @@ bool ScanBatcher::operator()(const uint8_t* packet_buf, LidarScan& ls) { zero_header_cols(ls, next_m_id, w); std::memcpy(cache.data(), packet_buf, cache.size()); cached_packet = true; + return true; } @@ -398,6 +519,6 @@ bool ScanBatcher::operator()(const uint8_t* packet_buf, LidarScan& ls) { impl::foreach_field(ls, parse_field_col(), m_id, pf, col_buf); } return false; -} +} // namespace ouster } // namespace ouster diff --git a/ouster_client/src/logging.cpp b/ouster_client/src/logging.cpp new file mode 100644 index 00000000..5ab4299e --- /dev/null +++ b/ouster_client/src/logging.cpp @@ -0,0 +1,103 @@ +#include "logging.h" + +#include + +#if (SPDLOG_VER_MAJOR < 1) +namespace spdlog { +namespace sinks { +// rename simple_file_sink_st to basic_file_sink_st +using basic_file_sink_st = simple_file_sink_st; +} // namespace sinks +} // namespace spdlog +#else +#include +#include +#endif + +#include + +#include + +using namespace ouster::sensor::impl; + +const std::string Logger::logger_name{"ouster::sensor"}; + +Logger& Logger::instance() { + static Logger logger_singleton; + return logger_singleton; +} + +spdlog::logger& Logger::get_logger() { return *logger_; } + +Logger::Logger() + : logger_(std::make_unique( + logger_name, std::make_shared())) { + logger_->set_level(spdlog::level::info); + logger_->flush_on(spdlog::level::info); +} + +void Logger::update_sink_and_log_level(spdlog::sink_ptr sink, + const std::string& log_level) { +#if (SPDLOG_VER_MAJOR < 1) + // recreate the logger with the new sink + logger_ = std::make_unique(logger_name, sink); + + using namespace spdlog::level; + auto idx = std::find(std::begin(level_names), std::end(level_names), + log_level.c_str()); + auto ll = idx == std::end(level_names) + ? spdlog::level::off + : static_cast( + std::distance(std::begin(level_names), idx)); +#else + // replace the logger sink with the new sink + logger_->sinks() = {sink}; + + auto ll = spdlog::level::from_str(log_level); +#endif + logger_->set_level(ll); + logger_->flush_on(ll); +} + +bool Logger::configure_stdout_sink(const std::string& log_level) { + try { + update_sink_and_log_level( + std::make_shared(), log_level); + } catch (const spdlog::spdlog_ex& ex) { + std::cerr << logger_name << " init_logger failed: " << ex.what() + << std::endl; + return false; + } + + return true; +} + +bool Logger::configure_file_sink(const std::string& log_level, + const std::string& log_file_path, + bool rotating, int max_size_in_bytes, + int max_files) { + try { + std::shared_ptr> + file_sink; + if (rotating) { + file_sink = std::make_shared( + log_file_path, max_size_in_bytes, max_files); + } else { + file_sink = std::make_shared( + log_file_path, true); + } + update_sink_and_log_level(file_sink, log_level); + } catch (const spdlog::spdlog_ex& ex) { + std::cerr << logger_name << " init_logger failed: " << ex.what() + << std::endl; + return false; + } + + return true; +} + +namespace ouster { +namespace sensor { +spdlog::logger& logger() { return Logger::instance().get_logger(); } +} // namespace sensor +} // namespace ouster \ No newline at end of file diff --git a/ouster_client/src/logging.h b/ouster_client/src/logging.h new file mode 100644 index 00000000..01d6b6b6 --- /dev/null +++ b/ouster_client/src/logging.h @@ -0,0 +1,37 @@ +#pragma once + +#include + +namespace ouster { +namespace sensor { +namespace impl { + +class Logger { + public: + static Logger& instance(); + + spdlog::logger& get_logger(); + + bool configure_stdout_sink(const std::string& log_level); + + bool configure_file_sink(const std::string& log_level, + const std::string& log_file_path, bool rotating, + int max_size_in_bytes, int max_files); + + private: + Logger(); + + void update_sink_and_log_level(spdlog::sink_ptr sink, + const std::string& log_level); + + private: + static const std::string logger_name; + std::unique_ptr logger_; +}; + +} // namespace impl + +spdlog::logger& logger(); + +} // namespace sensor +} // namespace ouster diff --git a/ouster_client/src/parsing.cpp b/ouster_client/src/parsing.cpp index b5e4cd68..8aed25d0 100644 --- a/ouster_client/src/parsing.cpp +++ b/ouster_client/src/parsing.cpp @@ -263,32 +263,29 @@ 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; - } else { - uint16_t res; - std::memcpy(&res, lidar_buf + 0, sizeof(uint16_t)); - return res; } + uint16_t res; + std::memcpy(&res, lidar_buf + 0, sizeof(uint16_t)); + return res; } uint16_t packet_format::frame_id(const uint8_t* lidar_buf) const { if (udp_profile_lidar == UDPProfileLidar::PROFILE_LIDAR_LEGACY) { return col_frame_id(nth_col(0, lidar_buf)); - } else { - uint16_t res; - std::memcpy(&res, lidar_buf + 2, sizeof(uint16_t)); - return res; } + uint16_t res; + std::memcpy(&res, lidar_buf + 2, sizeof(uint16_t)); + return res; } 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; - } else { - uint32_t res; - std::memcpy(&res, lidar_buf + 4, sizeof(uint32_t)); - return res & 0x00ffffff; } + uint32_t res; + std::memcpy(&res, lidar_buf + 4, sizeof(uint32_t)); + return res & 0x00ffffff; } uint64_t packet_format::prod_sn(const uint8_t* lidar_buf) const { @@ -296,11 +293,56 @@ uint64_t packet_format::prod_sn(const uint8_t* lidar_buf) const { // LEGACY profile has no prod_sn (serial number) - use 0 to code as // 'legacy' return 0; - } else { - uint64_t res; - std::memcpy(&res, lidar_buf + 7, sizeof(uint64_t)); - return res & 0x000000ffffffffff; } + uint64_t res; + std::memcpy(&res, lidar_buf + 7, sizeof(uint64_t)); + return res & 0x000000ffffffffff; +} + +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; + std::memcpy(&res, lidar_buf + 16, sizeof(uint8_t)); + return res; +} + +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; + std::memcpy(&res, lidar_buf + 17, sizeof(uint8_t)); + return res; +} + +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; + std::memcpy(&res, lidar_buf + 18, sizeof(uint8_t)); + return res & 0x0f; +} + +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; + std::memcpy(&res, lidar_buf + 19, sizeof(uint8_t)); + return res & 0x0f; } /* Measurement block access */ diff --git a/ouster_client/src/sensor_http.h b/ouster_client/src/sensor_http.h index a1fbd1e8..ad2c0aee 100644 --- a/ouster_client/src/sensor_http.h +++ b/ouster_client/src/sensor_http.h @@ -69,6 +69,16 @@ class SensorHttp { virtual void set_config_param(const std::string& key, const std::string& value) const = 0; + /** + * Retrieves the active configuration on the sensor + */ + virtual Json::Value active_config_params() const = 0; + + /** + * Retrieves the staged configuration on the sensor + */ + virtual Json::Value staged_config_params() const = 0; + /** * Enables automatic assignment of udp destination ports. */ diff --git a/ouster_client/src/sensor_http_imp.cpp b/ouster_client/src/sensor_http_imp.cpp index 039156c9..8f03405b 100644 --- a/ouster_client/src/sensor_http_imp.cpp +++ b/ouster_client/src/sensor_http_imp.cpp @@ -32,6 +32,14 @@ void SensorHttpImp::set_config_param(const string& key, execute(url, "\"set_config_param\""); } +Json::Value SensorHttpImp::active_config_params() const { + return get_json("api/v1/sensor/cmd/get_config_param?args=active"); +} + +Json::Value SensorHttpImp::staged_config_params() const { + return get_json("api/v1/sensor/cmd/get_config_param?args=staged"); +} + void SensorHttpImp::set_udp_dest_auto() const { execute("api/v1/sensor/cmd/set_udp_dest_auto", "{}"); } diff --git a/ouster_client/src/sensor_http_imp.h b/ouster_client/src/sensor_http_imp.h index 489bb508..0298c056 100644 --- a/ouster_client/src/sensor_http_imp.h +++ b/ouster_client/src/sensor_http_imp.h @@ -66,6 +66,16 @@ class SensorHttpImp : public util::SensorHttp { void set_config_param(const std::string& key, const std::string& value) const override; + /** + * Retrieves the active configuration on the sensor + */ + Json::Value active_config_params() const override; + + /** + * Retrieves the staged configuration on the sensor + */ + Json::Value staged_config_params() const override; + /** * Enables automatic assignment of udp destination ports. */ diff --git a/ouster_client/src/sensor_tcp_imp.cpp b/ouster_client/src/sensor_tcp_imp.cpp index aad54cda..a2f95d96 100644 --- a/ouster_client/src/sensor_tcp_imp.cpp +++ b/ouster_client/src/sensor_tcp_imp.cpp @@ -7,9 +7,10 @@ #include #include -#include #include +#include "logging.h" + using std::string; using namespace ouster::sensor::impl; @@ -63,6 +64,14 @@ void SensorTcpImp::set_config_param(const string& key, "set_config_param"); } +Json::Value SensorTcpImp::active_config_params() const { + return tcp_cmd_json({"get_config_param", "active"}); +} + +Json::Value SensorTcpImp::staged_config_params() const { + return tcp_cmd_json({"get_config_param", "staged"}); +} + void SensorTcpImp::set_udp_dest_auto() const { tcp_cmd_with_validation({"set_udp_dest_auto"}, "set_udp_dest_auto"); } @@ -111,14 +120,13 @@ SOCKET SensorTcpImp::cfg_socket(const char* addr) { hints.ai_flags = 0; ret = getaddrinfo(addr, "7501", &hints, &info_start); if (ret != 0) { - std::cerr << "cfg getaddrinfo(): " << gai_strerror(ret) - << std::endl; + logger().error("cfg getaddrinfo(): {}", gai_strerror(ret)); return SOCKET_ERROR; } } if (info_start == nullptr) { - std::cerr << "cfg getaddrinfo(): empty result" << std::endl; + logger().error("cfg getaddrinfo(): empty result"); return SOCKET_ERROR; } @@ -126,7 +134,7 @@ SOCKET SensorTcpImp::cfg_socket(const char* addr) { for (ai = info_start; ai != nullptr; ai = ai->ai_next) { sock_fd = socket(ai->ai_family, ai->ai_socktype, ai->ai_protocol); if (!socket_valid(sock_fd)) { - std::cerr << "cfg socket(): " << socket_get_error() << std::endl; + logger().error("cfg socket(): {}", socket_get_error()); continue; } @@ -136,8 +144,7 @@ SOCKET SensorTcpImp::cfg_socket(const char* addr) { } if (socket_set_rcvtimeout(sock_fd, RCVTIMEOUT_SEC)) { - std::cerr << "cfg set_rcvtimeout(): " << socket_get_error() - << std::endl; + logger().error("cfg set_rcvtimeout(): {}", socket_get_error()); socket_close(sock_fd); continue; } diff --git a/ouster_client/src/sensor_tcp_imp.h b/ouster_client/src/sensor_tcp_imp.h index 3bc6feab..7e5bc816 100644 --- a/ouster_client/src/sensor_tcp_imp.h +++ b/ouster_client/src/sensor_tcp_imp.h @@ -71,6 +71,16 @@ class SensorTcpImp : public util::SensorHttp { void set_config_param(const std::string& key, const std::string& value) const override; + /** + * Retrieves the active configuration on the sensor + */ + Json::Value active_config_params() const override; + + /** + * Retrieves the staged configuration on the sensor + */ + Json::Value staged_config_params() const override; + /** * Enables automatic assignment of udp destination ports. */ diff --git a/ouster_client/src/types.cpp b/ouster_client/src/types.cpp index 6187f75f..7bcf707a 100644 --- a/ouster_client/src/types.cpp +++ b/ouster_client/src/types.cpp @@ -11,13 +11,13 @@ #include #include #include -#include #include #include #include #include #include +#include "logging.h" #include "ouster/impl/build.h" #include "ouster/version.h" @@ -106,6 +106,27 @@ Table udp_profile_imu_strings{{ {PROFILE_IMU_LEGACY, "LEGACY"}, }}; +// TODO: should we name them something better? feel like the most important is +// SHOT_LIMITING_NORMAL +Table shot_limiting_status_strings{{ + {SHOT_LIMITING_NORMAL, "SHOT_LIMITING_NORMAL"}, + {SHOT_LIMITING_IMMINENT, "SHOT_LIMITING_IMMINENT"}, + {SHOT_LIMITING_REDUCTION_0_10, "SHOT_LIMITING_REDUCTION_0_10"}, + {SHOT_LIMITING_REDUCTION_10_20, "SHOT_LIMITING_REDUCTION_10_20"}, + {SHOT_LIMITING_REDUCTION_20_30, "SHOT_LIMITING_REDUCTION_20_30"}, + {SHOT_LIMITING_REDUCTION_30_40, "SHOT_LIMITING_REDUCTION_30_40"}, + {SHOT_LIMITING_REDUCTION_40_50, "SHOT_LIMITING_REDUCTION_40_50"}, + {SHOT_LIMITING_REDUCTION_50_60, "SHOT_LIMITING_REDUCTION_50_60"}, + {SHOT_LIMITING_REDUCTION_60_70, "SHOT_LIMITING_REDUCTION_60_70"}, + {SHOT_LIMITING_REDUCTION_70_75, "SHOT_LIMITING_REDUCTION_70_75"}, +}}; + +// TODO: do we want these? do we like the names? +Table thermal_shutdown_status_strings{{ + {THERMAL_SHUTDOWN_NORMAL, "THERMAL_SHUTDOWN_NORMAL"}, + {THERMAL_SHUTDOWN_IMMINENT, "THERMAL_SHUTDOWN_IMMINENT"}, +}}; + } // namespace impl /* Equality operators */ @@ -132,6 +153,7 @@ bool operator==(const sensor_info& lhs, const sensor_info& rhs) { lhs.beam_altitude_angles == rhs.beam_altitude_angles && lhs.lidar_origin_to_beam_origin_mm == rhs.lidar_origin_to_beam_origin_mm && + lhs.beam_to_lidar_transform == rhs.beam_to_lidar_transform && lhs.imu_to_sensor_transform == rhs.imu_to_sensor_transform && lhs.lidar_to_sensor_transform == rhs.lidar_to_sensor_transform && lhs.extrinsic == rhs.extrinsic && lhs.init_id == rhs.init_id && @@ -225,6 +247,13 @@ static double default_lidar_origin_to_beam_origin(std::string prod_line) { return lidar_origin_to_beam_origin_mm; } +mat4d default_beam_to_lidar_transform(std::string prod_line) { + mat4d beam_to_lidar_transform = mat4d::Identity(); + beam_to_lidar_transform(0, 3) = + default_lidar_origin_to_beam_origin(prod_line); + return beam_to_lidar_transform; +} + sensor_info default_sensor_info(lidar_mode mode) { return sensor::sensor_info{"UNKNOWN", "000000000000", @@ -235,6 +264,7 @@ sensor_info default_sensor_info(lidar_mode mode) { gen1_azimuth_angles, gen1_altitude_angles, default_lidar_origin_to_beam_origin("OS-1-64"), + default_beam_to_lidar_transform("OS-1-64"), default_imu_to_sensor_transform, default_lidar_to_sensor_transform, mat4d::Identity(), @@ -404,6 +434,23 @@ std::string to_string(ChanField field) { return res ? res.value() : "UNKNOWN"; } +std::string to_string(ChanFieldType ft) { + switch (ft) { + case sensor::ChanFieldType::VOID: + return "VOID"; + case sensor::ChanFieldType::UINT8: + return "UINT8"; + case sensor::ChanFieldType::UINT16: + return "UINT16"; + case sensor::ChanFieldType::UINT32: + return "UINT32"; + case sensor::ChanFieldType::UINT64: + return "UINT64"; + default: + return "UNKNOWN"; + } +} + std::string to_string(UDPProfileLidar profile) { auto res = lookup(impl::udp_profile_lidar_strings, profile); return res ? res.value() : "UNKNOWN"; @@ -422,6 +469,17 @@ optional udp_profile_imu_of_string(const std::string& s) { return rlookup(impl::udp_profile_imu_strings, s.c_str()); } +std::string to_string(ShotLimitingStatus shot_limiting_status) { + auto res = lookup(impl::shot_limiting_status_strings, shot_limiting_status); + return res ? res.value() : "UNKNOWN"; +} + +std::string to_string(ThermalShutdownStatus thermal_shutdown_status) { + auto res = + lookup(impl::thermal_shutdown_status_strings, thermal_shutdown_status); + return res ? res.value() : "UNKNOWN"; +} + static sensor_config parse_config(const Json::Value& root) { sensor_config config{}; @@ -430,9 +488,9 @@ static sensor_config parse_config(const Json::Value& root) { } else if (!root["udp_ip"].empty()) { // deprecated params from FW 1.13. Set FW 2.0+ configs appropriately config.udp_dest = root["udp_ip"].asString(); - std::cerr << "Please note that udp_ip has been deprecated in favor of " - "udp_dest. Will set udp_dest appropriately..." - << std::endl; + logger().warn( + "Please note that udp_ip has been deprecated in favor " + "of udp_dest. Will set udp_dest appropriately..."); } if (!root["udp_port_lidar"].empty()) @@ -451,7 +509,7 @@ static sensor_config parse_config(const Json::Value& root) { root["azimuth_window"][1].asInt()); if (!root["signal_multiplier"].empty()) - config.signal_multiplier = root["signal_multiplier"].asInt(); + config.signal_multiplier = root["signal_multiplier"].asDouble(); if (!root["operating_mode"].empty()) { auto operating_mode = @@ -462,10 +520,9 @@ static sensor_config parse_config(const Json::Value& root) { throw std::invalid_argument("Unexpected Operating Mode"); } } else if (!root["auto_start_flag"].empty()) { - std::cerr - << "Please note that auto_start_flag has been deprecated in favor " - "of operating_mode. Will set operating_mode appropriately..." - << std::endl; + logger().warn( + "Please note that auto_start_flag has been deprecated in favor " + "of operating_mode. Will set operating_mode appropriately..."); config.operating_mode = root["auto_start_flag"].asBool() ? sensor::OPERATING_NORMAL : sensor::OPERATING_STANDBY; @@ -574,8 +631,8 @@ static bool valid_response(const Json::Value& root, return (root.isMember(tcp_request) && root[tcp_request].isObject()); } -// TODO make robust to new formats that are incorrect instead of returning false -// and sending to legacy +// TODO make robust to new formats that are incorrect instead of returning +// false and sending to legacy static bool is_new_format(const std::string& metadata) { Json::Value root{}; Json::CharReaderBuilder builder{}; @@ -631,7 +688,7 @@ static data_format parse_data_format(const Json::Value& root) { format.column_window.first = root["column_window"][0].asInt(); format.column_window.second = root["column_window"][1].asInt(); } else { - std::cerr << "WARNING: No column window found." << std::endl; + logger().warn("No column window found."); format.column_window = default_column_window(format.columns_per_frame); } @@ -646,7 +703,7 @@ static data_format parse_data_format(const Json::Value& root) { throw std::invalid_argument{"Unexpected udp lidar profile"}; } } else { - std::cerr << "WARNING: No lidar profile found." << std::endl; + logger().warn("No lidar profile found."); format.udp_profile_lidar = PROFILE_LIDAR_LEGACY; } @@ -689,20 +746,41 @@ static sensor_info parse_legacy(const std::string& meta) { if (root.isMember("data_format")) { info.format = parse_data_format(root["data_format"]); } else { - std::cerr << "WARNING: No data_format found." << std::endl; + logger().warn("No data_format found."); info.format = default_data_format(info.mode); } - // "lidar_origin_to_beam_origin_mm" introduced in fw 2.0. Fall back - // to 1.13 + // "lidar_origin_to_beam_origin_mm" introduced in fw 2.0 BUT missing + // on OS-DOME. Handle falling back to FW 1.13 or setting to 0 according + // to prod-line if (root.isMember("lidar_origin_to_beam_origin_mm")) { info.lidar_origin_to_beam_origin_mm = root["lidar_origin_to_beam_origin_mm"].asDouble(); } else { - std::cerr << "WARNING: No lidar_origin_to_beam_origin_mm found." - << std::endl; - info.lidar_origin_to_beam_origin_mm = - default_lidar_origin_to_beam_origin(info.prod_line); + 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."); + info.lidar_origin_to_beam_origin_mm = + default_lidar_origin_to_beam_origin(info.prod_line); + } + } + + // beam_to_lidar_transform" introduced in fw 2.5/fw 3.0 + if (root.isMember("beam_to_lidar_transform")) { + for (int i = 0; i < 4; i++) { + for (int j = 0; j < 4; j++) { + const Json::Value::ArrayIndex ind = i * 4 + j; + info.beam_to_lidar_transform(i, j) = + root["beam_to_lidar_transform"][ind].asDouble(); + } + } + } 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 (root["beam_altitude_angles"].size() != info.format.pixels_per_column) { @@ -731,8 +809,7 @@ static sensor_info parse_legacy(const std::string& meta) { } } } else { - std::cerr << "WARNING: No valid imu_to_sensor_transform found." - << std::endl; + logger().warn("No valid imu_to_sensor_transform found."); info.imu_to_sensor_transform = default_imu_to_sensor_transform; } @@ -747,8 +824,7 @@ static sensor_info parse_legacy(const std::string& meta) { } } } else { - std::cerr << "WARNING: No valid lidar_to_sensor_transform found." - << std::endl; + logger().warn("No valid lidar_to_sensor_transform found."); info.lidar_to_sensor_transform = default_lidar_to_sensor_transform; } @@ -760,7 +836,7 @@ static sensor_info parse_legacy(const std::string& meta) { info.udp_port_imu = root["udp_port_imu"].asInt(); return info; -} +} // namespace ouster static void update_json_obj(Json::Value& dst, const Json::Value& src) { const std::vector& members = src.getMemberNames(); @@ -832,8 +908,10 @@ sensor_info parse_metadata(const std::string& metadata) { sensor_info info{}; if (is_new_format(metadata)) { + logger().debug("parsing new metadata format"); info = parse_legacy(convert_to_legacy(metadata)); } else { + logger().debug("parsing legacy metadata format"); info = parse_legacy(metadata); } return info; @@ -884,6 +962,13 @@ std::string to_string(const sensor_info& info) { root["lidar_origin_to_beam_origin_mm"] = info.lidar_origin_to_beam_origin_mm; + for (size_t i = 0; i < 4; i++) { + for (size_t j = 0; j < 4; j++) { + root["beam_to_lidar_transform"].append( + info.beam_to_lidar_transform(i, j)); + } + } + for (auto i : info.beam_azimuth_angles) root["beam_azimuth_angles"].append(i); for (auto i : info.beam_altitude_angles) @@ -956,7 +1041,17 @@ Json::Value to_json(const sensor_config& config) { } if (config.signal_multiplier) { - root["signal_multiplier"] = config.signal_multiplier.value(); + if (config.signal_multiplier < 1) { + root["signal_multiplier"] = config.signal_multiplier.value(); + } else { + // jsoncpp < 1.7.7 strips 0s off of exact representation so 2.0 + // becomes 2 + // On ubuntu 18.04, the default jsoncpp is 1.7.4-3 + // Fix was: + // https://github.com/open-source-parsers/jsoncpp/pull/547 Work + // around by always casting to int before writing out to json + root["signal_multiplier"] = int(config.signal_multiplier.value()); + } } if (config.sync_pulse_out_angle) { diff --git a/ouster_pcap/src/os_pcap.cpp b/ouster_pcap/src/os_pcap.cpp index 06e450a4..687c178e 100644 --- a/ouster_pcap/src/os_pcap.cpp +++ b/ouster_pcap/src/os_pcap.cpp @@ -9,9 +9,9 @@ #include #include #include -#include #include #include +#include #include diff --git a/ouster_viz/include/ouster/point_viz.h b/ouster_viz/include/ouster/point_viz.h index 88db9238..c001ddb8 100644 --- a/ouster_viz/include/ouster/point_viz.h +++ b/ouster_viz/include/ouster/point_viz.h @@ -30,6 +30,7 @@ constexpr mat4d identity4d = {1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1}; using vec4f = std::array; using vec3d = std::array; +using vec2d = std::array; // TODO: messes up lidar_scan_viz namespace impl { @@ -169,6 +170,18 @@ class PointViz { void push_mouse_pos_handler( std::function&& f); + /** + * Add a callback for processing every new draw frame buffer. + * + * NOTE: Any processing in the callback slows the renderer update loop + * dramatically. Primary use to store frame buffer images to disk + * for further processing. + * + * @param[in] f function callback of a form f(fb_data, fb_width, fb_height) + */ + void push_frame_buffer_handler( + std::function&, int, int)>&& f); + /** * Remove the last added callback for handling keyboard input */ @@ -189,6 +202,11 @@ class PointViz { */ void pop_mouse_pos_handler(); + /** + * @copydoc pop_key_handler() + */ + void pop_frame_buffer_handler(); + /** * Get a reference to the camera controls * @@ -259,6 +277,20 @@ class PointViz { */ bool remove(const std::shared_ptr