diff --git a/.clangd b/.clangd new file mode 100644 index 00000000..ef516ada --- /dev/null +++ b/.clangd @@ -0,0 +1,2 @@ +CompileFlags: + Add: -ferror-limit=0 diff --git a/.gitignore b/.gitignore index 513101bf..85bea34b 100644 --- a/.gitignore +++ b/.gitignore @@ -5,3 +5,8 @@ python/.tox/ python/**/*.egg-info/ python/**/*.so python/**/*.dll +python/**/*.pyd + +# visual studio +.vs/ +out/ \ No newline at end of file diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 2f4e1988..cccf065d 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,6 +2,48 @@ Changelog ========= +[20220826] +========== + +* drop support for buliding C++ libraries and Python bindings on Ubuntu 16.04 +* drop support for buliding C++ libraries and Python bindings on Mac 10.13, Mac 10.14 +* Python 3.6 wheels are no longer built and published +* drop support for sensors running FW < 2.0 +* require C++ 14 to build + +ouster_client +-------------- +* add ```CUSTOM0-9`` ChanFields to LidarScan object +* fix parsing measurement status from packets: previously, with some UDP profiles, higher order bits + could be randomly set +* add option for EIGEN_MAX_ALIGN_BYTES, ON by default +* use of sensor http interface for comms with sensors for FW 2.1+ +* propogate C++ 17 usage requirement in cmake for C++ libraries built as C++17 +* allow vcpkg configuration via environment variables + +ouster_pcap +----------- +* fix incorrect encapsulation protocol being reported in ``packet_info`` + +ouster_viz +---------- +* clean up GL context logic to avoid errors on window/intel UHD graphics + +python +------ +* windows extension modules are now statically linked to avoid potential issues with vendored dlls + +ouster_ros +---------- +* drop ROS kinetic support +* switch from nodes to nodelets +* update topic names, group under single ros namespace +* separate launch files for play, replay, and recording +* 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) + + [20220608] ========== @@ -13,6 +55,7 @@ python ------ * single return parsing for FW 2.3.1 reflects change from ouster_client + [20220504] ========== @@ -55,7 +98,7 @@ ouster_ros fields are filled with zeroes [20220107] -============ +========== * add support for arm64 macos and linux. Releases are now built and tested on these platforms * add support for Python 3.10 diff --git a/CMakeLists.txt b/CMakeLists.txt index ecb53b6b..6d5140b4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,36 +1,45 @@ -cmake_minimum_required(VERSION 3.1.0) +cmake_minimum_required(VERSION 3.10...3.22) list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake) include(DefaultBuildType) +# configure vcpkg from environment variables, if present +include(VcpkgEnv) + # ==== Project Name ==== -project(ouster_example VERSION 20220608) +project(ouster_example VERSION 20220826) + +# generate version header +set(OusterSDK_VERSION_STRING 0.5.0) +include(VersionGen) # ==== Options ==== option(CMAKE_POSITION_INDEPENDENT_CODE "Build position independent code." ON) option(BUILD_SHARED_LIBS "Build shared libraries." OFF) -option(BUILD_PCAP "Build pcap utils." OFF) -option(BUILD_TESTING "Build tests" OFF) +option(BUILD_PCAP "Build pcap utils." ON) option(BUILD_VIZ "Build Ouster visualizer." ON) +option(BUILD_TESTING "Build tests" OFF) option(BUILD_EXAMPLES "Build C++ examples" OFF) -option(USE_EIGEN_MAX_ALIGN_BYTES_32 "Eigen max aligned bytes." ON) +option(OUSTER_USE_EIGEN_MAX_ALIGN_BYTES_32 "Eigen max aligned bytes." ON) -# when building as a top-level project and not as conan library +# when building as a top-level project if(CMAKE_PROJECT_NAME STREQUAL PROJECT_NAME) - if(NOT "${CMAKE_CXX_STANDARD}") - set(CMAKE_CXX_STANDARD 11) + set(CMAKE_EXPORT_COMPILE_COMMANDS ON) + + if(NOT DEFINED CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD_REQUIRED ON) endif() - set(CMAKE_CXX_STANDARD_REQUIRED ON) - if(NOT "${CMAKE_CXX_EXTENSIONS}") + if(NOT DEFINED CMAKE_CXX_EXTENSIONS) set(CMAKE_CXX_EXTENSIONS OFF) endif() if(MSVC) - add_compile_options(/W3) + add_compile_options(/W2) add_compile_definitions(NOMINMAX _USE_MATH_DEFINES WIN32_LEAN_AND_MEAN) else() - add_compile_options(-Wall -Wextra -Werror -Wno-error=deprecated-declarations) + add_compile_options(-Wall -Wextra) endif() include(CTest) @@ -38,8 +47,12 @@ endif() # === Subdirectories === add_subdirectory(ouster_client) +if(CMAKE_PROJECT_NAME STREQUAL PROJECT_NAME AND OUSTER_USE_EIGEN_MAX_ALIGN_BYTES_32) + message(STATUS "Ouster SDK client: Using EIGEN_MAX_ALIGN_BYTES = 32") + target_compile_definitions(ouster_client PUBLIC EIGEN_MAX_ALIGN_BYTES=32) +endif() -if(BUILD_PCAP OR BUILD_EXAMPLES) +if(BUILD_PCAP) add_subdirectory(ouster_pcap) endif() @@ -51,36 +64,12 @@ if(BUILD_EXAMPLES) add_subdirectory(examples) endif() -if(USE_EIGEN_MAX_ALIGN_BYTES_32) - message(STATUS "Ouster SDK client: Using EIGEN_MAX_ALIGN_BYTES = 32") - target_compile_definitions(ouster_client PUBLIC EIGEN_MAX_ALIGN_BYTES=32) - if(BUILD_TESTING) - add_subdirectory(tests) - endif() +if(CMAKE_PROJECT_NAME STREQUAL PROJECT_NAME AND BUILD_TESTING) + add_subdirectory(tests) endif() -# ==== Install ==== -include(CMakePackageConfigHelpers) -write_basic_package_version_file( - OusterSDKConfigVersion.cmake - VERSION ${PACKAGE_VERSION} - COMPATIBILITY AnyNewerVersion) - -install(EXPORT ouster-sdk-targets - FILE OusterSDKTargets.cmake - NAMESPACE OusterSDK:: - DESTINATION lib/cmake/OusterSDK) - -configure_file(cmake/OusterSDKConfig.cmake.in OusterSDKConfig.cmake @ONLY) -install(FILES "${CMAKE_CURRENT_BINARY_DIR}/OusterSDKConfig.cmake" - "${CMAKE_CURRENT_BINARY_DIR}/OusterSDKConfigVersion.cmake" - DESTINATION lib/cmake/OusterSDK) - -install(FILES LICENSE LICENSE-bin - DESTINATION share) - # ==== Packaging ==== -set(CPACK_PACKAGE_CONTACT "support@ouster.io") +set(CPACK_PACKAGE_CONTACT "oss@ouster.io") set(CPACK_PACKAGE_VENDOR "Ouster") set(CPACK_PACKAGE_DESCRIPTION_SUMMARY "Ouster sensor SDK") set(CPACK_PACKAGE_VERSION ${PROJECT_VERSION}) @@ -98,7 +87,28 @@ set(CPACK_SOURCE_IGNORE_FILES /.git /dist) # deb options set(CPACK_DEBIAN_PACKAGE_NAME ouster-sdk) set(CPACK_DEBIAN_FILE_NAME DEB-DEFAULT) -set(CPACK_DEBIAN_PACKAGE_DEPENDS "cmake, libjsoncpp-dev, libtclap-dev, libeigen3-dev, - libglfw3-dev, libglew-dev") +set(CPACK_DEBIAN_PACKAGE_DEPENDS + "libjsoncpp-dev, libeigen3-dev, libtins-dev, libglfw3-dev, libglew-dev") include(CPack) + +# ==== Install ==== +include(CMakePackageConfigHelpers) +write_basic_package_version_file( + OusterSDKConfigVersion.cmake + VERSION ${PACKAGE_VERSION} + COMPATIBILITY AnyNewerVersion) + +install(EXPORT ouster-sdk-targets + FILE OusterSDKTargets.cmake + NAMESPACE OusterSDK:: + DESTINATION lib/cmake/OusterSDK) + +configure_file(cmake/OusterSDKConfig.cmake.in OusterSDKConfig.cmake @ONLY) +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) diff --git a/CMakeSettings.json b/CMakeSettings.json index 7e91e2c8..dcf61699 100644 --- a/CMakeSettings.json +++ b/CMakeSettings.json @@ -1,40 +1,40 @@ { - "configurations": [ + "configurations": [ + { + "name": "x64-Release", + "generator": "Ninja", + "configurationType": "Release", + "buildRoot": "${projectDir}\\out\\build\\${name}", + "installRoot": "${projectDir}\\out\\install\\${name}", + "cmakeCommandArgs": "", + "buildCommandArgs": "", + "ctestCommandArgs": "", + "inheritEnvironments": [ "msvc_x64_x64" ], + "variables": [ { - "name": "x64-Release", - "generator": "Ninja", - "configurationType": "Release", - "buildRoot": "${projectDir}\\out\\build\\${name}", - "installRoot": "${projectDir}\\out\\install\\${name}", - "cmakeCommandArgs": "", - "buildCommandArgs": "", - "ctestCommandArgs": "", - "inheritEnvironments": [ "msvc_x64_x64" ], - "variables": [] - }, - { - "name": "x64-Debug", - "generator": "Ninja", - "configurationType": "Debug", - "inheritEnvironments": [ "msvc_x64_x64" ], - "buildRoot": "${projectDir}\\out\\build\\${name}", - "installRoot": "${projectDir}\\out\\install\\${name}", - "cmakeCommandArgs": "", - "buildCommandArgs": "", - "ctestCommandArgs": "", - "variables": [] - }, + "name": "BUILD_EXAMPLES", + "value": "True", + "type": "BOOL" + } + ] + }, + { + "name": "x64-Debug", + "generator": "Ninja", + "configurationType": "Debug", + "inheritEnvironments": [ "msvc_x64_x64" ], + "buildRoot": "${projectDir}\\out\\build\\${name}", + "installRoot": "${projectDir}\\out\\install\\${name}", + "cmakeCommandArgs": "", + "buildCommandArgs": "", + "ctestCommandArgs": "", + "variables": [ { - "name": "x64-RelWithDebInfo", - "generator": "Ninja", - "configurationType": "RelWithDebInfo", - "buildRoot": "${projectDir}\\out\\build\\${name}", - "installRoot": "${projectDir}\\out\\install\\${name}", - "cmakeCommandArgs": "", - "buildCommandArgs": "", - "ctestCommandArgs": "", - "inheritEnvironments": [ "msvc_x64_x64" ], - "variables": [] + "name": "BUILD_EXAMPLES", + "value": "True", + "type": "BOOL" } - ] -} + ] + } + ] +} \ No newline at end of file diff --git a/LICENSE b/LICENSE index 343812ab..2d09cf12 100644 --- a/LICENSE +++ b/LICENSE @@ -1,29 +1,63 @@ -BSD 3-Clause License - -Copyright (c) 2018, Ouster, Inc. -All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - -* Redistributions of source code must retain the above copyright notice, this - list of conditions and the following disclaimer. - -* Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - -* Neither the name of the copyright holder nor the names of its - contributors may be used to endorse or promote products derived from - this software without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +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 + +Files: * +Copyright: 2018-2022 Ouster, Inc +License: BSD-3-Clause + +Files: include/optional-lite/* +Copyright: 2014-2021 Martin Moene +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: + + * Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +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. \ No newline at end of file diff --git a/LICENSE-bin b/LICENSE-bin index 9cbf203e..d86c379a 100644 --- a/LICENSE-bin +++ b/LICENSE-bin @@ -227,3 +227,29 @@ License: MIT 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. + + +Name: curl +Description: statically linked +Availability: https://github.com/curl/curl +License: Curl License + Copyright (c) 1996 - 2022, Daniel Stenberg, , and many + contributors, see the THANKS file. + + All rights reserved. + + Permission to use, copy, modify, and distribute this software for any purpose + with or without fee is hereby granted, provided that the above copyright + notice and this permission notice appear in all copies. + + 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 OF THIRD PARTY RIGHTS. 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. + + 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. diff --git a/cmake/FindCURL.cmake b/cmake/FindCURL.cmake new file mode 100644 index 00000000..826c27c5 --- /dev/null +++ b/cmake/FindCURL.cmake @@ -0,0 +1,45 @@ +# Define forwards-compatible imported target for old platforms (Ubuntu 18.04) +# Note: curl from VCPKG appears to completely ignore curl find modules despite +# CMAKE_MODULE_PATH settings + +include(FindPackageHandleStandardArgs) + +# Prefer package config if it exists +find_package(CURL CONFIG QUIET) +if(CURL_FOUND) + find_package_handle_standard_args(CURL CONFIG_MODE) + return() +endif() + +# Trying to find with pkg-config +find_package(PkgConfig QUIET) +if(PKG_CONFIG_FOUND) + pkg_check_modules(pkg_curl QUIET libcurl) + if(pkg_curl_FOUND) + set(CURL_VERSION_STRING ${pkg_curl_VERSION}) + endif() +endif() + +find_path(CURL_INCLUDE_DIRS + NAMES curl/curl.h + HINTS ${pkg_curl_INCLUDE_DIRS}) +mark_as_advanced(CURL_INCLUDE_DIRS) + +# Linux/macos only +find_library(CURL_LIBRARIES NAMES + curl + HINTS ${pkg_curl_LIBRARY_DIRS}) +mark_as_advanced(CURL_LIBRARIES) + +# Check that we have everything that we need +find_package_handle_standard_args(CURL + REQUIRED_VARS CURL_INCLUDE_DIRS CURL_LIBRARIES + VERSION_VAR CURL_VERSION_STRING) + +if(NOT TARGET CURL::libcurl) + add_library(CURL::libcurl UNKNOWN IMPORTED) + set_target_properties(CURL::libcurl PROPERTIES + INTERFACE_INCLUDE_DIRECTORIES "${CURL_INCLUDE_DIRS}" + IMPORTED_LINK_INTERFACE_LANGUAGES "C" + IMPORTED_LOCATION "${CURL_LIBRARIES}") +endif() diff --git a/cmake/FindEigen3.cmake b/cmake/FindEigen3.cmake index 5b41116f..8ba8cbba 100644 --- a/cmake/FindEigen3.cmake +++ b/cmake/FindEigen3.cmake @@ -4,10 +4,7 @@ include(FindPackageHandleStandardArgs) find_package(Eigen3 CONFIG QUIET) -find_package_handle_standard_args(Eigen3 - REQUIRED_VARS EIGEN3_INCLUDE_DIR - VERSION_VAR EIGEN3_VERSION -) +find_package_handle_standard_args(Eigen3 CONFIG_MODE) if(NOT TARGET Eigen3::Eigen) add_library(Eigen3::Eigen INTERFACE IMPORTED) @@ -15,4 +12,3 @@ if(NOT TARGET Eigen3::Eigen) set_target_properties(Eigen3::Eigen PROPERTIES INTERFACE_INCLUDE_DIRECTORIES ${EIGEN3_INCLUDE_DIR}) endif() - diff --git a/cmake/FindOpenGL.cmake b/cmake/FindOpenGL.cmake deleted file mode 100644 index 954e7340..00000000 --- a/cmake/FindOpenGL.cmake +++ /dev/null @@ -1,23 +0,0 @@ -# try to define a forward-compatible imported target on old platforms (Ubunu 16.04) - -function(find_opengl) - set(CMAKE_MODULE_PATH "") - - find_package(OpenGL QUIET REQUIRED) - find_package_handle_standard_args(OpenGL DEFAULT_MSG OPENGL_LIBRARIES) - - if(NOT TARGET OpenGL::GL) - if(OPENGL_gl_LIBRARY AND OPENGL_INCLUDE_DIR) - add_library(OpenGL::GL UNKNOWN IMPORTED) - set_target_properties(OpenGL::GL PROPERTIES - IMPORTED_LOCATION "${OPENGL_gl_LIBRARY}") - set_target_properties(OpenGL::GL PROPERTIES - INTERFACE_INCLUDE_DIRECTORIES "${OPENGL_INCLUDE_DIR}") - else() - message(FATAL_ERROR "Failed to provide required OpenGL::GL target") - endif() - endif() - -endfunction() - -find_opengl() diff --git a/cmake/Findglfw3.cmake b/cmake/Findglfw3.cmake index 37cb3cd2..da49773b 100644 --- a/cmake/Findglfw3.cmake +++ b/cmake/Findglfw3.cmake @@ -5,16 +5,10 @@ include(FindPackageHandleStandardArgs) find_package(glfw3 CONFIG REQUIRED) -# Package on >=18.04 sets a target, 16.04 uses default vars -if (TARGET glfw) +# Package on >=18.04 sets a target +if(TARGET glfw) get_target_property(GLFW3_LIBRARIES glfw LOCATION) get_target_property(GLFW3_INCLUDE_DIRS glfw INTERFACE_INCLUDE_DIRECTORIES) -else() - set(GLFW3_INCLUDE_DIRS ${GLFW3_INCLUDE_DIR}) - set(GLFW3_LIBRARIES ${GLFW3_LIBRARY}) endif() -find_package_handle_standard_args(glfw3 - REQUIRED_VARS GLFW3_INCLUDE_DIRS GLFW3_LIBRARIES - VERSION_VAR glfw3_VERSION -) +find_package_handle_standard_args(glfw3 CONFIG_MODE) diff --git a/cmake/Findjsoncpp.cmake b/cmake/Findjsoncpp.cmake index 3d392019..967e7326 100644 --- a/cmake/Findjsoncpp.cmake +++ b/cmake/Findjsoncpp.cmake @@ -1,46 +1,50 @@ # Targets defined for jsoncpp differ a good amount between distributions - include(FindPackageHandleStandardArgs) # Recent jsoncpp cmake config fails if called twice if(NOT jsoncpp_FOUND) - find_package(jsoncpp CONFIG) + find_package(jsoncpp CONFIG QUIET) endif() # This target exists on ubuntu / debian. For some reason debian bullseye doesn't # ship a static lib / define the jsoncpp_lib_static target. if(jsoncpp_FOUND AND TARGET jsoncpp_lib) - find_package_handle_standard_args(jsoncpp DEFAULT_MSG jsoncpp_CONFIG) -# Target available in recent versions of vcpkg. We can make an alias of an -# imported target since we're guaranteed to be using a recent cmake -# Note: newer cmake will error if one but not both targets are in scope -elseif(jsoncpp_FOUND AND TARGET jsoncpp_static) - set_target_properties(jsoncpp_static PROPERTIES IMPORTED_GLOBAL TRUE) - set_target_properties(jsoncpp_object PROPERTIES IMPORTED_GLOBAL TRUE) - add_library(jsoncpp_lib ALIAS jsoncpp_static) - find_package_handle_standard_args(jsoncpp DEFAULT_MSG jsoncpp_CONFIG) -else() - # fall back to pkg-config - find_package(PkgConfig QUIET REQUIRED) - pkg_check_modules(PC_JSONCPP QUIET jsoncpp) + find_package_handle_standard_args(jsoncpp CONFIG_MODE) + return() +endif() - set(jsoncpp_VERSION ${PC_JSONCPP_VERSION}) +# With vcpkg, only a static lib is available so create a target for compatibility +if(jsoncpp_FOUND AND TARGET jsoncpp_static) + add_library(jsoncpp_lib INTERFACE) + target_link_libraries(jsoncpp_lib INTERFACE jsoncpp_static) + install(TARGETS jsoncpp_lib EXPORT ouster-sdk-targets) + find_package_handle_standard_args(jsoncpp CONFIG_MODE) + return() +endif() + +# Fall back to find_library with hints from pkgconfig +find_package(PkgConfig QUIET) +if(PKG_CONFIG_FOUND) + pkg_check_modules(PC_JSONCPP QUIET jsoncpp) + if(PC_JSONCPP_FOUND) + set(jsoncpp_VERSION_STRING ${PC_JSONCPP_VERSION}) + endif() +endif() - find_library(jsoncpp_LIBRARY NAMES libjsoncpp.so libjsoncpp.dylib - PATHS ${PC_JSONCPP_LIBDIR} ${PC_JSONCPP_LIBRARY_DIRS}) - find_library(jsoncpp_STATIC_LIBRARY NAMES libjsoncpp.a - PATHS ${PC_JSONCPP_LIBDIR} ${PC_JSONCPP_LIBRARY_DIRS}) +find_library(jsoncpp_LIBRARY NAMES libjsoncpp.so libjsoncpp.dylib + PATHS ${PC_JSONCPP_LIBDIR} ${PC_JSONCPP_LIBRARY_DIRS}) +find_library(jsoncpp_STATIC_LIBRARY NAMES libjsoncpp.a + PATHS ${PC_JSONCPP_LIBDIR} ${PC_JSONCPP_LIBRARY_DIRS}) - find_path(jsoncpp_INCLUDE_DIR - NAMES json/json.h - PATHS ${PC_JSONCPP_INCLUDE_DIRS} - ) +find_path(jsoncpp_INCLUDE_DIR + NAMES json/json.h + PATHS ${PC_JSONCPP_INCLUDE_DIRS}) - find_package_handle_standard_args(jsoncpp - REQUIRED_VARS jsoncpp_LIBRARY jsoncpp_INCLUDE_DIR - VERSION_VAR json_VERSION - ) +find_package_handle_standard_args(jsoncpp + REQUIRED_VARS jsoncpp_LIBRARY jsoncpp_INCLUDE_DIR + VERSION_VAR jsoncpp_VERSION_STRING) +if(jsoncpp_FOUND) add_library(jsoncpp_lib_static STATIC IMPORTED) set_target_properties(jsoncpp_lib_static PROPERTIES INTERFACE_INCLUDE_DIRECTORIES ${jsoncpp_INCLUDE_DIR} diff --git a/cmake/Findlibtins.cmake b/cmake/Findlibtins.cmake index c72cdc29..ae181213 100644 --- a/cmake/Findlibtins.cmake +++ b/cmake/Findlibtins.cmake @@ -1,35 +1,58 @@ +# Package configuration on supported platforms: +# debian: only pkgconfig provided +# vcpkg: config.cmake, `tins` target and LIBTINS_INCLUDE_DIRS, LIBTINS_LIBRARIES for compat +# brew: same as vcpkg, but LIBTINS_INCLUDE_DIRS is wrong +# conan: `libtins`, `libtins::libtins` targets and libtins_INCLUDE_DIRS, libtins_LIBRARIES +# libtins_LIBRARIES can't be used because it contains paths that differ between build and +# install when building with conan + # Try to find a cmake config compatible with vcpkg and fall back to defining # targets using pkgconfig. - include(FindPackageHandleStandardArgs) -# recent libtins config shipped with vcpkg defines this target +# Prefer config, if found find_package(libtins CONFIG QUIET) -if(WIN32 OR (libtins_FOUND AND TARGET tins)) - find_package_handle_standard_args(libtins DEFAULT_MSG libtins_CONFIG) - return() -endif() - -# fall back to pkg-config -find_package(PkgConfig QUIET REQUIRED) -pkg_check_modules(PC_TINS REQUIRED libtins) - -set(libtins_VERSION ${PC_TINS_VERSION}) +if(libtins_FOUND AND TARGET tins) + # The config in vcpkg and homebrew doesn't set interface include directories + # on the target (since it's usually installed and included relative to the + # system include path). That assumption fails if brew is installed under /opt. + # The config does, however, set LIBTINS_INCLUDE_DIRS but bizarrely, this + # appears to be set to a path under /tmp on homebrew, so we find the actual + # include dir here and create our own target below. + unset(LIBTINS_INCLUDE_DIRS) + find_path(LIBTINS_INCLUDE_DIRS NAMES tins/tins.h) + set_target_properties(tins PROPERTIES + INTERFACE_INCLUDE_DIRECTORIES ${LIBTINS_INCLUDE_DIRS}) + + find_package_handle_standard_args(libtins CONFIG_MODE) +else() + # Fall back to find_library with hints from pkgconfig + find_package(PkgConfig QUIET) + if(PKG_CONFIG_FOUND) + pkg_check_modules(PC_TINS QUIET libtins) + + if(PC_TINS_FOUND) + set(LIBTINS_VERSION ${PC_TINS_VERSION}) + endif() + endif() + + find_library(LIBTINS_LIBRARIES NAMES libtins.so libtins.dylib + PATHS ${PC_TINS_LIBDIR} ${PC_TINS_LIBRARY_DIRS}) + + find_path(LIBTINS_INCLUDE_DIRS + NAMES tins/tins.h + PATHS ${PC_TINS_INCLUDE_DIRS}) -find_library(tins_LIBRARY NAMES libtins.so libtins.dylib - PATHS ${PC_TINS_LIBDIR} ${PC_TINS_LIBRARY_DIRS}) + find_package_handle_standard_args(libtins + REQUIRED_VARS LIBTINS_LIBRARIES LIBTINS_INCLUDE_DIRS + VERSION_VAR LIBTINS_VERSION) +endif() -find_path(tins_INCLUDE_DIR - NAMES tins/tins.h - PATHS ${PC_TINS_INCLUDE_DIRS} -) - -find_package_handle_standard_args(libtins - REQUIRED_VARS tins_LIBRARY tins_INCLUDE_DIR - VERSION_VAR tins_VERSION -) - -add_library(tins SHARED IMPORTED) -set_target_properties(tins PROPERTIES - INTERFACE_INCLUDE_DIRECTORIES ${tins_INCLUDE_DIR} - IMPORTED_LOCATION ${tins_LIBRARY}) +# LIBTINS_LIBRARIES is set, add target with a name compatible with the conan +# package +if(NOT TARGET libtins) + add_library(libtins INTERFACE) + set_target_properties(libtins PROPERTIES + INTERFACE_LINK_LIBRARIES ${LIBTINS_LIBRARIES}) + install(TARGETS libtins EXPORT ouster-sdk-targets) +endif() diff --git a/cmake/OusterSDKConfig.cmake.in b/cmake/OusterSDKConfig.cmake.in index 66d3df10..d6e97567 100644 --- a/cmake/OusterSDKConfig.cmake.in +++ b/cmake/OusterSDKConfig.cmake.in @@ -5,30 +5,26 @@ include(CMakeFindDependencyMacro) # ouster_client dependencies find_dependency(Eigen3) find_dependency(jsoncpp) - -set(BUILD_VIZ @BUILD_VIZ@) +find_dependency(CURL) # viz dependencies -if (${BUILD_VIZ}) +if(@BUILD_VIZ@) set(OpenGL_GL_PREFERENCE GLVND) find_dependency(OpenGL) find_dependency(Threads) + find_dependency(glfw3) - find_package(glad QUIET) - if(glad_FOUND) - message(STATUS " OusterSDK: Found glad ${glad_CONFIG}") - set(GL_LOADER glad::glad) + if(@OUSTER_VIZ_USE_GLAD@) + find_dependency(glad) else() - message(STATUS " OusterSDK: glad NOT found, falling back to GLEW") - find_package(GLEW REQUIRED) - set(GL_LOADER GLEW::GLEW) - add_definitions("-DOUSTER_VIZ_GLEW") + find_dependency(GLEW) endif() - - find_dependency(glfw3) endif() -# pcap dependencies (no cmake config on debian 10) -# find_dependency(libtins) +if(@BUILD_PCAP@) + # make libtins dependency optional; on debian distros, libtins doesn't include + # a config module and sdk targets will just include paths in that case + find_package(libtins QUIET) +endif() include("${CMAKE_CURRENT_LIST_DIR}/OusterSDKTargets.cmake") diff --git a/cmake/VcpkgEnv.cmake b/cmake/VcpkgEnv.cmake new file mode 100644 index 00000000..c46f0f08 --- /dev/null +++ b/cmake/VcpkgEnv.cmake @@ -0,0 +1,17 @@ +# Make it possible to configure builds against vcpkg using env variables +# - must include() this before project() in CMakeLists.txt +# https://vcpkg.readthedocs.io/en/latest/users/integration/ + +# set toolchain file if VCPKG_ROOT is defind +if(DEFINED ENV{VCPKG_ROOT} AND NOT DEFINED CMAKE_TOOLCHAIN_FILE) + message(STATUS "Using CMAKE_TOOLCHAIN_FILE from env VCPKG_ROOT: $ENV{VCPKG_ROOT}") + set(CMAKE_TOOLCHAIN_FILE "$ENV{VCPKG_ROOT}/scripts/buildsystems/vcpkg.cmake" + CACHE STRING "The CMake toolchain file") +endif() + +# set VCPKG_TARGET_TRIPLET from corresponding env variable +if(DEFINED ENV{VCPKG_TARGET_TRIPLET} AND NOT DEFINED VCPKG_TARGET_TRIPLET) + message(STATUS "Using VCPKG_TARGET_TRIPLET from env: $ENV{VCPKG_TARGET_TRIPLET}") + set(VCPKG_TARGET_TRIPLET "$ENV{VCPKG_TARGET_TRIPLET}" + CACHE STRING "Triplet used for vcpkg build") +endif() diff --git a/cmake/VersionGen.cmake b/cmake/VersionGen.cmake index 00442a96..83b84bab 100644 --- a/cmake/VersionGen.cmake +++ b/cmake/VersionGen.cmake @@ -1,23 +1,54 @@ -cmake_minimum_required(VERSION 3.1.0) +if(CMAKE_SCRIPT_MODE_FILE) + # in build stage + set(GIT_HASH "unknown") -set(GIT_HASH "unknown") + find_package(Git QUIET) + if(GIT_FOUND) + execute_process( + COMMAND ${GIT_EXECUTABLE} describe --always --match none --dirty + OUTPUT_VARIABLE GIT_OUTPUT + RESULT_VARIABLE GIT_RESULT + ERROR_QUIET + WORKING_DIRECTORY ${VERSION_GEN_SOURCE_DIR}) -find_package(Git QUIET) + if(${GIT_RESULT} EQUAL 0) + set(GIT_HASH "${GIT_OUTPUT}") + endif() + endif() -if(GIT_FOUND) - execute_process( - COMMAND ${GIT_EXECUTABLE} describe --always --match none --dirty - OUTPUT_VARIABLE GIT_OUTPUT - RESULT_VARIABLE GIT_RESULT - ERROR_QUIET - WORKING_DIRECTORY ${VERSION_GEN_SOURCE_DIR}) + string(STRIP "${GIT_HASH}" GIT_HASH) + string(TOLOWER "${BUILD_TYPE}" BUILD_TYPE) - if(${GIT_RESULT} EQUAL 0) - set(GIT_HASH "${GIT_OUTPUT}") + configure_file( + ${VERSION_GEN_SOURCE_DIR}/build.h.in + ${VERSION_GEN_OUT_DIR}/build.h @ONLY) +elseif(NOT TARGET ouster_build) + # in configuration stage: expects OusterSDK_VERSION_STRING to be set + if(OusterSDK_VERSION_STRING MATCHES "^([0-9]+\.[0-9]+\.[0-9]+)(-([.0-9A-z]+))?$") + set(OusterSDK_VERSION "${CMAKE_MATCH_1}") + set(OusterSDK_VERSION_SUFFIX "${CMAKE_MATCH_3}") + else() + message(FATAL_ERROR "Failed to parse version string: ${OusterSDK_VERSION_STRING}") endif() -endif() -string(STRIP "${GIT_HASH}" GIT_HASH) -string(TOLOWER "${BUILD_TYPE}" BUILD_TYPE) + add_custom_target(ouster_generate_header) + add_custom_command(TARGET ouster_generate_header PRE_BUILD + WORKING_DIRECTORY ${CMAKE_BINARY_DIR} + COMMENT "Generating build info header" + COMMAND ${CMAKE_COMMAND} + -DVERSION_GEN_OUT_DIR="${CMAKE_CURRENT_BINARY_DIR}/generated/ouster/impl" + -DVERSION_GEN_SOURCE_DIR="${CMAKE_CURRENT_LIST_DIR}" + -DBUILD_TYPE="${CMAKE_BUILD_TYPE}" + -DBUILD_SYSTEM="${CMAKE_SYSTEM_NAME}" + -DOusterSDK_VERSION="${OusterSDK_VERSION}" + -DOusterSDK_VERSION_SUFFIX="${OusterSDK_VERSION_SUFFIX}" + -P ${CMAKE_CURRENT_LIST_FILE}) -configure_file(${VERSION_GEN_SOURCE_DIR}/cmake/build.h.in ${VERSION_GEN_OUT_DIR}/build.h @ONLY) + add_library(ouster_build INTERFACE) + target_include_directories(ouster_build INTERFACE + $) + add_dependencies(ouster_build ouster_generate_header) + install(DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/generated/ouster + DESTINATION include) + +endif() diff --git a/cmake/build.h.in b/cmake/build.h.in index a587b689..0ad0901f 100644 --- a/cmake/build.h.in +++ b/cmake/build.h.in @@ -8,10 +8,10 @@ const char* const BUILD_TYPE = "@BUILD_TYPE@"; const char* const BUILD_SYSTEM = "@BUILD_SYSTEM@"; -const char* const CLIENT_VERSION = - "@ouster_client_VERSION@@ouster_client_VERSION_SUFFIX@"; +const char* const SDK_VERSION = + "@OusterSDK_VERSION@@OusterSDK_VERSION_SUFFIX@"; -const char* const CLIENT_VERSION_FULL = - "@ouster_client_VERSION@@ouster_client_VERSION_SUFFIX@+@GIT_HASH@-@BUILD_TYPE@"; +const char* const SDK_VERSION_FULL = + "@OusterSDK_VERSION@@OusterSDK_VERSION_SUFFIX@+@GIT_HASH@-@BUILD_TYPE@"; } diff --git a/conan/test_package/CMakeLists.txt b/conan/test_package/CMakeLists.txt index 07e79252..dacc3d41 100644 --- a/conan/test_package/CMakeLists.txt +++ b/conan/test_package/CMakeLists.txt @@ -1,19 +1,7 @@ -cmake_minimum_required(VERSION 3.1) -project(PackageTest CXX) - -set(CMAKE_CXX_STANDARD 11) -set(CMAKE_CXX_STANDARD_REQUIRED ON) +cmake_minimum_required(VERSION 3.10.0) -include(${CMAKE_BINARY_DIR}/conanbuildinfo.cmake) -conan_basic_setup() +project(PackageTest CXX) find_package(OusterSDK REQUIRED) -add_executable(example_client example_client.cpp) -target_link_libraries(example_client OusterSDK::ouster_client) - -# CTest is a testing tool that can be used to test your project. -# enable_testing() -# add_test(NAME example -# WORKING_DIRECTORY ${CMAKE_BINARY_DIR}/bin -# COMMAND example) +add_subdirectory(../../examples examples) diff --git a/conan/test_package/conanfile.py b/conan/test_package/conanfile.py index 2e3d6120..782c8272 100644 --- a/conan/test_package/conanfile.py +++ b/conan/test_package/conanfile.py @@ -4,12 +4,15 @@ class OusterSDKTestConan(ConanFile): settings = "os", "compiler", "build_type", "arch" - generators = "cmake", "cmake_find_package" + generators = "cmake_paths", "cmake_find_package" def build(self): cmake = CMake(self) # Current dir is "test_package/build/" and CMakeLists.txt is # in "test_package" + cmake.definitions[ + "CMAKE_PROJECT_PackageTest_INCLUDE"] = os.path.join( + self.build_folder, "conan_paths.cmake") cmake.configure() cmake.build() @@ -20,5 +23,5 @@ def imports(self): def test(self): if not tools.cross_building(self): - os.chdir("bin") - self.run(".%sexample_client" % os.sep) + os.chdir("examples") + self.run(".%sclient_example" % os.sep) diff --git a/conan/test_package/example_client.cpp b/conan/test_package/example_client.cpp deleted file mode 100644 index b35bd2ae..00000000 --- a/conan/test_package/example_client.cpp +++ /dev/null @@ -1,14 +0,0 @@ -#include - -#include - -int main() { - ouster::LidarScan scan{1024, 64}; - - for (const auto& f : scan) { - std::cout << "Field: " << ouster::sensor::to_string(f.first) - << std::endl; - } - - return 0; -} diff --git a/conanfile.py b/conanfile.py index 944ae2e2..82d9f1d2 100644 --- a/conanfile.py +++ b/conanfile.py @@ -1,12 +1,11 @@ import os +import re from conans import ConanFile, CMake, tools from pprint import pformat - class OusterSDKConan(ConanFile): name = "ouster_sdk" - version = "0.4.1" license = "BSD 3-Clause License" author = "Ouster, Inc." url = "https://github.com/ouster-lidar/ouster_example" @@ -19,14 +18,16 @@ class OusterSDKConan(ConanFile): "build_pcap": [True, False], "shared": [True, False], "fPIC": [True, False], - "ensure_cpp17": [True, False] + "ensure_cpp17": [True, False], + "eigen_max_align_bytes": [True, False], } default_options = { "build_viz": False, "build_pcap": False, "shared": False, "fPIC": True, - "ensure_cpp17": False + "ensure_cpp17": False, + "eigen_max_align_bytes": False, } generators = "cmake_paths", "cmake_find_package" @@ -44,6 +45,12 @@ class OusterSDKConan(ConanFile): "README.rst" ] + # https://docs.conan.io/en/1.51/howtos/capture_version.html#how-to-capture-package-version-from-text-or-build-files + def set_version(self): + content = tools.load(os.path.join(self.recipe_folder, "CMakeLists.txt")) + version = re.search("set\(OusterSDK_VERSION_STRING (.*)\)", content).group(1) + self.version = version.strip() + def config_options(self): if self.settings.os == "Windows": del self.options.fPIC @@ -51,22 +58,30 @@ def config_options(self): def requirements(self): self.requires("eigen/3.4.0") self.requires("jsoncpp/1.9.5") + self.requires("libcurl/7.82.0") if self.options.build_pcap: self.requires("libtins/4.3") + # override due to conflict b/w libtins and libcurl + self.requires("openssl/1.1.1q") + if self.options.build_viz: self.requires("glad/0.1.35") # glew is optional, and probably will not be needed # self.requires("glew/2.2.0") + # glfw pulls in xorg/system, the latest revision of which (7c17659) requires updates + # pin to older revision which doesn't require install/update, thus overriding glfw's xorg/system + self.requires("xorg/system@#60bff7b91495dc0366ae6a9ae60d73a9") self.requires("glfw/3.3.6") # maybe needed for cpp examples, but not for the lib # self.requires("tclap/1.2.4") def configure_cmake(self): cmake = CMake(self) - cmake.definitions["BUILD_VIZ"] = True if self.options.build_viz else False - cmake.definitions["BUILD_PCAP"] = True if self.options.build_pcap else False + cmake.definitions["BUILD_VIZ"] = self.options.build_viz + cmake.definitions["BUILD_PCAP"] = self.options.build_pcap + cmake.definitions["USE_EIGEN_MAX_ALIGN_BYTES_32"] = self.options.eigen_max_align_bytes # alt way, but we use CMAKE_TOOLCHAIN_FILE in other pipeline so avoid overwrite # cmake.definitions["CMAKE_TOOLCHAIN_FILE"] = os.path.join(self.build_folder, "conan_paths.cmake") cmake.definitions[ diff --git a/docs/conf.py b/docs/conf.py index 8f0f9bcc..a7acf8fb 100644 --- a/docs/conf.py +++ b/docs/conf.py @@ -24,13 +24,13 @@ import os import sys -project = 'Ouster Python SDK' +project = 'Ouster Sensor SDK' copyright = '2022, Ouster, Inc.' author = 'Ouster SW' # The full version, including alpha/beta/rc tags -version = '0.4.0' -release = '0.4.0' +version = '0.5.0' +release = '0.5.0' # -- General configuration --------------------------------------------------- @@ -135,7 +135,7 @@ # ----- Todos Configs ------ todo_include_todos = False todo_link_only = True -todo_emit_warnings = False +todo_emit_warnings = True # copybutton configs # Note: last entry treats four spaces as a prompt to support "continuation lines" diff --git a/docs/cpp/building.rst b/docs/cpp/building.rst index 2da58102..cecc44d1 100644 --- a/docs/cpp/building.rst +++ b/docs/cpp/building.rst @@ -4,40 +4,55 @@ Building the C++ Client from Source =================================== -Building the example code requires a compiler supporting C++11 and CMake 3.1 or newer and the tclap, -jsoncpp, and Eigen3 libraries with headers installed on the system. The sample visualizer also +Building the example code requires a compiler supporting C++11 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. +The C++ example code is available `on the Ouster Github +`_. Follow the instructions for cloning the project. + Building on Linux / macOS -========================== +========================= + +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 + +You may also install curl with a different ssl backend, for example libcurl4-gnutls-dev or +libcurl4-nss-dev. -To install build dependencies on Ubuntu, run:: +On macOS, install XCode and `homebrew `_ and run: - sudo apt install build-essential cmake libglfw3-dev libglew-dev libeigen3-dev \ - libjsoncpp-dev libtclap-dev +.. code:: console -On macOS, install XCode and `homebrew `_ and run:: + $ brew install cmake pkg-config jsoncpp eigen curl libtins glfw glew - brew install cmake pkg-config glfw glew eigen jsoncpp tclap +To build run the following commands: -To build run the following commands:: +.. code:: console - mkdir build - cd build - cmake -DCMAKE_BUILD_TYPE=Release - make + $ mkdir build + $ cd build + $ cmake -DCMAKE_BUILD_TYPE=Release -DBUILD_EXAMPLES=ON + $ cmake --build . where ```` is the location of the ``ouster_example`` source directory. The -CMake build script supports several optional flags:: +CMake build script supports several optional flags. Add any of the following to override the +defaults: - -DBUILD_VIZ=OFF Do not build the sample visualizer - -DBUILD_PCAP=ON Build pcap tools. Requires libpcap and libtins dev packages - -DBUILD_SHARED_LIBS Build shared libraries (.dylib or .so) - -DCMAKE_POSITION_INDEPENDENT_CODE Standard flag for position independent code - -DBUILD_EXAMPLES=ON Build C++ examples +.. code:: console + + -DBUILD_VIZ=OFF # Do not build the sample visualizer + -DBUILD_PCAP=OFF # Do not build pcap tools + -DBUILD_EXAMPLES=ON # Build C++ examples + -DBUILD_TESTING=ON # Build tests + -DBUILD_SHARED_LIBS=ON # Build shared instead of static libraries Building on Windows -==================== +=================== The example code can be built on Windows 10 with Visual Studio 2019 using CMake support and vcpkg for dependencies. Follow the official documentation to set up your build environment: @@ -54,13 +69,17 @@ for dependencies. Follow the official documentation to set up your build environ to use the correct versions of the dependencies. Building may fail unexpectedly if you skip this step. -Don't forget to integrate vcpkg with Visual Studio after bootstrapping:: +Don't forget to integrate vcpkg with Visual Studio after bootstrapping: + +.. code:: powershell + + PS > .\vcpkg.exe integrate install - .\vcpkg.exe integrate install +You should be able to install dependencies with -You should be able to install dependencies with:: +.. code:: powershell - .\vcpkg.exe install --triplet x64-windows glfw3 glad[gl-api-33] tclap jsoncpp eigen3 + PS > .\vcpkg.exe install --triplet x64-windows jsoncpp eigen3 curl libtins glfw3 glew After these steps are complete, you should be able to open, build and run the ``ouster_example`` project using Visual Studio: @@ -72,24 +91,28 @@ project using Visual Studio: 4. 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. To use the resulting binaries, go to **View > Terminal** and run, for example: - .\out\build\x64-Release\ouster_client\ouster_client_example.exe +.. code:: powershell + + .\out\build\x64-Release\examples\client_example.exe .. _building in release mode: https://docs.microsoft.com/en-us/visualstudio/debugger/how-to-set-debug-and-release-configurations?view=vs-2019 Running the Sample Client -========================== +========================= Make sure the sensor is connected to the network. See "Connecting to the Sensor" in the `Software User Manual `_ for instructions and different options for network configuration. -Navigate to ``ouster_client`` under the build directory, which should contain an executable named -``ouster_client_example``. This program will attempt to connect to the sensor, capture lidar data, -and write point clouds out to CSV files:: +Navigate to ``examples`` under the build directory, which should contain an executable named +``client_example``. This program will attempt to connect to the sensor, capture lidar data, and +write point clouds out to CSV files: + +.. code:: console - ./ouster_client_example + $ ./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 diff --git a/docs/cpp/ouster_client/client.rst b/docs/cpp/ouster_client/client.rst index 32a4a8f1..1a786dfa 100644 --- a/docs/cpp/ouster_client/client.rst +++ b/docs/cpp/ouster_client/client.rst @@ -16,9 +16,9 @@ Data Fetching .. doxygenfunction:: ouster::sensor::poll_client -.. doxygenfunction:: ouster::sensor::read_lidar_packet - -.. doxygenfunction:: ouster::sensor::read_imu_packet +.. doxygenfunction:: ouster::sensor::read_lidar_packet(const client& cli, uint8_t* buf, const packet_format& pf) + +.. doxygenfunction:: ouster::sensor::read_imu_packet(const client& cli, uint8_t* buf, const packet_format& pf) Config And Metadata =================== diff --git a/docs/cpp/ouster_pcap/os_pcap.rst b/docs/cpp/ouster_pcap/os_pcap.rst index 73280e93..2af04aaf 100644 --- a/docs/cpp/ouster_pcap/os_pcap.rst +++ b/docs/cpp/ouster_pcap/os_pcap.rst @@ -37,6 +37,6 @@ Functions .. doxygenfunction:: ouster::sensor_utils::record_uninitialize -.. doxygenfunction:: ouster::sensor_utils::record_packet +.. doxygenfunction:: ouster::sensor_utils::record_packet(record_handle& handle, const packet_info& info, const uint8_t* buf, size_t buffer_size) diff --git a/docs/installation.rst b/docs/installation.rst index 81aea196..4425ec80 100644 --- a/docs/installation.rst +++ b/docs/installation.rst @@ -29,7 +29,9 @@ Supported Platforms Installation -------------- -The Ouster Python SDK requires Python >= 3.6 and pip >= 19.0. +The Ouster Python SDK binary packages require Python >= 3.7 and pip >= 19.0 on most platforms. On +Ubuntu 18.04, the default Python 3 version is is 3.6, so you'll have to install and use +``python3.7`` explicitly. On Apple M1, you'll need need Python >= 3.8. .. note:: @@ -38,18 +40,12 @@ The Ouster Python SDK requires Python >= 3.6 and pip >= 19.0. venv. If you're using venv on Windows, you'll want to use ``python`` and ``pip`` instead of ``py -3`` and ``py -3 -m pip`` in the following Powershell snippets. -.. note:: - - If you're running a non-glibc-based linux distribution, or wish to modify the Ouster Python - SDK, you will need to build from source. See the `build instructions`_ for requirements needed to - build from a source distribution or from a clone of the repository. - -.. note:: - - Apple M1 users should be aware that due to numpy support limitations they will need to use Python - >=3.8. +If you're using an unsupported platform like a non-glibc-based linux distribution, or wish to modify +the Ouster Python SDK, you will need to build from source. See the `build instructions`_ for +requirements needed to build from a source distribution or from a clone of the repository. -To install on :ref:`supported platforms`, please upgrade your pip: +To install on :ref:`supported platforms`, first make sure you have the latest +version of pip: .. tabs:: diff --git a/docs/overview.rst b/docs/overview.rst index ba43ecf3..909f1d4d 100644 --- a/docs/overview.rst +++ b/docs/overview.rst @@ -1,22 +1,9 @@ Ouster Sensor SDK ================= -The Ouster Sensor SDK provides developers interfaces for interacting with sensor hardware and -recorded sensor data suitable for prototyping, evaluation, and other non-safety-critical -applications in Python and C++. Example and reference code is provided for common operations on -sensor data in both languages. The SDK includes APIs for: - -* Querying and setting sensor configuration -* Recording and reading data in pcap format -* Reading and buffering sensor UDP data streams reliably -* Conversion of raw data to range/signal/near_ir/reflectivity images (destaggering) -* Efficient projection of range measurements to Cartesian (x, y, z) corrdinates -* Visualization of multi-beam flash lidar data - -Additionally, in Python, the SDK also provides: - -* Frame-based access to lidar data as numpy datatypes -* A responsive visualizer utility for pcap and sensor +.. include:: /../python/README.rst + :start-after: [sdk-overview-start] + :end-before: [sdk-overview-end] Quick links ----------- diff --git a/docs/python/api/examples.rst b/docs/python/api/examples.rst index 450ddddf..166326ae 100644 --- a/docs/python/api/examples.rst +++ b/docs/python/api/examples.rst @@ -12,7 +12,7 @@ Module :mod:`ouster.sdk.examples` Client Examples :mod:`ouster.sdk.examples.client` -================================================== +================================================= .. automodule:: ouster.sdk.examples.client :members: @@ -22,7 +22,7 @@ Client Examples :mod:`ouster.sdk.examples.client` PCAP Examples :mod:`ouster.sdk.examples.pcap` -================================================== +============================================= .. automodule:: ouster.sdk.examples.pcap :members: @@ -31,14 +31,14 @@ PCAP Examples :mod:`ouster.sdk.examples.pcap` Open3D Examples :mod:`ouster.sdk.examples.open3d` -================================================== +================================================= .. automodule:: ouster.sdk.examples.open3d :members: ---- -``PointViz`` Examples :mod:`ouster.sdk.examples.viz` +Examples ``PointViz`` :mod:`ouster.sdk.examples.viz` ==================================================== .. automodule:: ouster.sdk.examples.viz diff --git a/docs/python/devel.rst b/docs/python/devel.rst index 2d1ebb90..70d950c3 100644 --- a/docs/python/devel.rst +++ b/docs/python/devel.rst @@ -12,14 +12,16 @@ Building the Python SDK from source requires several dependencies: - a C++14-capable compiler - `cmake `_ >= 3.5 - `eigen `_ >= 3.3 +- `curl `_ >= 7.58 - `jsoncpp `_ >= 1.7 - `libtins `_ >= 3.4 - `libpcap `_ - `libglfw3 `_ >= 3.2 - `libglew `_ >= 2.1 or `glad `_ -- `Python `_ >= 3.6 (with headers and development libraries) +- `Python `_ >= 3.7 (with headers and development libraries) - `pybind11 `_ >= 2.0 +The Python SDK source is available `on the Ouster Github `_. You should clone the whole project. Linux and macos --------------- @@ -30,13 +32,14 @@ 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 libglfw3-dev libglew-dev + python3-dev python3-pip pybind11-dev libcurl4-openssl-dev \ + libglfw3-dev libglew-dev On macos >= 10.13, using homebrew, you should be able to run: .. code:: console - $ brew install cmake eigen jsoncpp libtins python3 pybind11 glfw glew + $ brew install cmake eigen curl jsoncpp libtins python3 pybind11 glfw glew After you have the system dependencies, you can build the SDK with: @@ -70,7 +73,7 @@ package manager and run: .. code:: powershell - PS > vcpkg install eigen3 jsoncpp libtins pybind11 glfw3 glad[gl-api-33] + PS > vcpkg install --triplet=x64-windows eigen3 jsoncpp libtins pybind11 glfw3 glad[gl-api-33] The currently tested vcpkg tag is ``2022.02.23``. After that, using a developer powershell prompt: @@ -82,6 +85,13 @@ The currently tested vcpkg tag is ``2022.02.23``. After that, using a developer # point cmake to the location of vcpkg (make sure to use an absolute path) PS > $env:CMAKE_TOOLCHAIN_FILE="\scripts\buildsystems\vcpkg.cmake" + # set the correct vcpkg triplet + PS > $env:VCPKG_TARGET_TRIPLET="x64-windows" + + # set build options related to the compiler + PS > $env:CMAKE_GENERATOR_PLATFORM="x64" + PS > $env:CMAKE_GENERATOR="Visual Studio 15 2017" + # then, build an installable "wheel" package PS > py -m pip wheel --no-deps "$env:OUSTER_SDK_PATH\python" @@ -127,6 +137,13 @@ SDK package: $ cd ${OUSTER_SDK_PATH}/python $ python3 -m pytest +To run interactive :class:`.viz.PointViz` tests, use ``--interactive`` argument: + +.. code:: console + + $ cd ${OUSTER_SDK_PATH}/python + $ python3 -m pytest --interactive + To run tests against multiple Python versions simultaneously, use the ``tox`` package: .. code:: console @@ -149,10 +166,10 @@ image, run: $ docker build ${OUSTER_SDK_PATH} -f ${OUSTER_SDK_PATH}/python/Dockerfile \ --build-arg BASE=ubuntu:20.04 \ - -t ouster-sdk-tox \ + -t ouster-sdk-tox the ``BASE`` argument will default to ``ubuntu:18.04``, but can also be set to other docker tags, -e.g. ``ubuntu:20.04`` or ``debian:10``. Then, run the container to invoke tox: +e.g. ``ubuntu:20.04``, ``ubuntu:22.04`` or ``debian:10``. Then, run the container to invoke tox: .. code:: console diff --git a/docs/python/examples/lidar-scan.rst b/docs/python/examples/lidar-scan.rst index 0111c483..7b7c25ae 100644 --- a/docs/python/examples/lidar-scan.rst +++ b/docs/python/examples/lidar-scan.rst @@ -8,7 +8,7 @@ The LidarScan Representation :local: :depth: 3 -The :py:class:`.LidarScan` class is explained in depth in the `LidarScan reference `, which we recommend reading. +The :py:class:`.LidarScan` class is explained in depth in the :doc:`/reference/lidar-scan`, which we recommend reading. We provide example code to aid in understanding. diff --git a/docs/python/examples/record-stream.rst b/docs/python/examples/record-stream.rst index 98e9ecff..444897e8 100644 --- a/docs/python/examples/record-stream.rst +++ b/docs/python/examples/record-stream.rst @@ -53,11 +53,11 @@ a live feed from your :ref:`configured` sensor: .. code-tab:: console Linux/macOS - $ python3 -m ouster.sdk.examples.client $SENSOR_HOSTNAME live-plot-signal + $ python3 -m ouster.sdk.examples.client $SENSOR_HOSTNAME live-plot-reflectivity .. code-tab:: powershell Windows x64 - PS > py -3 -m ouster.sdk.examples.client $SENSOR_HOSTNAME live-plot-signal + PS > py -3 -m ouster.sdk.examples.client $SENSOR_HOSTNAME live-plot-reflectivity This should give you a live feed from your sensor that looks like a black and white moving image. Try waving your hand or moving around to find yourself within the image! @@ -65,8 +65,8 @@ Try waving your hand or moving around to find yourself within the image! So how did we do that? .. literalinclude:: /../python/src/ouster/sdk/examples/client.py - :start-after: [doc-stag-live-plot-signal] - :end-before: [doc-etag-live-plot-signal] + :start-after: [doc-stag-live-plot-reflectivity] + :end-before: [doc-etag-live-plot-reflectivity] :emphasize-lines: 2-3 :linenos: :dedent: diff --git a/docs/python/viz/index.rst b/docs/python/viz/index.rst index aa001eb8..9851f0f0 100644 --- a/docs/python/viz/index.rst +++ b/docs/python/viz/index.rst @@ -10,7 +10,7 @@ consists of the following: - ``ouster_viz``: the core C++ library - :mod:`ouster.sdk.viz`: the Python module for the bindings -``simaple-viz`` is a fastest way to visualize live sensor data or replaying a recorded ``pcap``: +``simple-viz`` is a fastest way to visualize data from a connected sensor or a recorded ``pcap``: .. figure:: /images/simple-viz.png :align: center diff --git a/docs/reference/lidar-scan.rst b/docs/reference/lidar-scan.rst index 12443aba..22e7296c 100644 --- a/docs/reference/lidar-scan.rst +++ b/docs/reference/lidar-scan.rst @@ -114,6 +114,15 @@ scan through an iterator: :end-before: [doc-etag-cpp-scan-iter] :dedent: +.. note:: + + The units of a particular field from a ``LidarScan`` are consistent even + when you use lidar profiles which scale the returned data from the sensor. + This is because the LidarScan will reverse the scaling for you when + parsing. For example, the RANGE field on a LidarScan constructed with the + low data rate profile will be in millimeters even though the return from + the sensor is given in 8mm increments. + Running the above code on a sample ``LidarScan`` will give you output that looks like: .. include:: /python/examples/lidar-scan.rst diff --git a/docs/ros/index.rst b/docs/ros/index.rst index 7f05b2a7..3a958849 100644 --- a/docs/ros/index.rst +++ b/docs/ros/index.rst @@ -4,12 +4,12 @@ Example ROS Code ================ -The sample code include tools for publishing sensor data as standard ROS topics. Since ROS uses its -own build system, it must be compiled separately from the rest of the sample code. +The sample code include tools for publishing sensor data as standard ROS topics. Since ROS uses +its own build system, it must be compiled separately from the rest of the sample code. -The provided ROS code has been tested on ROS Kinetic, Melodic, and Noetic on Ubuntu 16.04, 18.04, -and 20.04, respectively. Use the `installation instructions `_ to get -started with ROS on your platform. +The provided ROS code has been tested on ROS Melodic on Ubuntu 18, and ROS Noetic Ubuntu 20. Use +the `installation instructions `_ to get started with ROS +on your platform. Building ROS Driver ==================== @@ -20,11 +20,12 @@ The build dependencies include those of the sample code:: Additionally, you should install the ros dependencies:: - sudo apt install ros--ros-core ros--pcl-ros \ - ros--tf2-geometry-msgs ros--rviz - -where ```` is ``kinetic``, ``melodic``, or ``noetic``. + sudo apt install \ + ros--pcl-ros \ + ros--tf2-geometry-msgs \ + ros--rviz +where ```` is ``melodic`` or ``noetic``. Alternatively, if you would like to install dependencies with `rosdep`:: @@ -32,19 +33,20 @@ Alternatively, if you would like to install dependencies with `rosdep`:: To build:: - source /opt/ros//setup.bash - mkdir -p ./myworkspace/src - cd myworkspace + mkdir -p ./catkin_ws/src + cd catkin_ws ln -s ./src/ + source /opt/ros//setup.bash catkin_make -DCMAKE_BUILD_TYPE=Release -**Warning:** Do not create your workspace directory inside the cloned ouster_example repository, as -this will confuse the ROS build system. +.. warning:: + Do not create your workspace directory inside the cloned ouster_example repository, + as this will confuse the ROS build system. For each command in the following sections, make sure to first set up the ROS environment in each new terminal by running:: - source myworkspace/devel/setup.bash + source catkin_ws/devel/setup.bash Running ROS Nodes with a Sensor ================================ @@ -53,60 +55,112 @@ Make sure the sensor is connected to the network. See "Connecting to the Sensor" User Manual `_ for instructions and different options for network configuration. -To publish ROS topics from a running sensor, run:: +To connect to a sensor and publish its data as ROS topics, execute the command:: - roslaunch ouster_ros ouster.launch sensor_hostname:= \ - metadata:= + roslaunch ouster_ros sensor.launch sensor_hostname:= where: +- ``sensor_hostname:=`` can be the hostname (os-99xxxxxxxxxx.local) or IP of the + sensor + +Additionally, the launch file has following list of arguments that you can use: +- ``metadata:=`` to set the name where sensor metadata configuration will be + saved to. Note that by default the working directory of all ROS nodes is set to ``${ROS_HOME}``, + which is generally ``$HOME/.ros``. If you provide a relative path to ``metadata``, i.e., + ``metadata:=meta.json`` it will write to ``${ROS_HOME}/meta.json``. If you wish the file be saved + in the current directory you may use the absolute path instead, such as ``metadata:=$PWD/meta.json`` +- ``udp_dest:=`` to specify the hostname or IP to which the sensor should send data +- ``lidar_mode:=`` where mode is one of ``512x10``, ``512x20``, ``1024x10``, ``1024x20``, or + ``2048x10``, and +- ``viz:=true/false`` to visualize the sensor output, if you have the rviz ROS package installed -* ```` can be the hostname (os-99xxxxxxxxxx) or IP of the sensor -* ```` is the path you want to save sensor metadata to. - You must provide a JSON filename at the end, not just a path to a directory. -Note that by default the working directory of all ROS nodes is set to ``${ROS_HOME}``, generally -``$HOME/.ros``. If you provide a relative path to ``metadata``, i.e., ``metadata:=meta.json``, it -will write to ``${ROS_HOME}/meta.json``. To avoid this, you can provide an absolute path to -``metadata``, i.e. ``metadata:=/home/user/meta.json``. +Recording Data +=============== -You can also optionally specify: +To record raw sensor output you may use the provided ``record.launch`` file as follows:: -* ``udp_dest:=`` to specify the hostname or IP to which the sensor should send data -* ``lidar_mode:=`` where mode is one of ``512x10``, ``512x20``, ``1024x10``, ``1024x20``, or - ``2048x10``, and -* ``viz:=true`` to visualize the sensor output, if you have the rviz ROS package installed + roslaunch ouster_ros record.launch \ + sensor_hostname:= \ + metadata:= \ + bag_file:= +This will connect to the specified sensor, write the sensor metadata to a file and start +recording imu and lidar packets to the specified bag_file once the sensor is connected. -Recording Data -=============== +It is necessary that you provide a name for the metadata file and maintain this file along +with the recorded bag_file otherwise you won't be able to play the file correctly. + +If no bag_file is specified then a name will be generated based on the current date/time. -To record raw sensor output use `rosbag record`_. After starting the ``roslaunch`` command above, in -another terminal, run:: +By default ROS saves all files to $ROS_HOME, if you want to have these files saved in the +current directory, simply give the absolute path to each file. For example:: - rosbag record /os_node/imu_packets /os_node/lidar_packets + roslaunch ouster_ros record.launch \ + sensor_hostname:= \ + metadata:=$PWD/ \ + bag_file:=$PWD/ -This will save a bag file of recorded data in the current working directory. +Alternatively, you may connect to the sensor using the ``roslaunch ouster_ros sensor.launch ..`` +command and then use the rosbag command in a separate terminal to start recording lidar packets +at any time using the following command:: -You should copy and save the metadata file alongside your data. The metadata file will be saved at -the provided path to `roslaunch`. If you run the node and cannot find the metadata file, try looking -inside your ``${ROS_HOME}``, generally ``$HOME/.ros``. Regardless, you must retain the metadata -file, as you will not be able to replay your data later without it. + rosbag record /ouster/imu_packets /ouster/lidar_packets + +For more information on rosbag functionality refer to `rosbag record`_. .. _rosbag record: https://wiki.ros.org/rosbag/Commandline#rosbag_record +.. warning:: + When recording a bag file directly via the ``rosbag record``, you need to + save the metadata information of the sensor you are connected to. This can be + achieved by supplying a path to the ``metadata`` argument of the ``sensor.launch``. + You will need the metadata file information to properly replay the recorded bag + file. + Playing Back Recorded Data ========================== -To publish ROS topics from recorded data, specify the ``replay`` and ``metadata`` parameters when -running ``roslaunch``:: - - roslaunch ouster_ros ouster.launch replay:=true metadata:= +You may use the ``replay.launch`` file to repalay previously captured sensor data. +Simply invoke the launch file with the following parameters:: -And in a second terminal run `rosbag play`_:: - - rosbag play --clock + roslaunch ouster_ros replay.launch \ + metadata:= \ + bag_file:= A metadata file is mandatory for replay of data. See `Recording Data`_ for how to obtain the metadata file when recording your data. -.. _rosbag play: https://wiki.ros.org/rosbag/Commandline#rosbag_play +Ouster ROS Services +=================== + +The ROS driver currently advertises three services ``/ouster/get_metadata``, +``/ouster/get_config``, and ``/ouster/set_config``. The first one is available +in all three modes of operation: ``Sensor``, ``Replay``, and ``Recording``. +The latter two, however, are only available in ``Sensor`` and ``Recording`` +modes. i.e. when connected to a sensor. + +The usage of the three services is described below: + +- ``/ouster/get_metadata``: This service takes no parameters and returns the + current sensor metadata, you may use as follows:: + + rosservice call /ouster/get_metadata + + This will return a json string that contains the sensor metadata + +- ``/ouster/get_config``: This service takes no parameters and returns the + current sensor configuration, you may use as follows:: + + rosservice call /ouster/get_config + + This will return a json string represting the current configuration + +- ``/ouster/set_config``: Takes a single parameter and also returns the updated + sensor configuration. You may use as follows:: + + rosservice call /ouster/set_config "config_file: ''" + + It is not guranteed that all requested configuration are applied to the sensor, + thus it is the caller responsibilty to examine the returned json object and + check which of the sensor configuration parameters were successfully applied. diff --git a/docs/sample-data.rst b/docs/sample-data.rst index db4c3b4c..709548d2 100644 --- a/docs/sample-data.rst +++ b/docs/sample-data.rst @@ -27,7 +27,7 @@ Download one of the following samples of recorded Ouster data and unzip the cont .. _OS0 128 Rev 06 Urban Drive (Dual Returns): https://data.ouster.io/sdk-samples/Rev-06-fw23/OS0-128_Rev-06_fw23_Urban-Drive_Dual-Returns.zip .. _OS1 128 Rev 06 Urban Drive (Dual Returns): https://data.ouster.io/sdk-samples/Rev-06-fw23/OS1-128_Rev-06_fw23_Urban-Drive_Dual-Returns.zip .. _OS2 128 Rev 06 Urban Drive (Dual Returns): https://data.ouster.io/sdk-samples/Rev-06-fw23/OS2-128_Rev-06_fw23_Urban-Drive_Dual-Returns.zip -.. _OS1 128 Rev 06 Urban Drive (Low Bandwidth): https://data.ouster.io/sdk-examples/Rev-06-fs23/OS1-128_Rev-06-fw23_Urban-Drive_Low-Bandwidth.zip +.. _OS1 128 Rev 06 Urban Drive (Low Bandwidth): https://data.ouster.io/sdk-samples/Rev-06-fw23/OS1-128_Rev-06_fw23_Urban-Drive_Low-Bandwidth.zip .. _OS2 128 Rev 05 Bridge: https://data.ouster.io/sdk-samples/Rev-05/OS2-128_Rev-05_Bridge/OS2-128_Rev-05_Bridge.zip In your unzipped directory, you should have two files, one ``.pcap`` file and one ``.json`` file. @@ -43,6 +43,12 @@ console. .. [end-download-instructions] +.. note:: + + All Ouster sample data is provided under the `CC BY-NC-SA license + `_, whether obtained + through the above links or from the Ouster website. + Visualize It! ============= diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index b797ce21..5b5828c3 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -1,18 +1,22 @@ -cmake_minimum_required(VERSION 3.1.0) +add_executable(client_example client_example.cpp) +target_link_libraries(client_example PRIVATE OusterSDK::ouster_client) -# === Libraries === -add_library(ouster_example_helpers helpers.cpp) -target_link_libraries(ouster_example_helpers ouster_build ouster_client ouster_pcap) - -# === Executables === add_executable(config_example config_example.cpp) -target_link_libraries(config_example PRIVATE ouster_client ouster_build ouster_example_helpers) -add_executable(OusterSDK::config_example ALIAS config_example) +target_link_libraries(config_example PRIVATE OusterSDK::ouster_client) + +if(TARGET OusterSDK::ouster_pcap) + add_executable(lidar_scan_example lidar_scan_example.cpp helpers.cpp) + target_link_libraries(lidar_scan_example PRIVATE OusterSDK::ouster_client OusterSDK::ouster_pcap) -add_executable(lidar_scan_example lidar_scan_example.cpp) -target_link_libraries(lidar_scan_example PRIVATE ouster_client ouster_build ouster_pcap ouster_example_helpers) -add_executable(OutserSDK::lidar_scan_example ALIAS lidar_scan_example) + add_executable(representations_example representations_example.cpp helpers.cpp) + target_link_libraries(representations_example PRIVATE OusterSDK::ouster_client OusterSDK::ouster_pcap) +else() + message(STATUS "No ouster_pcap library available; skipping examples") +endif() -add_executable(representations_example representations_example.cpp) -target_link_libraries(representations_example PRIVATE ouster_client ouster_build ouster_example_helpers) -add_executable(OusterSDK::ex ALIAS representations_example) +if(TARGET OusterSDK::ouster_viz) + add_executable(viz_example viz_example.cpp) + target_link_libraries(viz_example PRIVATE OusterSDK::ouster_client OusterSDK::ouster_viz) +else() + message(STATUS "No ouster_viz library available; skipping examples") +endif() diff --git a/ouster_client/src/example.cpp b/examples/client_example.cpp similarity index 75% rename from ouster_client/src/example.cpp rename to examples/client_example.cpp index 2ac0b44e..36320c14 100644 --- a/ouster_client/src/example.cpp +++ b/examples/client_example.cpp @@ -1,14 +1,15 @@ /** - * Copyright (c) 2018, Ouster, Inc. + * Copyright (c) 2022, Ouster, Inc. * All rights reserved. */ +#include #include #include #include #include -#include "ouster/build.h" +#include "ouster/impl/build.h" #include "ouster/client.h" #include "ouster/lidar_scan.h" #include "ouster/types.h" @@ -25,18 +26,17 @@ void FATAL(const char* msg) { int main(int argc, char* argv[]) { if (argc != 2 && argc != 3) { - std::cerr << "Version: " << ouster::CLIENT_VERSION_FULL << " (" - << ouster::BUILD_SYSTEM << ")" - << "\n\nUsage: ouster_client_example " - "[]" - "\n\n is optional: leave blank for " - "automatic destination detection" - << std::endl; - - return EXIT_FAILURE; + 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; + + return argc == 1 ? EXIT_SUCCESS : EXIT_FAILURE; } - std::cerr << "Ouster client example " << ouster::CLIENT_VERSION - << std::endl; + 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. @@ -72,11 +72,6 @@ int main(int argc, char* argv[]) { ouster::sensor::ColumnWindow column_window = info.format.column_window; - // azimuth_window config param reduce the amount of valid columns per scan - // that we will receive - int column_window_length = - (column_window.second - column_window.first + w) % w + 1; - std::cerr << " Firmware version: " << info.fw_rev << "\n Serial number: " << info.sn << "\n Product line: " << info.prod_line @@ -101,7 +96,7 @@ int main(int argc, char* argv[]) { std::cerr << "Capturing points... "; // buffer to store raw packet data - std::unique_ptr packet_buf(new uint8_t[UDP_BUF_SIZE]); + auto packet_buf = std::make_unique(UDP_BUF_SIZE); for (size_t i = 0; i < N_SCANS;) { // wait until sensor data is available @@ -113,20 +108,15 @@ int main(int argc, char* argv[]) { // 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, packet_buf.get(), pf)) + if (!sensor::read_lidar_packet(*handle, packet_buf.get(), pf)) { FATAL("Failed to read a packet of the expected size!"); + } // batcher will return "true" when the current scan is complete if (batch_to_scan(packet_buf.get(), scans[i])) { - // LidarScan provides access to azimuth block data and headers - auto n_invalid = std::count_if( - scans[i].headers.begin(), scans[i].headers.end(), - [](const LidarScan::BlockHeader& h) { - return !(h.status & 0x01); - }); // retry until we receive a full set of valid measurements // (accounting for azimuth_window settings if any) - if (n_invalid <= (int)w - column_window_length) i++; + if (scans[i].complete(info.format.column_window)) i++; } } @@ -139,14 +129,15 @@ int main(int argc, char* argv[]) { /* * The example code includes functions for efficiently and accurately - * computing point clouds from range measurements. LidarScan data can also - * be accessed directly using the Eigen[0] linear algebra library. + * computing point clouds from range measurements. LidarScan data can + * also be accessed directly using the Eigen[0] linear algebra library. * * [0] http://eigen.tuxfamily.org */ std::cerr << "Computing point clouds... " << std::endl; - // pre-compute a table for efficiently calculating point clouds from range + // pre-compute a table for efficiently calculating point clouds from + // range XYZLut lut = ouster::make_xyz_lut(info); std::vector clouds; @@ -155,10 +146,24 @@ int main(int argc, char* argv[]) { clouds.push_back(ouster::cartesian(scan, lut)); // channel fields can be queried as well - auto n_returns = (scan.field(sensor::RANGE) != 0).count(); - - std::cerr << " Frame no. " << scan.frame_id << " with " << n_returns - << " returns" << std::endl; + auto n_valid_first_returns = (scan.field(sensor::RANGE) != 0).count(); + + // LidarScan also provides access to header information such as + // status and timestamp + auto status = scan.status(); + auto it = std::find_if(status.data(), status.data() + status.size(), + [](const uint32_t s) { + return (s & 0x01); + }); // find first valid status + if (it != status.data() + status.size()) { + auto ts_ms = std::chrono::duration_cast( + std::chrono::nanoseconds(scan.timestamp()( + it - status.data()))); // get corresponding timestamp + + std::cerr << " Frame no. " << scan.frame_id << " with " + << n_valid_first_returns << " valid first returns at " + << ts_ms.count() << " ms" << std::endl; + } } /* diff --git a/examples/config_example.cpp b/examples/config_example.cpp index f61a4cc3..15718dd8 100644 --- a/examples/config_example.cpp +++ b/examples/config_example.cpp @@ -12,19 +12,19 @@ #include #include -#include "ouster/build.h" +#include "ouster/impl/build.h" #include "ouster/client.h" using namespace ouster; int main(int argc, char* argv[]) { if (argc != 2) { - std::cerr - << "Version: " << ouster::CLIENT_VERSION_FULL << " {" - << ouster::BUILD_SYSTEM << ")" - << "\n\nUsage: ouster_client_config_example " - << std::endl; - return EXIT_FAILURE; + std::cerr << "Version: " << ouster::SDK_VERSION_FULL << " (" + << ouster::BUILD_SYSTEM << ")" + << "\n\nUsage: config_example " + << std::endl; + + return argc == 1 ? EXIT_SUCCESS : EXIT_FAILURE; } const std::string sensor_hostname = argv[1]; diff --git a/examples/helpers.cpp b/examples/helpers.cpp index bfa99048..e7156922 100644 --- a/examples/helpers.cpp +++ b/examples/helpers.cpp @@ -5,6 +5,10 @@ #include "helpers.h" +#include "ouster/lidar_scan.h" +#include "ouster/os_pcap.h" +#include "ouster/types.h" + using namespace ouster::sensor; constexpr std::size_t BUF_SIZE = 65536; @@ -22,7 +26,7 @@ void get_complete_scan( ouster::ScanBatcher batch_to_scan(info.format.columns_per_frame, pf); // Buffer to store raw packet data - std::unique_ptr packet_buf(new uint8_t[BUF_SIZE]); + auto packet_buf = std::make_unique(BUF_SIZE); ouster::sensor_utils::packet_info packet_info; diff --git a/examples/lidar_scan_example.cpp b/examples/lidar_scan_example.cpp index 91a43874..2c921509 100644 --- a/examples/lidar_scan_example.cpp +++ b/examples/lidar_scan_example.cpp @@ -12,7 +12,7 @@ #include #include "helpers.h" -#include "ouster/build.h" +#include "ouster/impl/build.h" #include "ouster/client.h" #include "ouster/lidar_scan.h" #include "ouster/os_pcap.h" @@ -22,9 +22,12 @@ using namespace ouster::sensor; int main(int argc, char* argv[]) { if (argc != 3) { - std::cerr << "\n\nUsage: lidar_scan_example " + std::cerr << "Version: " << ouster::SDK_VERSION_FULL << " (" + << ouster::BUILD_SYSTEM << ")" + << "\n\nUsage: lidar_scan_example " << std::endl; - return EXIT_FAILURE; + + return argc == 1 ? EXIT_SUCCESS : EXIT_FAILURE; } const std::string pcap_file = argv[1]; @@ -36,7 +39,7 @@ int main(int argc, char* argv[]) { size_t w = info.format.columns_per_frame; size_t h = info.format.pixels_per_column; - // specifyiing only w and h for lidar scan creates one using the LEGACY udp + // Specifiying only w and h for lidar scan creates one using the LEGACY udp // profile //! [doc-stag-lidarscan-default-constructor] auto legacy_scan = ouster::LidarScan(w, h); @@ -59,34 +62,46 @@ int main(int argc, char* argv[]) { // Finally, you can construct by specifying fields directly static const std::array, 2> reduced_slots{{{ChanField::RANGE, ChanFieldType::UINT32}, - {ChanField::REFLECTIVITY, ChanFieldType::UINT8}}}; + {ChanField::NEAR_IR, ChanFieldType::UINT16}}}; auto reduced_fields_scan = ouster::LidarScan(w, h, reduced_slots.begin(), reduced_slots.end()); //! [doc-etag-lidarscan-reduced-slots] - std::cerr << "Creating scans from pcap..."; - get_complete_scan(handle, legacy_scan, info); + std::cerr << "Creating scans from pcap... "; + get_complete_scan(handle, profile_scan, info); - get_complete_scan(handle, dual_returns_scan, info); get_complete_scan(handle, reduced_fields_scan, info); - std::cerr << ".. scans created!" << std::endl; + + if (info.format.udp_profile_lidar != + UDPProfileLidar::PROFILE_RNG15_RFL8_NIR8) { + get_complete_scan(handle, legacy_scan, info); + } + + if (info.format.udp_profile_lidar == + UDPProfileLidar::PROFILE_RNG19_RFL8_SIG16_NIR16_DUAL) { + get_complete_scan(handle, dual_returns_scan, info); + } + std::cerr << "Scans created!" << std::endl; ouster::sensor_utils::replay_uninitialize(*handle); // Headers - auto frame_id = dual_returns_scan.frame_id; + auto frame_id = profile_scan.frame_id; //! [doc-stag-lidarscan-cpp-headers] - auto ts = dual_returns_scan.timestamp(); - auto status = dual_returns_scan.status(); - auto measurement_id = dual_returns_scan.measurement_id(); + auto ts = profile_scan.timestamp(); + auto status = profile_scan.status(); + auto measurement_id = profile_scan.measurement_id(); //! [doc-etag-lidarscan-cpp-headers] // to access a field: //! [doc-stag-lidarscan-cpp-fields] - auto range = dual_returns_scan.field(ChanField::RANGE); - auto range2 = dual_returns_scan.field(ChanField::RANGE); + auto range = profile_scan.field(ChanField::RANGE); //! [doc-etag-lidarscan-cpp-fields] + // On dual returns, second returns are often the same field name with 2 + // appended: + auto range2 = dual_returns_scan.field(ChanField::RANGE2); + std::cerr << "\nPrinting first element of received scan headers\n\tframe_id : " << frame_id << "\n\tts : " << ts(0) << "\n\tstatus : " << status(0) @@ -94,8 +109,14 @@ int main(int argc, char* argv[]) { std::cerr << "\nPrinting range of pixel at 15th row and 498th " "column...\n\trange(15, 498): " - << range(15, 498) << " " << range2(15, 498) << std::endl; + << range(15, 498) << std::endl; + if (info.format.udp_profile_lidar == + ouster::sensor::UDPProfileLidar::PROFILE_RNG19_RFL8_SIG16_NIR16_DUAL) { + std::cerr << "\nPrinting range of second return at 15th row and 498th " + "column...\n\trange(15, 498): " + << range2(15, 498) << std::endl; + } // Let's see what happens if you try to access a field that isn't in a // LidarScan std::cerr << "Accessing field that isn't available..."; @@ -123,7 +144,7 @@ int main(int argc, char* argv[]) { }; 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")); } - diff --git a/examples/representations_example.cpp b/examples/representations_example.cpp index 6078f55f..c0501f66 100644 --- a/examples/representations_example.cpp +++ b/examples/representations_example.cpp @@ -13,7 +13,7 @@ #include #include "helpers.h" -#include "ouster/build.h" +#include "ouster/impl/build.h" #include "ouster/client.h" #include "ouster/lidar_scan.h" #include "ouster/types.h" @@ -45,9 +45,11 @@ img_t get_x_in_image_form(const LidarScan& scan, bool destaggered, int main(int argc, char* argv[]) { if (argc != 3) { - std::cerr << "\n\nUsage: lidar_scan_example " + std::cerr << "Version: " << ouster::SDK_VERSION_FULL << " (" + << ouster::BUILD_SYSTEM << ")" + << "\n\nUsage: representation_example " << std::endl; - return EXIT_FAILURE; + return argc == 1 ? EXIT_SUCCESS : EXIT_FAILURE; } const std::string pcap_file = argv[1]; @@ -59,7 +61,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); + auto scan = LidarScan(w, h, info.format.udp_profile_lidar); std::cerr << "Reading in scan from pcap..." << std::endl; get_complete_scan(handle, scan, info); @@ -119,8 +121,21 @@ int main(int argc, char* argv[]) { << "\n3. Getting staggered and destaggered images of Reflectivity..." << std::endl; + Eigen::Array reflectivity; + + if (info.format.udp_profile_lidar == + sensor::UDPProfileLidar::PROFILE_LIDAR_LEGACY) { + reflectivity = scan.field(sensor::ChanField::REFLECTIVITY); + } else if (info.format.udp_profile_lidar == + sensor::UDPProfileLidar::PROFILE_RNG19_RFL8_SIG16_NIR16_DUAL) { + reflectivity = scan.field(sensor::ChanField::REFLECTIVITY) + .cast(); + } else { // legacy or single return profile + reflectivity = scan.field(sensor::ChanField::REFLECTIVITY) + .cast(); + } + //! [doc-stag-cpp-destagger] - auto reflectivity = scan.field(sensor::ChanField::REFLECTIVITY); auto reflectivity_destaggered = destagger(reflectivity, info.format.pixel_shift_by_row); //! [doc-etag-cpp-destagger] diff --git a/examples/viz_example.cpp b/examples/viz_example.cpp new file mode 100644 index 00000000..ef8333f6 --- /dev/null +++ b/examples/viz_example.cpp @@ -0,0 +1,63 @@ +/** + * Copyright (c) 2022, Ouster, Inc. + * All rights reserved. + * + * Minimal static point viz library example. + */ +#include +#include +#include +#include + +#include "ouster/impl/build.h" +#include "ouster/point_viz.h" + +using namespace ouster; + +int main(int argc, char*[]) { + if (argc != 1) { + std::cerr << "Version: " << ouster::SDK_VERSION_FULL << " (" + << ouster::BUILD_SYSTEM << ")" + << "\n\nUsage: viz_example" << std::endl; + + return EXIT_FAILURE; + } + + // std::random boilerplate + std::random_device rd; + std::default_random_engine re(rd()); + std::uniform_real_distribution dis(-20.0, 20.0); + std::uniform_real_distribution dis2(0.0, 1.0); + + // number of points to display + const size_t cloud_size = 1024; + + // populate random coordinates and color indices + std::vector points(3 * cloud_size); + std::generate(points.begin(), points.end(), [&]() { return dis(re); }); + + std::vector colors(cloud_size); + std::generate(colors.begin(), colors.end(), [&]() { return dis2(re); }); + + // initialize visualizer and add keyboard/mouse callbacks + ouster::viz::PointViz viz("Viz example"); + ouster::viz::add_default_controls(viz); + + // create a point cloud and register it with the visualizer + auto cloud = std::make_shared(cloud_size); + viz.add(cloud); + + // update visualizer cloud object + cloud->set_xyz(points.data()); + cloud->set_key(colors.data()); + + // send updates to be rendered. This method is thread-safe + viz.update(); + + // run rendering loop. Will return when the window is closed + std::cout << "Running rendering loop: press ESC to exit" << std::endl; + viz.run(); + std::cout << "Window closed, exiting" << std::endl; + + return EXIT_SUCCESS; +} diff --git a/ouster_client/CMakeLists.txt b/ouster_client/CMakeLists.txt index afe8efff..9cce683e 100644 --- a/ouster_client/CMakeLists.txt +++ b/ouster_client/CMakeLists.txt @@ -1,38 +1,30 @@ -cmake_minimum_required(VERSION 3.1.0) - -# ==== Project Name ==== -project(ouster_client VERSION 0.4.1) -set(ouster_client_VERSION_SUFFIX "") - # ==== Requirements ==== find_package(Eigen3 REQUIRED) find_package(jsoncpp REQUIRED) - -# ==== Build Metadata Target ==== -add_custom_target(generate_build_header) -add_custom_command(TARGET generate_build_header PRE_BUILD - COMMAND ${CMAKE_COMMAND} -DVERSION_GEN_OUT_DIR="${CMAKE_CURRENT_BINARY_DIR}/include/ouster" - -DVERSION_GEN_SOURCE_DIR="${CMAKE_CURRENT_SOURCE_DIR}/.." - -DBUILD_TYPE="${CMAKE_BUILD_TYPE}" - -DBUILD_SYSTEM="${CMAKE_SYSTEM_NAME}" - -Douster_client_VERSION="${ouster_client_VERSION}" - -Douster_client_VERSION_SUFFIX="${ouster_client_VERSION_SUFFIX}" - -P ${CMAKE_CURRENT_SOURCE_DIR}/../cmake/VersionGen.cmake) - -add_library(ouster_build INTERFACE) -target_include_directories(ouster_build INTERFACE - $) -add_dependencies(ouster_build generate_build_header) +find_package(CURL 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/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) target_link_libraries(ouster_client - PUBLIC Eigen3::Eigen $ - PRIVATE jsoncpp_lib) + PUBLIC + Eigen3::Eigen + $ + PRIVATE + CURL::libcurl + jsoncpp_lib) target_compile_definitions(ouster_client PRIVATE EIGEN_MPL2_ONLY) 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() @@ -43,13 +35,8 @@ target_include_directories(ouster_client SYSTEM PUBLIC $ $) -# ==== Executables ==== -add_executable(ouster_client_example src/example.cpp) -target_link_libraries(ouster_client_example PRIVATE ouster_client ouster_build) -add_executable(OusterSDK::ouster_client_example ALIAS ouster_client_example) - # ==== Install ==== -install(TARGETS ouster_client ouster_client_example +install(TARGETS ouster_client EXPORT ouster-sdk-targets LIBRARY DESTINATION lib ARCHIVE DESTINATION lib diff --git a/ouster_client/include/optional-lite/LICENSE.txt b/ouster_client/include/optional-lite/LICENSE.txt deleted file mode 100644 index 36b7cd93..00000000 --- a/ouster_client/include/optional-lite/LICENSE.txt +++ /dev/null @@ -1,23 +0,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. diff --git a/ouster_client/include/ouster/client.h b/ouster_client/include/ouster/client.h index 510fd26b..8bb5c844 100644 --- a/ouster_client/include/ouster/client.h +++ b/ouster_client/include/ouster/client.h @@ -89,7 +89,7 @@ client_state poll_client(const client& cli, int timeout_sec = 1); * Read lidar data from the sensor. Will not block. * * @param[in] cli client returned by init_client associated with the connection. - * @param[out] buf buffer to which to write lidar data. Must be at least.\ + * @param[out] buf buffer to which to write lidar data. Must be at least * lidar_packet_bytes + 1 bytes. * @param[in] pf The packet format. * diff --git a/ouster_client/include/ouster/lidar_scan.h b/ouster_client/include/ouster/lidar_scan.h index 8d007c3c..870b6b47 100644 --- a/ouster_client/include/ouster/lidar_scan.h +++ b/ouster_client/include/ouster/lidar_scan.h @@ -39,7 +39,11 @@ struct FieldSlot; */ class LidarScan { public: - [[deprecated]] static constexpr int N_FIELDS = 4; ///< @deprecated + [[deprecated]] static constexpr int N_FIELDS = + 4; ///< @deprecated Number of fields now varies by lidar profile or + ///< constructor provided arguments. Use N_FIELDS with caution even + ///< when working with legacy lidar profile data, and do not use for + ///< all non-legacy lidar profile formats. using raw_t [[deprecated]] = uint32_t; ///< @deprecated using ts_t [[deprecated]] = std::chrono::nanoseconds; ///< @deprecated @@ -64,7 +68,8 @@ class LidarScan { /** * Measurement block information, other than the channel data. * - * @deprecated + * @deprecated BlockHeaders are deprecated in favor of Header. See + * ``timestamp()``, ``measurement_id()``, and ``status()`` */ struct [[deprecated]] BlockHeader { ts_t timestamp; @@ -104,7 +109,7 @@ class LidarScan { /** * Vector containing the header definitions. * - * @deprecated + * @deprecated BlockHeader is deprecated in favor of Header * * @warning Members variables: use with caution, some of these will become * private. @@ -141,7 +146,7 @@ class LidarScan { * @param[in] w horizontal resoulution, i.e. the number of measurements per * scan. * @param[in] h vertical resolution, i.e. the number of channels. - * @param[in] profile udp profile for. + * @param[in] profile udp profile. */ LidarScan(size_t w, size_t h, sensor::UDPProfileLidar profile); @@ -188,7 +193,7 @@ class LidarScan { /** * Access timestamps as a vector. * - * @deprecated + * @deprecated See `timestamp()` instead * * @returns copy of the measurement timestamps as a vector. */ @@ -197,7 +202,8 @@ class LidarScan { /** * Access measurement block header fields. * - * @deprecated + * @deprecated Please see `status()`, `measurement_id()`, and `timestamp()` + * instead * * @return the header values for the specified measurement id. */ @@ -275,6 +281,13 @@ class LidarScan { /** @copydoc status() */ Eigen::Ref> status() const; + /** + * Assess completeness of scan. + * @param[in] window The column window to use for validity assessment + * @return whether all columns within given column window were valid + */ + bool complete(sensor::ColumnWindow window) const; + friend bool operator==(const LidarScan& a, const LidarScan& b); }; @@ -286,7 +299,7 @@ class LidarScan { /** * Equality for column headers. * - * @deprecated + * @deprecated BlockHeaders are deprecated * * @param[in] a The first column header to compare. * @param[in] b The second column header to compare. diff --git a/ouster_client/include/ouster/types.h b/ouster_client/include/ouster/types.h index 1bbe2a05..e4df9a66 100644 --- a/ouster_client/include/ouster/types.h +++ b/ouster_client/include/ouster/types.h @@ -62,7 +62,10 @@ enum lidar_mode { MODE_512x20, ///< lidar mode: 20 scans of 512 columns per second MODE_1024x10, ///< lidar mode: 10 scans of 1024 columns per second MODE_1024x20, ///< lidar mode: 20 scans of 1024 columsn per second - MODE_2048x10 ///< lidar mode: 10 scans of 2048 columns per second + MODE_2048x10, ///< lidar mode: 10 scans of 2048 columns per second + MODE_4096x5 ///< lidar mode: 5 scans of 4096 columns per second. Only + ///< available on select sensors + }; /** @@ -338,11 +341,11 @@ struct data_format { /** Stores necessary information from sensor to parse and project sensor data. */ struct sensor_info { - [[deprecated]] std::string - name; ///< @deprecated: will be removed in the next version - std::string sn; ///< sensor serial number - std::string fw_rev; ///< fw revision - lidar_mode mode; ///< lidar mode of sensor + [[deprecated("Will be removed in the next version")]] std::string + name; ///< @deprecated Will be removed in the next version + std::string sn; ///< sensor serial number + std::string fw_rev; ///< fw revision + lidar_mode mode; ///< lidar mode of sensor std::string prod_line; ///< prod line data_format format; ///< data format of sensor std::vector @@ -629,7 +632,8 @@ sensor_info parse_metadata(const std::string& metadata); sensor_info metadata_from_json(const std::string& json_file); /** - * Get a string representation of metadata. All fields included. + * Get a string representation of the sensor_info. All fields included. Not + * equivalent or interchangeable with metadata from sensor. * * @param[in] info sensor_info struct * @@ -677,26 +681,40 @@ std::string convert_to_legacy(const std::string& metadata); */ std::string client_version(); +// clang-format off /** Tag to identitify a paricular value reported in the sensor channel data * block. */ enum ChanField { - RANGE = 1, ///< 1st return range - RANGE2 = 2, ///< 2nd return range - INTENSITY = 3, ///< @deprecated (gcc 5.4 doesn't support annotations here) - SIGNAL = 3, ///< 1st return signal - SIGNAL2 = 4, ///< 2nd return signal - REFLECTIVITY = 5, ///< 1st return reflectivity - REFLECTIVITY2 = 6, ///< 2nd return reflectivity - AMBIENT = 7, //< @deprecated, use near_ir instead - NEAR_IR = 7, ///< near_ir + RANGE = 1, ///< 1st return range in mm + RANGE2 = 2, ///< 2nd return range in mm + INTENSITY = 3, ///< @deprecated Use SIGNAL instead + SIGNAL = 3, ///< 1st return signal in photons + SIGNAL2 = 4, ///< 2nd return signal in photons + REFLECTIVITY = 5, ///< 1st return reflectivity, calibrated by range and sensor + ///< sensitivity in FW 2.1+. See sensor docs for more details + REFLECTIVITY2 = 6, ///< 2nd return reflectivity, calibrated by range and sensor + ///< sensitivity in FW 2.1+. See sensor docs for more details + AMBIENT = 7, ///< @deprecated Use NEAR_IR instead + NEAR_IR = 7, ///< near_ir in photons FLAGS = 8, ///< 1st return flags FLAGS2 = 9, ///< 2nd return flags + CUSTOM0 = 50, ///< custom user field + CUSTOM1 = 51, ///< custom user field + CUSTOM2 = 52, ///< custom user field + CUSTOM3 = 53, ///< custom user field + CUSTOM4 = 54, ///< custom user field + CUSTOM5 = 55, ///< custom user field + CUSTOM6 = 56, ///< custom user field + CUSTOM7 = 57, ///< custom user field + CUSTOM8 = 58, ///< custom user field + CUSTOM9 = 59, ///< custom user field RAW32_WORD1 = 60, ///< raw word access to packet for dev use RAW32_WORD2 = 61, ///< raw word access to packet for dev use RAW32_WORD3 = 62, ///< raw word access to packet for dev use RAW32_WORD4 = 63, ///< raw word access to packet for dev use CHAN_FIELD_MAX = 64, ///< max which allows us to introduce future fields }; +// clang-format on /** * Get string representation of a channel field. @@ -726,9 +744,6 @@ enum ChanFieldType { VOID = 0, UINT8, UINT16, UINT32, UINT64 }; * direction. Use imu_av_{x,y,z} to read the angular velocity. */ class packet_format final { - packet_format( - const sensor_info& info); //< create packet_format from sensor_info - template T px_field(const uint8_t* px_buf, ChanField i) const; @@ -739,6 +754,9 @@ class packet_format final { field_types_; public: + packet_format( + const sensor_info& info); //< create packet_format from sensor_info + using FieldIter = decltype(field_types_)::const_iterator; ///< iterator over field types ///< of packet @@ -845,10 +863,13 @@ class packet_format final { */ uint32_t col_status(const uint8_t* col_buf) const; - [[deprecated]] uint32_t col_encoder( - const uint8_t* col_buf) const; ///< @deprecated - [[deprecated]] uint16_t col_frame_id( - const uint8_t* col_buf) const; ///< @deprecated + [[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 + [[deprecated("Use frame_id instead")]] uint16_t col_frame_id( + const uint8_t* col_buf) const; ///< @deprecated Use frame_id instead /** * Copy the specified channel field out of a packet measurement block. diff --git a/ouster_client/src/buffered_udp_source.cpp b/ouster_client/src/buffered_udp_source.cpp index 939986e9..16ac2af3 100644 --- a/ouster_client/src/buffered_udp_source.cpp +++ b/ouster_client/src/buffered_udp_source.cpp @@ -41,7 +41,7 @@ BufferedUDPSource::BufferedUDPSource(size_t buf_size) std::generate_n(std::back_inserter(bufs_), capacity_, [&] { return std::make_pair( client_state::CLIENT_ERROR, - std::unique_ptr{new uint8_t[packet_size]}); + std::make_unique(packet_size)); }); } diff --git a/ouster_client/src/client.cpp b/ouster_client/src/client.cpp index 1a85d070..82fb47de 100644 --- a/ouster_client/src/client.cpp +++ b/ouster_client/src/client.cpp @@ -1,5 +1,5 @@ /** - * Copyright (c) 2018, Ouster, Inc. + * Copyright(c) 2018, Ouster, Inc. * All rights reserved. */ @@ -8,6 +8,7 @@ #include #include +#include #include #include #include @@ -22,15 +23,17 @@ #include #include -#include "ouster/build.h" -#include "ouster/impl/netcompat.h" +#include "netcompat.h" #include "ouster/types.h" +#include "sensor_http.h" + +using namespace std::chrono_literals; +namespace chrono = std::chrono; +using ouster::sensor::util::SensorHttp; namespace ouster { namespace sensor { -namespace chrono = std::chrono; - struct client { SOCKET lidar_fd; SOCKET imu_fd; @@ -43,16 +46,13 @@ struct client { }; // defined in types.cpp -Json::Value to_json(const sensor_config& config, bool compat); +Json::Value to_json(const sensor_config& config); namespace { // default udp receive buffer size on windows is very low -- use 256K const int RCVBUF_SIZE = 256 * 1024; -// timeout for reading from a TCP socket during config -const int RCVTIMEOUT_SEC = 10; - int32_t get_sock_port(SOCKET sock_fd) { struct sockaddr_storage ss; socklen_t addrlen = sizeof ss; @@ -124,7 +124,7 @@ SOCKET udp_data_socket(int port) { << impl::socket_get_error() << std::endl; } - if (bind(sock_fd, ai->ai_addr, (socklen_t)ai->ai_addrlen)) { + if (::bind(sock_fd, ai->ai_addr, (socklen_t)ai->ai_addrlen)) { std::cerr << "udp bind(): " << impl::socket_get_error() << std::endl; impl::socket_close(sock_fd); @@ -157,293 +157,99 @@ SOCKET udp_data_socket(int port) { return SOCKET_ERROR; } -SOCKET cfg_socket(const char* addr) { - struct addrinfo hints, *info_start, *ai; - - memset(&hints, 0, sizeof hints); - hints.ai_family = AF_UNSPEC; - hints.ai_socktype = SOCK_STREAM; - - // try to parse as numeric address first: avoids spurious errors from DNS - // lookup when not using a hostname (and should be faster) - hints.ai_flags = AI_NUMERICHOST; - int ret = getaddrinfo(addr, "7501", &hints, &info_start); - if (ret != 0) { - hints.ai_flags = 0; - ret = getaddrinfo(addr, "7501", &hints, &info_start); - if (ret != 0) { - std::cerr << "cfg getaddrinfo(): " << gai_strerror(ret) - << std::endl; - return SOCKET_ERROR; - } - } - - if (info_start == NULL) { - std::cerr << "cfg getaddrinfo(): empty result" << std::endl; - return SOCKET_ERROR; - } - - SOCKET sock_fd; - for (ai = info_start; ai != NULL; ai = ai->ai_next) { - sock_fd = socket(ai->ai_family, ai->ai_socktype, ai->ai_protocol); - if (!impl::socket_valid(sock_fd)) { - std::cerr << "cfg socket(): " << impl::socket_get_error() - << std::endl; - continue; - } - - if (connect(sock_fd, ai->ai_addr, (socklen_t)ai->ai_addrlen) < 0) { - impl::socket_close(sock_fd); - continue; - } - - if (impl::socket_set_rcvtimeout(sock_fd, RCVTIMEOUT_SEC)) { - std::cerr << "cfg set_rcvtimeout(): " << impl::socket_get_error() - << std::endl; - impl::socket_close(sock_fd); - continue; - } - - break; - } - - freeaddrinfo(info_start); - if (ai == NULL) { - return SOCKET_ERROR; - } - - return sock_fd; -} - -bool do_tcp_cmd(SOCKET sock_fd, const std::vector& cmd_tokens, - std::string& res) { - const size_t max_res_len = 16 * 1024; - auto read_buf = std::unique_ptr{new char[max_res_len + 1]}; - - std::stringstream ss; - for (const auto& token : cmd_tokens) ss << token << " "; - ss << "\n"; - std::string cmd = ss.str(); - - ssize_t len = send(sock_fd, cmd.c_str(), cmd.length(), 0); - if (len != (ssize_t)cmd.length()) { - return false; - } - - // need to synchronize with server by reading response - std::stringstream read_ss; - do { - len = recv(sock_fd, read_buf.get(), max_res_len, 0); - if (len < 0) { - std::cerr << "do_tcp_cmd recv(): " << impl::socket_get_error() - << std::endl; - return false; - } - read_buf.get()[len] = '\0'; - read_ss << read_buf.get(); - } while (len > 0 && read_buf.get()[len - 1] != '\n'); - - res = read_ss.str(); - res.erase(res.find_last_not_of(" \r\n\t") + 1); - - return true; -} - -bool collect_metadata(client& cli, SOCKET sock_fd, chrono::seconds timeout) { - Json::CharReaderBuilder builder{}; - auto reader = std::unique_ptr{builder.newCharReader()}; - Json::Value root{}; - std::string errors{}; - - std::string res; - bool success = true; - +bool collect_metadata(client& cli, SensorHttp& sensor_http, + chrono::seconds timeout) { auto timeout_time = chrono::steady_clock::now() + timeout; - std::string status; - do { - success &= do_tcp_cmd(sock_fd, {"get_sensor_info"}, res); - success &= - reader->parse(res.c_str(), res.c_str() + res.size(), &root, NULL); + do { if (chrono::steady_clock::now() >= timeout_time) return false; - std::this_thread::sleep_for(chrono::seconds(1)); - status = root["status"].asString(); - } while (success && status == "INITIALIZING"); - cli.meta["sensor_info"] = root; + std::this_thread::sleep_for(1s); + status = sensor_http.sensor_info()["status"].asString(); + } while (status == "INITIALIZING"); // not all metadata available when sensor isn't RUNNING if (status != "RUNNING") { throw std::runtime_error( - "Cannot initialize with sensor status: " + status + - ". Please ensure operating mode is set to NORMAL"); + "Cannot obtain full metadata with sensor status: " + status + + ". Please ensure that sensor is not in a STANDBY, UNCONFIGURED, " + "WARMUP, or ERROR state"); } - success &= do_tcp_cmd(sock_fd, {"get_beam_intrinsics"}, res); - success &= - reader->parse(res.c_str(), res.c_str() + res.size(), &root, NULL); - cli.meta["beam_intrinsics"] = root; - - success &= do_tcp_cmd(sock_fd, {"get_imu_intrinsics"}, res); - success &= - reader->parse(res.c_str(), res.c_str() + res.size(), &root, NULL); - cli.meta["imu_intrinsics"] = root; - - success &= do_tcp_cmd(sock_fd, {"get_lidar_intrinsics"}, res); - success &= - reader->parse(res.c_str(), res.c_str() + res.size(), &root, NULL); - cli.meta["lidar_intrinsics"] = root; - - success &= do_tcp_cmd(sock_fd, {"get_lidar_data_format"}, res); - if (success) { - if (reader->parse(res.c_str(), res.c_str() + res.size(), &root, NULL)) { - cli.meta["lidar_data_format"] = root; - } else { - cli.meta["lidar_data_format"] = res; - } - } - - success &= do_tcp_cmd(sock_fd, {"get_calibration_status"}, res); - if (success) { - if (reader->parse(res.c_str(), res.c_str() + res.size(), &root, NULL)) { - cli.meta["calibration_status"] = root; - } else { - cli.meta["calibration_status"] = res; - } - } - - success &= do_tcp_cmd(sock_fd, {"get_config_param", "active"}, res); - success &= - reader->parse(res.c_str(), res.c_str() + res.size(), &root, NULL); - cli.meta["config_params"] = root; + cli.meta = sensor_http.metadata(); // merge extra info into metadata cli.meta["client_version"] = client_version(); - return success; + return true; } -bool set_config_helper(SOCKET sock_fd, const sensor_config& config, - uint8_t config_flags) { - std::string res{}; +} // namespace - // reset staged config to avoid spurious errors - std::string active_params; - if (!do_tcp_cmd(sock_fd, {"get_config_param", "active"}, active_params)) - throw std::runtime_error("Failed to run 'get_config_param'"); - if (!do_tcp_cmd(sock_fd, {"set_config_param", ".", active_params}, res)) - throw std::runtime_error("Failed to run 'set_config_param'"); - if (res != "set_config_param") - throw std::runtime_error("Error on 'set_config_param': " + res); +bool get_config(const std::string& hostname, sensor_config& config, + bool active) { + auto sensor_http = SensorHttp::create(hostname); + auto res = sensor_http->get_config_params(active); + config = parse_config(res); + return true; +} - // set automatic udp dest, if flag specified - if (config_flags & CONFIG_UDP_DEST_AUTO) { - if (config.udp_dest) - throw std::invalid_argument( - "UDP_DEST_AUTO flag set but provided config has udp_dest"); +bool set_config(const std::string& hostname, const sensor_config& config, + uint8_t config_flags) { + auto sensor_http = SensorHttp::create(hostname); - if (!do_tcp_cmd(sock_fd, {"set_udp_dest_auto"}, res)) - throw std::runtime_error("Failed to run 'set_udp_dest_auto'"); + // reset staged config to avoid spurious errors + auto active_params = sensor_http->get_config_params(true); - if (res != "set_udp_dest_auto") - throw std::runtime_error("Error on 'set_udp_dest_auto': " + res); - } + 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."); // set all desired config parameters - Json::Value config_json = to_json(config, true); + Json::Value config_json = to_json(config); for (const auto& key : config_json.getMemberNames()) { - auto value = Json::FastWriter().write(config_json[key]); - value.erase(std::remove(value.begin(), value.end(), '\n'), value.end()); + root[key] = config_json[key]; + } - if (!do_tcp_cmd(sock_fd, {"set_config_param", key, value}, res)) - throw std::runtime_error("Failed to run 'set_config_param'"); + active_params = Json::FastWriter().write(root); + sensor_http->set_config_param(".", active_params); - if (res != "set_config_param") - throw std::invalid_argument("Error on 'set_config_param': " + res); + // set automatic udp dest, if flag specified + if (config_flags & CONFIG_UDP_DEST_AUTO) { + if (config.udp_dest) + throw std::invalid_argument( + "UDP_DEST_AUTO flag set but provided config has udp_dest"); + sensor_http->set_udp_dest_auto(); } // reinitialize to make all staged parameters effective - if (!do_tcp_cmd(sock_fd, {"reinitialize"}, res)) - throw std::runtime_error("Failed to run 'reinitialize'"); - - // reinit will report an error only when staged configs are incompatible - if (res != "reinitialize") throw std::invalid_argument(res); - - // save if indicated, use deprecated write_config_txt to support 1.13 + sensor_http->reinitialize(); + // save if indicated if (config_flags & CONFIG_PERSIST) { - if (!do_tcp_cmd(sock_fd, {"write_config_txt"}, res)) - throw std::runtime_error("Failed to run 'write_config_txt'"); - - if (res != "write_config_txt") - throw std::runtime_error("Error on 'write_config_txt': " + res); - } - - return true; -} -} // namespace - -bool get_config(const std::string& hostname, sensor_config& config, - bool active) { - Json::CharReaderBuilder builder{}; - auto reader = std::unique_ptr{builder.newCharReader()}; - Json::Value root{}; - std::string errors{}; - - SOCKET sock_fd = cfg_socket(hostname.c_str()); - if (sock_fd < 0) return false; - - std::string res; - bool success = true; - - std::string active_or_staged = active ? "active" : "staged"; - success &= do_tcp_cmd(sock_fd, {"get_config_param", active_or_staged}, res); - success &= - reader->parse(res.c_str(), res.c_str() + res.size(), &root, NULL); - - config = parse_config(res); - - impl::socket_close(sock_fd); - - return success; -} - -bool set_config(const std::string& hostname, const sensor_config& config, - uint8_t config_flags) { - // open socket - SOCKET sock_fd = cfg_socket(hostname.c_str()); - if (sock_fd < 0) return false; - - try { - set_config_helper(sock_fd, config, config_flags); - } catch (...) { - impl::socket_close(sock_fd); - throw; + sensor_http->save_config_params(); } - impl::socket_close(sock_fd); return true; } std::string get_metadata(client& cli, int timeout_sec, bool legacy_format) { if (!cli.meta) { - SOCKET sock_fd = cfg_socket(cli.hostname.c_str()); - if (sock_fd < 0) return ""; - - bool success = - collect_metadata(cli, sock_fd, chrono::seconds{timeout_sec}); - - impl::socket_close(sock_fd); - - if (!success) return ""; + auto sensor_http = SensorHttp::create(cli.hostname); + if (!collect_metadata(cli, *sensor_http, chrono::seconds{timeout_sec})) + return ""; } Json::StreamWriterBuilder builder; builder["enableYAMLCompatibility"] = true; builder["precision"] = 6; builder["indentation"] = " "; - auto metadata_string = Json::writeString(builder, cli.meta); - return legacy_format ? convert_to_legacy(metadata_string) : metadata_string; } @@ -451,7 +257,6 @@ std::shared_ptr init_client(const std::string& hostname, int lidar_port, int 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); @@ -475,71 +280,48 @@ std::shared_ptr init_client(const std::string& hostname, if (!impl::socket_valid(lidar_port) || !impl::socket_valid(imu_port)) return std::shared_ptr(); - SOCKET sock_fd = cfg_socket(hostname.c_str()); - if (!impl::socket_valid(sock_fd)) return std::shared_ptr(); - - std::string res; bool success = true; - // fail fast if we can't reach the sensor via TCP - success &= do_tcp_cmd(sock_fd, {"get_sensor_info"}, res); - if (!success) { - impl::socket_close(sock_fd); - return std::shared_ptr(); - } - - // if dest address is not specified, have the sensor to set it automatically - if (udp_dest_host == "") { - success &= do_tcp_cmd(sock_fd, {"set_udp_dest_auto"}, res); - success &= res == "set_udp_dest_auto"; - } else { - success &= do_tcp_cmd( - sock_fd, {"set_config_param", "udp_ip", udp_dest_host}, res); - success &= res == "set_config_param"; - } - - success &= do_tcp_cmd( - sock_fd, - {"set_config_param", "udp_port_lidar", std::to_string(lidar_port)}, - res); - success &= res == "set_config_param"; - - success &= do_tcp_cmd( - sock_fd, {"set_config_param", "udp_port_imu", std::to_string(imu_port)}, - res); - success &= res == "set_config_param"; - - // if specified (not UNSPEC), set the lidar and timestamp modes - if (mode) { - success &= do_tcp_cmd( - sock_fd, {"set_config_param", "lidar_mode", to_string(mode)}, res); - success &= res == "set_config_param"; - } - - if (ts_mode) { - success &= do_tcp_cmd( - sock_fd, {"set_config_param", "timestamp_mode", to_string(ts_mode)}, - res); - success &= res == "set_config_param"; - } + try { + // fail fast if we can't reach the sensor via HTTP + auto sensor_http = SensorHttp::create(hostname); - // wake up from STANDBY, if necessary - success &= - do_tcp_cmd(sock_fd, {"set_config_param", "auto_start_flag", "1"}, res); - success &= res == "set_config_param"; + // 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); + } - // reinitialize to activate new settings - success &= do_tcp_cmd(sock_fd, {"reinitialize"}, res); - success &= res == "reinitialize"; + 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)); - // will block until no longer INITIALIZING - success &= collect_metadata(*cli, sock_fd, chrono::seconds{timeout_sec}); + // if specified (not UNSPEC), set the lidar and timestamp modes + if (mode) { + sensor_http->set_config_param("lidar_mode", std::to_string(mode)); + } - // check for sensor error states - auto status = cli->meta["sensor_info"]["status"].asString(); - success &= (status != "ERROR" && status != "UNCONFIGURED"); + if (ts_mode) { + sensor_http->set_config_param("timestamp_mode", + std::to_string(ts_mode)); + } - impl::socket_close(sock_fd); + // 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}); + // check for sensor error states + auto status = cli->meta["sensor_info"]["status"].asString(); + success &= (status != "ERROR" && status != "UNCONFIGURED"); + } catch (const std::runtime_error& e) { + // log error message + std::cerr << "init_client error: " << e.what() << std::endl; + return std::shared_ptr(); + } return success ? cli : std::shared_ptr(); } @@ -574,13 +356,15 @@ client_state poll_client(const client& c, const int timeout_sec) { } static bool recv_fixed(SOCKET fd, void* buf, int64_t len) { - int64_t n = recv(fd, (char*)buf, len + 1, 0); - if (n == len) { + int64_t bytes_read = recv(fd, (char*)buf, len + 1, 0); + + if (bytes_read == len) { return true; - } else if (n == -1) { + } else if (bytes_read == -1) { std::cerr << "recvfrom: " << impl::socket_get_error() << std::endl; } else { - std::cerr << "Unexpected udp packet length: " << n << std::endl; + std::cerr << "Unexpected udp packet length of: " << bytes_read + << " bytes. Expected: " << len << " bytes." << std::endl; } return false; } @@ -598,5 +382,23 @@ int get_lidar_port(client& cli) { return get_sock_port(cli.lidar_fd); } int get_imu_port(client& cli) { return get_sock_port(cli.imu_fd); } +/** + * Return the socket file descriptor used to listen for lidar UDP data. + * + * @param[in] cli client returned by init_client associated with the connection. + * + * @return the socket file descriptor. + */ +extern SOCKET get_lidar_socket_fd(client& cli) { return cli.lidar_fd; } + +/** + * Return the socket file descriptor used to listen for imu UDP data. + * + * @param[in] cli client returned by init_client associated with the connection. + * + * @return the socket file descriptor. + */ +extern SOCKET get_imu_socket_fd(client& cli) { return cli.imu_fd; } + } // namespace sensor } // namespace ouster diff --git a/ouster_client/src/curl_client.h b/ouster_client/src/curl_client.h new file mode 100644 index 00000000..eea30c97 --- /dev/null +++ b/ouster_client/src/curl_client.h @@ -0,0 +1,95 @@ +#include + +#include +#include + +#include "http_client.h" + +class CurlClient : public ouster::util::HttpClient { + public: + CurlClient(const std::string& base_url_) : HttpClient(base_url_) { + curl_global_init(CURL_GLOBAL_ALL); + curl_handle = curl_easy_init(); + curl_easy_setopt(curl_handle, CURLOPT_WRITEFUNCTION, + &CurlClient::write_memory_callback); + curl_easy_setopt(curl_handle, CURLOPT_WRITEDATA, this); + } + + virtual ~CurlClient() override { + curl_easy_cleanup(curl_handle); + curl_global_cleanup(); + } + + public: + std::string get(const std::string& url) const override { + auto full_url = url_combine(base_url, url); + return execute_get(full_url); + } + + std::string encode(const std::string& str) const override { + auto curl_str_deleter = [](char* s) { curl_free(s); }; + auto encoded_str = std::unique_ptr( + curl_easy_escape(curl_handle, str.c_str(), str.length()), + curl_str_deleter); + return std::string(encoded_str.get()); + } + + private: + static std::string url_combine(const std::string& url1, + const std::string& url2) { + if (!url1.empty() && !url2.empty()) { + if (url1[url1.length() - 1] == '/' && url2[0] == '/') { + return url1 + url2.substr(1); + } + if (url1[url1.length() - 1] != '/' && url2[0] != '/') { + return url1 + '/' + url2; + } + } + + return url1 + url2; + } + + std::string execute_get(const std::string& url) const { + curl_easy_setopt(curl_handle, CURLOPT_URL, url.c_str()); + curl_easy_setopt(curl_handle, CURLOPT_HTTPGET, 1L); + buffer.clear(); + auto res = curl_easy_perform(curl_handle); + if (res == CURLE_SEND_ERROR) { + // Specific versions of curl does't play well with the sensor http + // server. When CURLE_SEND_ERROR happens for the first time silently + // re-attempting the http request resolves the problem. + res = curl_easy_perform(curl_handle); + } + if (res != CURLE_OK) { + throw std::runtime_error( + "CurlClient::execute_get failed for the url: [" + url + + "] with the error message: " + curl_easy_strerror(res)); + } + long http_code = 0; + curl_easy_getinfo(curl_handle, CURLINFO_RESPONSE_CODE, &http_code); + if (http_code != 200) { + throw std::runtime_error( + std::string("CurlClient::execute_get failed for url: [" + url + + "] with the code: [") + + std::to_string(http_code) + std::string("] - and return: ") + + buffer); + } + return buffer; + } + + static size_t write_memory_callback(void* contents, size_t element_size, + size_t elements_count, + void* user_pointer) { + size_t size_increment = element_size * elements_count; + auto cc = static_cast(user_pointer); + auto size_current = cc->buffer.size(); + cc->buffer.resize(size_current + size_increment); + memcpy((void*)&cc->buffer[size_current], contents, size_increment); + size_current += size_increment; + return size_increment; + } + + private: + CURL* curl_handle; + mutable std::string buffer; +}; diff --git a/ouster_client/src/http_client.h b/ouster_client/src/http_client.h new file mode 100644 index 00000000..be6cb2ff --- /dev/null +++ b/ouster_client/src/http_client.h @@ -0,0 +1,56 @@ +/** + * Copyright (c) 2022, Ouster, Inc. + * All rights reserved. + * + * @file http_client.h + * @brief This file holds a minimal abstract interface for an http client. + * + * + */ + +#pragma once + +#include + +namespace ouster { +namespace util { + +/** + * An abstraction of http client to handle http requests + */ +class HttpClient { + public: + /** + * Constructs an HttpClient object to communicate with an http server. + * + * @param[in] base_url url to the http server. + */ + HttpClient(const std::string& base_url_) : base_url(base_url_) {} + + virtual ~HttpClient() {} + + /** + * Executes a GET request towards the provided url. + * + * @param[in] url http request url. + * + * @return the result of the execution. If request fails it returns an empty + * string. + */ + virtual std::string get(const std::string& url) const = 0; + + /** + * Encodes the given string as a url. + * + * @param[in] str the string to be encoded as a url. + * + * @return Returns the string as encoded url. + */ + virtual std::string encode(const std::string& str) const = 0; + + protected: + std::string base_url; +}; + +} // namespace util +} // namespace ouster \ No newline at end of file diff --git a/ouster_client/src/image_processing.cpp b/ouster_client/src/image_processing.cpp index 956f2fbb..7707aefe 100644 --- a/ouster_client/src/image_processing.cpp +++ b/ouster_client/src/image_processing.cpp @@ -82,12 +82,14 @@ void AutoExposure::update(Eigen::Ref> image, bool update_state) { return key_eigen(a) < key_eigen(b); }; - const size_t lo_kth_extreme = indices.size() * lo_percentile; + const size_t lo_kth_extreme = + static_cast(indices.size() * lo_percentile); std::nth_element(indices.begin(), indices.begin() + lo_kth_extreme, indices.end(), cmp); lo = key_eigen[*(indices.begin() + lo_kth_extreme)]; - const size_t hi_kth_extreme = indices.size() * hi_percentile; + const size_t hi_kth_extreme = + static_cast(indices.size() * hi_percentile); std::nth_element(indices.begin() + lo_kth_extreme, indices.end() - hi_kth_extreme - 1, indices.end(), cmp); @@ -208,7 +210,7 @@ static Eigen::Array compute_dark_count( Eigen::Matrix A(image_h, 2); for (size_t i = 0; i < image_h; i++) { A(i, 0) = 1; - A(i, 1) = i; + A(i, 1) = static_cast(i); } Eigen::Matrix x = A.fullPivLu().solve(new_dark_count.matrix()); new_dark_count -= (A * x).array(); diff --git a/ouster_client/src/lidar_scan.cpp b/ouster_client/src/lidar_scan.cpp index bbf71517..075afaf1 100644 --- a/ouster_client/src/lidar_scan.cpp +++ b/ouster_client/src/lidar_scan.cpp @@ -82,11 +82,9 @@ Table default_scan_fields{ static std::vector> lookup_scan_fields( UDPProfileLidar profile) { auto end = impl::default_scan_fields.end(); - auto it = std::find_if( - impl::default_scan_fields.begin(), end, - [&](const std::pair& kv) { - return kv.first == profile; - }); + auto it = + std::find_if(impl::default_scan_fields.begin(), end, + [profile](const auto& kv) { return kv.first == profile; }); if (it == end || it->first == 0) throw std::invalid_argument("Unknown lidar udp profile"); @@ -188,6 +186,25 @@ Eigen::Ref> LidarScan::status() const { return status_; } +bool LidarScan::complete(sensor::ColumnWindow window) const { + const auto& status = this->status(); + auto start = window.first; + auto end = window.second; + + if (start <= end) { + return status.segment(start, end - start + 1) + .unaryExpr([](uint32_t s) { return s & 0x01; }) + .isConstant(0x01); + } else { + return status.segment(0, end) + .unaryExpr([](uint32_t s) { return s & 0x01; }) + .isConstant(0x01) && + status.segment(start, this->w - start) + .unaryExpr([](uint32_t s) { return s & 0x01; }) + .isConstant(0x01); + } +} + bool operator==(const LidarScan::BlockHeader& a, const LidarScan::BlockHeader& b) { return a.timestamp == b.timestamp && a.encoder == b.encoder && @@ -266,7 +283,7 @@ LidarScan::Points cartesian(const Eigen::Ref>& range, if (range.cols() * range.rows() != lut.direction.rows()) throw std::invalid_argument("unexpected image dimensions"); - auto reshaped = Eigen::Map>( + auto reshaped = Eigen::Map>( range.data(), range.cols() * range.rows()); auto nooffset = lut.direction.colwise() * reshaped.cast(); return (nooffset.array() == 0.0).select(nooffset, nooffset + lut.offset); @@ -315,6 +332,7 @@ struct parse_field_col { template void operator()(Eigen::Ref> field, ChanField f, uint16_t m_id, const sensor::packet_format& pf, const uint8_t* col_buf) { + if (f >= ChanField::CUSTOM0 && f <= ChanField::CUSTOM9) return; pf.col_field(col_buf, f, field.col(m_id).data(), field.cols()); } }; diff --git a/ouster_client/src/netcompat.cpp b/ouster_client/src/netcompat.cpp index 4c43430f..3c2d4731 100644 --- a/ouster_client/src/netcompat.cpp +++ b/ouster_client/src/netcompat.cpp @@ -3,7 +3,7 @@ * All rights reserved. */ -#include "ouster/impl/netcompat.h" +#include "netcompat.h" #include diff --git a/ouster_client/include/ouster/impl/netcompat.h b/ouster_client/src/netcompat.h similarity index 100% rename from ouster_client/include/ouster/impl/netcompat.h rename to ouster_client/src/netcompat.h diff --git a/ouster_client/src/parsing.cpp b/ouster_client/src/parsing.cpp index 32b657bf..471d3c5c 100644 --- a/ouster_client/src/parsing.cpp +++ b/ouster_client/src/parsing.cpp @@ -66,8 +66,9 @@ static const Table legacy_field_info{{ {ChanField::RAW32_WORD3, {UINT32, 8, 0, 0}}, }}; -static const Table lb_field_info{{ +static const Table lb_field_info{{ {ChanField::RANGE, {UINT16, 0, 0x7fff, -3}}, + {ChanField::FLAGS, {UINT8, 1, 0b10000000, 7}}, {ChanField::REFLECTIVITY, {UINT8, 2, 0, 0}}, {ChanField::NEAR_IR, {UINT8, 3, 0, -4}}, {ChanField::RAW32_WORD1, {UINT32, 0, 0, 0}}, @@ -115,9 +116,7 @@ static const ProfileEntry& lookup_profile_entry(UDPProfileLidar profile) { auto end = profiles.end(); auto it = std::find_if(impl::profiles.begin(), end, - [&](const std::pair& kv) { - return kv.first == profile; - }); + [profile](const auto& kv) { return kv.first == profile; }); if (it == end || it->first == 0) throw std::invalid_argument("Unknown lidar udp profile"); @@ -309,7 +308,11 @@ 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; std::memcpy(&res, col_buf + impl_->status_offset, sizeof(uint32_t)); - return res; + 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 + } } uint64_t packet_format::col_timestamp(const uint8_t* col_buf) const { @@ -447,7 +450,7 @@ const packet_format& get_format(const sensor_info& info) { std::lock_guard lk{cache_mx}; if (!cache.count(k)) { - cache[k] = std::unique_ptr(new packet_format{info}); + cache[k] = std::make_unique(info); } return *cache.at(k); diff --git a/ouster_client/src/sensor_http.cpp b/ouster_client/src/sensor_http.cpp new file mode 100644 index 00000000..45f9cadb --- /dev/null +++ b/ouster_client/src/sensor_http.cpp @@ -0,0 +1,62 @@ +#include "sensor_http.h" + +#include + +#include "curl_client.h" +#include "sensor_http_imp.h" +#include "sensor_tcp_imp.h" + +using std::stoul; +using std::string; + +using namespace ouster::util; +using namespace ouster::sensor; +using namespace ouster::sensor::util; +using namespace ouster::sensor::impl; + +string SensorHttp::firmware_version_string(const string& hostname) { + auto http_client = std::make_unique("http://" + hostname); + return http_client->get("api/v1/system/firmware"); +} + +version SensorHttp::firmware_version(const string& hostname) { + auto result = firmware_version_string(hostname); + auto rgx = std::regex(R"(v(\d+).(\d+)\.(\d+))"); + std::smatch matches; + std::regex_search(result, matches, rgx); + + if (matches.size() < 4) return invalid_version; + + try { + return version{static_cast(stoul(matches[1])), + static_cast(stoul(matches[2])), + static_cast(stoul(matches[3]))}; + } catch (const std::exception&) { + return invalid_version; + } +} + +std::unique_ptr SensorHttp::create(const string& hostname) { + auto fw = firmware_version(hostname); + + if (fw == invalid_version || fw.major < 2) { + throw std::runtime_error( + "SensorHttp:: create firmware version information unavailable or " + "not fully supported version. Please upgrade your sensor to FW " + "2.0 or later."); + } + + if (fw.major == 2) { + switch (fw.minor) { + 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: + return std::make_unique(hostname); + } + } + + return std::make_unique(hostname); +} diff --git a/ouster_client/src/sensor_http.h b/ouster_client/src/sensor_http.h new file mode 100644 index 00000000..a1fbd1e8 --- /dev/null +++ b/ouster_client/src/sensor_http.h @@ -0,0 +1,136 @@ +/** + * Copyright (c) 2022, Ouster, Inc. + * All rights reserved. + * + * @file sensor_http.h + * @brief A high level HTTP interface for Ouster sensors. + * + */ + +#pragma once + +#include +#include + +#include + +namespace ouster { +namespace sensor { +namespace util { + +/** + * An interface to communicate with Ouster sensors via http requests + */ +class SensorHttp { + protected: + /** + * Constructs an http interface to communicate with the sensor. + * + * @param[in] hostname hostname of the sensor to communicate with. + */ + SensorHttp() = default; + + public: + /** + * Deconstruct the sensor http interface. + */ + virtual ~SensorHttp() = default; + + /** + * Queries the sensor metadata. + * + * @return returns a Json object of the sensor metadata. + */ + virtual Json::Value metadata() const = 0; + + /** + * Queries the sensor_info. + * + * @return returns a Json object representing the sensor_info. + */ + virtual Json::Value sensor_info() const = 0; + + /** + * Queries active/staged configuration on the sensor + * + * @param[in] active if true retrieve active, otherwise get staged configs. + * + * @return a string represnting the active or staged config + */ + virtual std::string get_config_params(bool active) const = 0; + + /** + * Set the value of a specfic configuration on the sensor, the changed + * configuration is not active until the sensor is restarted. + * + * @param[in] key name of the config to change. + * @param[in] value the new value to set for the selected configuration. + */ + virtual void set_config_param(const std::string& key, + const std::string& value) const = 0; + + /** + * Enables automatic assignment of udp destination ports. + */ + virtual void set_udp_dest_auto() const = 0; + + /** + * Retrieves beam intrinsics of the sensor. + */ + virtual Json::Value beam_intrinsics() const = 0; + + /** + * Retrieves imu intrinsics of the sensor. + */ + virtual Json::Value imu_intrinsics() const = 0; + + /** + * Retrieves lidar intrinsics of the sensor. + */ + virtual Json::Value lidar_intrinsics() const = 0; + + /** + * Retrieves lidar data format. + */ + virtual Json::Value lidar_data_format() const = 0; + + /** + * Gets the calibaration status of the sensor. + */ + virtual Json::Value calibration_status() const = 0; + + /** + * Restarts the sensor applying all staged configurations. + */ + virtual void reinitialize() const = 0; + + /** + * Persist active configuration parameters to the sensor. + */ + virtual void save_config_params() const = 0; + + /** + * Retrieves sensor firmware version information as a string. + * + * @param[in] hostname hostname of the sensor to communicate with. + */ + static std::string firmware_version_string(const std::string& hostname); + + /** + * Retrieves sensor firmware version information. + * + * @param[in] hostname hostname of the sensor to communicate with. + */ + static ouster::util::version firmware_version(const std::string& hostname); + + /** + * Creates an instance of the SensorHttp interface. + * + * @param[in] hostname hostname of the sensor to communicate with. + */ + static std::unique_ptr create(const std::string& hostname); +}; + +} // namespace util +} // namespace sensor +} // namespace ouster \ No newline at end of file diff --git a/ouster_client/src/sensor_http_imp.cpp b/ouster_client/src/sensor_http_imp.cpp new file mode 100644 index 00000000..039156c9 --- /dev/null +++ b/ouster_client/src/sensor_http_imp.cpp @@ -0,0 +1,143 @@ +#include "sensor_http_imp.h" + +#include "curl_client.h" + +using std::string; +using namespace ouster::sensor::impl; + +SensorHttpImp::SensorHttpImp(const string& hostname) + : http_client(std::make_unique("http://" + hostname)) {} + +SensorHttpImp::~SensorHttpImp() = default; + +Json::Value SensorHttpImp::metadata() const { + return get_json("api/v1/sensor/metadata"); +} + +Json::Value SensorHttpImp::sensor_info() const { + return get_json("api/v1/sensor/metadata/sensor_info"); +} + +string SensorHttpImp::get_config_params(bool active) const { + auto config_type = active ? "active" : "staged"; + return get(string("api/v1/sensor/cmd/get_config_param?args=") + + config_type); +} + +void SensorHttpImp::set_config_param(const string& key, + const string& value) const { + auto encoded_value = http_client->encode(value); // encode config params + auto url = + "api/v1/sensor/cmd/set_config_param?args=" + key + "+" + encoded_value; + execute(url, "\"set_config_param\""); +} + +void SensorHttpImp::set_udp_dest_auto() const { + execute("api/v1/sensor/cmd/set_udp_dest_auto", "{}"); +} + +Json::Value SensorHttpImp::beam_intrinsics() const { + return get_json("api/v1/sensor/metadata/beam_intrinsics"); +} + +Json::Value SensorHttpImp::imu_intrinsics() const { + return get_json("api/v1/sensor/metadata/imu_intrinsics"); +} + +Json::Value SensorHttpImp::lidar_intrinsics() const { + return get_json("api/v1/sensor/metadata/lidar_intrinsics"); +} + +Json::Value SensorHttpImp::lidar_data_format() const { + return get_json("api/v1/sensor/metadata/lidar_data_format"); +} + +Json::Value SensorHttpImp::calibration_status() const { + return get_json("api/v1/sensor/metadata/calibration_status"); +} + +// reinitialize to activate new settings +void SensorHttpImp::reinitialize() const { + execute("api/v1/sensor/cmd/reinitialize", "{}"); +} + +void SensorHttpImp::save_config_params() const { + execute("api/v1/sensor/cmd/save_config_params", "{}"); +} + +string SensorHttpImp::get(const string& url) const { + return http_client->get(url); +} + +Json::Value SensorHttpImp::get_json(const string& url) const { + Json::CharReaderBuilder builder; + auto reader = std::unique_ptr{builder.newCharReader()}; + Json::Value root; + auto result = get(url); + if (!reader->parse(result.c_str(), result.c_str() + result.size(), &root, + nullptr)) + throw std::runtime_error("SensorHttpImp::get_json failed! url: " + url); + return root; +} + +void SensorHttpImp::execute(const string& url, const string& validation) const { + auto result = get(url); + if (result != validation) + throw std::runtime_error("SensorHttpImp::execute failed! url: " + url + + " returned [" + result + "], expected [" + + validation + "]"); +} + +SensorHttpImp_2_2::SensorHttpImp_2_2(const string& hostname) + : SensorHttpImp(hostname) {} + +void SensorHttpImp_2_2::set_udp_dest_auto() const { + return execute("api/v1/sensor/cmd/set_udp_dest_auto", + "\"set_config_param\""); +} + +SensorHttpImp_2_1::SensorHttpImp_2_1(const string& hostname) + : SensorHttpImp_2_2(hostname) {} + +Json::Value SensorHttpImp_2_1::metadata() const { + Json::Value root; + root["sensor_info"] = sensor_info(); + root["beam_intrinsics"] = beam_intrinsics(); + root["imu_intrinsics"] = imu_intrinsics(); + root["lidar_intrinsics"] = lidar_intrinsics(); + root["lidar_data_format"] = lidar_data_format(); + root["calibration_status"] = calibration_status(); + + Json::CharReaderBuilder builder; + auto reader = std::unique_ptr{builder.newCharReader()}; + Json::Value node; + auto res = get_config_params(true); + auto parse_success = + reader->parse(res.c_str(), res.c_str() + res.size(), &node, nullptr); + root["config_params"] = parse_success ? node : res; + return root; +} + +Json::Value SensorHttpImp_2_1::sensor_info() const { + return get_json("api/v1/sensor/cmd/get_sensor_info"); +} + +Json::Value SensorHttpImp_2_1::beam_intrinsics() const { + return get_json("api/v1/sensor/cmd/get_beam_intrinsics"); +} + +Json::Value SensorHttpImp_2_1::imu_intrinsics() const { + return get_json("api/v1/sensor/cmd/get_imu_intrinsics"); +} + +Json::Value SensorHttpImp_2_1::lidar_intrinsics() const { + return get_json("api/v1/sensor/cmd/get_lidar_intrinsics"); +} + +Json::Value SensorHttpImp_2_1::lidar_data_format() const { + return get_json("api/v1/sensor/cmd/get_lidar_data_format"); +} + +Json::Value SensorHttpImp_2_1::calibration_status() const { + return get_json("api/v1/sensor/cmd/get_calibration_status"); +} diff --git a/ouster_client/src/sensor_http_imp.h b/ouster_client/src/sensor_http_imp.h new file mode 100644 index 00000000..489bb508 --- /dev/null +++ b/ouster_client/src/sensor_http_imp.h @@ -0,0 +1,182 @@ +/** + * Copyright (c) 2022, Ouster, Inc. + * All rights reserved. + * + * @file sensor_http_imp.h + * @brief An implementation of the HTTP interface for Ouster sensors. + * + */ + +#pragma once + +#include "http_client.h" +#include "sensor_http.h" + +namespace ouster { +namespace sensor { +namespace impl { + +/** + * An implementation of the sensor http interface + */ +class SensorHttpImp : public util::SensorHttp { + public: + /** + * Constructs an http interface to communicate with the sensor. + * + * @param[in] hostname hostname of the sensor to communicate with. + */ + SensorHttpImp(const std::string& hostname); + + /** + * Deconstruct the sensor http interface. + */ + ~SensorHttpImp() override; + + /** + * Queries the sensor metadata. + * + * @return returns a Json object of the sensor metadata. + */ + Json::Value metadata() const override; + + /** + * Queries the sensor_info. + * + * @return returns a Json object representing the sensor_info. + */ + Json::Value sensor_info() const override; + + /** + * Queries active/staged configuration on the sensor + * + * @param[in] active if true retrieve active, otherwise get staged configs. + * + * @return a string represnting the active or staged config + */ + std::string get_config_params(bool active) const override; + + /** + * Set the value of a specfic configuration on the sensor, the changed + * configuration is not active until the sensor is restarted. + * + * @param[in] key name of the config to change. + * @param[in] value the new value to set for the selected configuration. + */ + void set_config_param(const std::string& key, + const std::string& value) const override; + + /** + * Enables automatic assignment of udp destination ports. + */ + void set_udp_dest_auto() const override; + + /** + * Retrieves beam intrinsics of the sensor. + */ + Json::Value beam_intrinsics() const override; + + /** + * Retrieves imu intrinsics of the sensor. + */ + Json::Value imu_intrinsics() const override; + + /** + * Retrieves lidar intrinsics of the sensor. + */ + Json::Value lidar_intrinsics() const override; + + /** + * Retrieves lidar data format. + */ + Json::Value lidar_data_format() const override; + + /** + * Gets the calibaration status of the sensor. + */ + Json::Value calibration_status() const override; + + /** + * Restarts the sensor applying all staged configurations. + */ + void reinitialize() const override; + + /** + * Persist active configuration parameters to the sensor. + */ + void save_config_params() const override; + + protected: + std::string get(const std::string& url) const; + + Json::Value get_json(const std::string& url) const; + + void execute(const std::string& url, const std::string& validation) const; + + protected: + std::unique_ptr http_client; +}; + +// TODO: remove when firmware 2.2 has been fully phased out +class SensorHttpImp_2_2 : public SensorHttpImp { + public: + SensorHttpImp_2_2(const std::string& hostname); + + void set_udp_dest_auto() const override; +}; + +/** + * An implementation of the sensor http interface + */ +class SensorHttpImp_2_1 : public SensorHttpImp_2_2 { + public: + /** + * Constructs an http interface to communicate with the sensor. + * + * @param[in] hostname hostname of the sensor to communicate with. + */ + SensorHttpImp_2_1(const std::string& hostname); + + /** + * Queries the sensor metadata. + * + * @return returns a Json object of the sensor metadata. + */ + Json::Value metadata() const override; + + /** + * Queries the sensor_info. + * + * @return returns a Json object representing the sensor_info. + */ + Json::Value sensor_info() const override; + + /** + * Retrieves beam intrinsics of the sensor. + */ + Json::Value beam_intrinsics() const override; + + /** + * Retrieves imu intrinsics of the sensor. + */ + Json::Value imu_intrinsics() const override; + + /** + * Retrieves lidar intrinsics of the sensor. + */ + Json::Value lidar_intrinsics() const override; + + /** + * Retrieves lidar data format. + */ + Json::Value lidar_data_format() const override; + + /** + * Gets the calibaration status of the sensor. + */ + Json::Value calibration_status() const override; +}; + +} // namespace impl +} // namespace sensor +} // namespace ouster diff --git a/ouster_client/src/sensor_tcp_imp.cpp b/ouster_client/src/sensor_tcp_imp.cpp new file mode 100644 index 00000000..aad54cda --- /dev/null +++ b/ouster_client/src/sensor_tcp_imp.cpp @@ -0,0 +1,208 @@ +/** + * Copyright (c) 2022, Ouster, Inc. + * All rights reserved. + */ + +#include "sensor_tcp_imp.h" + +#include +#include +#include +#include + +using std::string; +using namespace ouster::sensor::impl; + +SensorTcpImp::SensorTcpImp(const string& hostname) + : socket_handle(cfg_socket(hostname.c_str())), + read_buf(std::unique_ptr{new char[MAX_RESULT_LENGTH + 1]}) {} + +SensorTcpImp::~SensorTcpImp() { socket_close(socket_handle); } + +Json::Value SensorTcpImp::metadata() const { + Json::Value root; + root["sensor_info"] = sensor_info(); + root["beam_intrinsics"] = beam_intrinsics(); + root["imu_intrinsics"] = imu_intrinsics(); + root["lidar_intrinsics"] = lidar_intrinsics(); + root["lidar_data_format"] = lidar_data_format(); + root["calibration_status"] = calibration_status(); + Json::CharReaderBuilder builder; + auto reader = std::unique_ptr{builder.newCharReader()}; + auto res = get_config_params(true); + Json::Value node; + auto parse_success = + reader->parse(res.c_str(), res.c_str() + res.size(), &node, nullptr); + root["config_params"] = parse_success ? node : res; + return root; +} + +Json::Value SensorTcpImp::sensor_info() const { + return tcp_cmd_json({"get_sensor_info"}); +} + +string SensorTcpImp::get_config_params(bool active) const { + auto config_type = active ? "active" : "staged"; + return tcp_cmd({"get_config_param", config_type}); +} + +namespace { +std::string rtrim(const std::string& s) { + return s.substr( + 0, + std::distance(s.begin(), + std::find_if(s.rbegin(), s.rend(), [](unsigned char ch) { + return !isspace(ch); + }).base())); +} +} // namespace + +void SensorTcpImp::set_config_param(const string& key, + const string& value) const { + tcp_cmd_with_validation({"set_config_param", key, rtrim(value)}, + "set_config_param"); +} + +void SensorTcpImp::set_udp_dest_auto() const { + tcp_cmd_with_validation({"set_udp_dest_auto"}, "set_udp_dest_auto"); +} + +Json::Value SensorTcpImp::beam_intrinsics() const { + return tcp_cmd_json({"get_beam_intrinsics"}); +} + +Json::Value SensorTcpImp::imu_intrinsics() const { + return tcp_cmd_json({"get_imu_intrinsics"}); +} + +Json::Value SensorTcpImp::lidar_intrinsics() const { + return tcp_cmd_json({"get_lidar_intrinsics"}); +} + +Json::Value SensorTcpImp::lidar_data_format() const { + return tcp_cmd_json({"get_lidar_data_format"}, false); +} + +Json::Value SensorTcpImp::calibration_status() const { + return tcp_cmd_json({"get_calibration_status"}, false); +} + +void SensorTcpImp::reinitialize() const { + // reinitialize to make all staged parameters effective + tcp_cmd_with_validation({"reinitialize"}, "reinitialize"); +} + +void SensorTcpImp::save_config_params() const { + tcp_cmd_with_validation({"write_config_txt"}, "write_config_txt"); +} + +SOCKET SensorTcpImp::cfg_socket(const char* addr) { + struct addrinfo hints, *info_start, *ai; + + std::memset(&hints, 0, sizeof hints); + hints.ai_family = AF_UNSPEC; + hints.ai_socktype = SOCK_STREAM; + + // try to parse as numeric address first: avoids spurious errors from + // DNS lookup when not using a hostname (and should be faster) + hints.ai_flags = AI_NUMERICHOST; + int ret = getaddrinfo(addr, "7501", &hints, &info_start); + if (ret != 0) { + hints.ai_flags = 0; + ret = getaddrinfo(addr, "7501", &hints, &info_start); + if (ret != 0) { + std::cerr << "cfg getaddrinfo(): " << gai_strerror(ret) + << std::endl; + return SOCKET_ERROR; + } + } + + if (info_start == nullptr) { + std::cerr << "cfg getaddrinfo(): empty result" << std::endl; + return SOCKET_ERROR; + } + + SOCKET sock_fd; + 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; + continue; + } + + if (connect(sock_fd, ai->ai_addr, (socklen_t)ai->ai_addrlen) < 0) { + socket_close(sock_fd); + continue; + } + + if (socket_set_rcvtimeout(sock_fd, RCVTIMEOUT_SEC)) { + std::cerr << "cfg set_rcvtimeout(): " << socket_get_error() + << std::endl; + socket_close(sock_fd); + continue; + } + + break; + } + + freeaddrinfo(info_start); + if (ai == nullptr) { + return SOCKET_ERROR; + } + + return sock_fd; +} + +string SensorTcpImp::tcp_cmd(const std::vector& cmd_tokens) const { + std::stringstream ss; + for (const auto& token : cmd_tokens) ss << token << " "; + ss << "\n"; + string cmd = ss.str(); + + ssize_t len = send(socket_handle, cmd.c_str(), cmd.length(), 0); + if (len != (ssize_t)cmd.length()) { + throw std::runtime_error("tcp_cmd socket::send failed"); + } + + // need to synchronize with server by reading response + std::stringstream read_ss; + do { + len = recv(socket_handle, read_buf.get(), MAX_RESULT_LENGTH, 0); + if (len < 0) { + throw std::runtime_error("tcp_cmd recv(): " + socket_get_error()); + } + + read_buf.get()[len] = '\0'; + read_ss << read_buf.get(); + } while (len > 0 && read_buf.get()[len - 1] != '\n'); + + auto res = read_ss.str(); + res.erase(res.find_last_not_of(" \r\n\t") + 1); + return res; +} + +Json::Value SensorTcpImp::tcp_cmd_json(const std::vector& cmd_tokens, + bool exception_on_parse_errors) const { + Json::CharReaderBuilder builder; + auto reader = std::unique_ptr{builder.newCharReader()}; + Json::Value root; + auto result = tcp_cmd(cmd_tokens); + auto success = reader->parse(result.c_str(), result.c_str() + result.size(), + &root, nullptr); + if (success) return root; + if (!exception_on_parse_errors) return result; + + throw std::runtime_error( + "SensorTcp::tcp_cmd_json failed for " + cmd_tokens[0] + + " command. returned json string [" + result + "] couldn't be parsed ["); +} + +void SensorTcpImp::tcp_cmd_with_validation( + const std::vector& cmd_tokens, const string& validation) const { + auto result = tcp_cmd(cmd_tokens); + if (result != validation) { + throw std::runtime_error("SensorTcp::tcp_cmd failed: " + cmd_tokens[0] + + " command returned [" + result + + "], expected [" + validation + "]"); + } +} diff --git a/ouster_client/src/sensor_tcp_imp.h b/ouster_client/src/sensor_tcp_imp.h new file mode 100644 index 00000000..3bc6feab --- /dev/null +++ b/ouster_client/src/sensor_tcp_imp.h @@ -0,0 +1,132 @@ +/** + * Copyright (c) 2022, Ouster, Inc. + * All rights reserved. + * + * @file sensor_tcp.h + * @brief A high level TCP interface for Ouster sensors. + * + */ + +#pragma once + +#include "netcompat.h" + +#include "sensor_http.h" + +namespace ouster { +namespace sensor { +namespace impl { + +/** + * A TCP implementation of the SensorHTTP interface + */ +class SensorTcpImp : public util::SensorHttp { + // timeout for reading from a TCP socket during config + const int RCVTIMEOUT_SEC = 10; + // maximum size to to handle during recv + const size_t MAX_RESULT_LENGTH = 16 * 1024; + + public: + /** + * Constructs an tcp interface to communicate with the sensor. + * + * @param[in] hostname hostname of the sensor to communicate with. + */ + SensorTcpImp(const std::string& hostname); + + /** + * Deconstruct the sensor tcp interface. + */ + ~SensorTcpImp() override; + /** + * Queries the sensor metadata. + * + * @return returns a Json object of the sensor metadata. + */ + Json::Value metadata() const override; + + /** + * Queries the sensor_info. + * + * @return returns a Json object representing the sensor_info. + */ + Json::Value sensor_info() const override; + + /** + * Queries active/staged configuration on the sensor + * + * @param[in] active if true retrieve active, otherwise get staged configs. + * + * @return a string represnting the active or staged config + */ + std::string get_config_params(bool active) const override; + + /** + * Set the value of a specfic configuration on the sensor, the changed + * configuration is not active until the sensor is restarted. + * + * @param[in] key name of the config to change. + * @param[in] value the new value to set for the selected configuration. + */ + void set_config_param(const std::string& key, + const std::string& value) const override; + + /** + * Enables automatic assignment of udp destination ports. + */ + void set_udp_dest_auto() const override; + + /** + * Retrieves beam intrinsics of the sensor. + */ + Json::Value beam_intrinsics() const override; + + /** + * Retrieves imu intrinsics of the sensor. + */ + Json::Value imu_intrinsics() const override; + + /** + * Retrieves lidar intrinsics of the sensor. + */ + Json::Value lidar_intrinsics() const override; + + /** + * Retrieves lidar data format. + */ + Json::Value lidar_data_format() const override; + + /** + * Gets the calibaration status of the sensor. + */ + Json::Value calibration_status() const override; + + /** + * Restarts the sensor applying all staged configurations. + */ + void reinitialize() const override; + + /** + * Persist active configuration parameters to the sensor. + */ + void save_config_params() const override; + + private: + SOCKET cfg_socket(const char* addr); + + std::string tcp_cmd(const std::vector& cmd_tokens) const; + + void tcp_cmd_with_validation(const std::vector& cmd_tokens, + const std::string& validation) const; + + Json::Value tcp_cmd_json(const std::vector& cmd_tokens, + bool exception_on_parse_errors = true) const; + + private: + SOCKET socket_handle; + std::unique_ptr read_buf; +}; + +} // namespace impl +} // namespace sensor +} // namespace ouster \ No newline at end of file diff --git a/ouster_client/src/types.cpp b/ouster_client/src/types.cpp index 242f8105..13c42311 100644 --- a/ouster_client/src/types.cpp +++ b/ouster_client/src/types.cpp @@ -18,7 +18,7 @@ #include #include -#include "ouster/build.h" +#include "ouster/impl/build.h" #include "ouster/version.h" namespace ouster { @@ -34,13 +34,14 @@ namespace impl { template using Table = std::array, N>; -extern const Table lidar_mode_strings{ +extern const Table lidar_mode_strings{ {{MODE_UNSPEC, "UNKNOWN"}, {MODE_512x10, "512x10"}, {MODE_512x20, "512x20"}, {MODE_1024x10, "1024x10"}, {MODE_1024x20, "1024x20"}, - {MODE_2048x10, "2048x10"}}}; + {MODE_2048x10, "2048x10"}, + {MODE_4096x5, "4096x5"}}}; extern const Table timestamp_mode_strings{ {{TIME_FROM_UNSPEC, "UNKNOWN"}, @@ -68,7 +69,7 @@ extern const Table polarity_strings{ extern const Table nmea_baud_rate_strings{ {{BAUD_9600, "BAUD_9600"}, {BAUD_115200, "BAUD_115200"}}}; -Table chanfield_strings{{ +Table chanfield_strings{{ {ChanField::RANGE, "RANGE"}, {ChanField::RANGE2, "RANGE2"}, {ChanField::SIGNAL, "SIGNAL"}, @@ -78,6 +79,16 @@ Table chanfield_strings{{ {ChanField::NEAR_IR, "NEAR_IR"}, {ChanField::FLAGS, "FLAGS"}, {ChanField::FLAGS2, "FLAGS2"}, + {ChanField::CUSTOM0, "CUSTOM0"}, + {ChanField::CUSTOM1, "CUSTOM1"}, + {ChanField::CUSTOM2, "CUSTOM2"}, + {ChanField::CUSTOM3, "CUSTOM3"}, + {ChanField::CUSTOM4, "CUSTOM4"}, + {ChanField::CUSTOM5, "CUSTOM5"}, + {ChanField::CUSTOM6, "CUSTOM6"}, + {ChanField::CUSTOM7, "CUSTOM7"}, + {ChanField::CUSTOM8, "CUSTOM8"}, + {ChanField::CUSTOM9, "CUSTOM9"}, {ChanField::RAW32_WORD1, "RAW32_WORD1"}, {ChanField::RAW32_WORD2, "RAW32_WORD2"}, {ChanField::RAW32_WORD3, "RAW32_WORD3"}, @@ -271,6 +282,8 @@ uint32_t n_cols_of_lidar_mode(lidar_mode mode) { return 1024; case MODE_2048x10: return 2048; + case MODE_4096x5: + return 4096; default: throw std::invalid_argument{"n_cols_of_lidar_mode"}; } @@ -278,6 +291,8 @@ uint32_t n_cols_of_lidar_mode(lidar_mode mode) { int frequency_of_lidar_mode(lidar_mode mode) { switch (mode) { + case MODE_4096x5: + return 5; case MODE_512x10: case MODE_1024x10: case MODE_2048x10: @@ -291,7 +306,7 @@ int frequency_of_lidar_mode(lidar_mode mode) { } std::string client_version() { - return std::string("ouster_client ").append(ouster::CLIENT_VERSION); + return std::string("ouster_client ").append(ouster::SDK_VERSION); } /* String conversion */ @@ -407,8 +422,16 @@ optional udp_profile_imu_of_string(const std::string& s) { static sensor_config parse_config(const Json::Value& root) { sensor_config config{}; - if (!root["udp_dest"].empty()) + if (!root["udp_dest"].empty()) { config.udp_dest = root["udp_dest"].asString(); + } 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; + } + if (!root["udp_port_lidar"].empty()) config.udp_port_lidar = root["udp_port_lidar"].asInt(); if (!root["udp_port_imu"].empty()) @@ -435,7 +458,16 @@ static sensor_config parse_config(const Json::Value& root) { } else { 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; + config.operating_mode = root["auto_start_flag"].asBool() + ? sensor::OPERATING_NORMAL + : sensor::OPERATING_STANDBY; } + if (!root["multipurpose_io_mode"].empty()) { auto multipurpose_io_mode = multipurpose_io_mode_of_string( root["multipurpose_io_mode"].asString()); @@ -504,13 +536,6 @@ static sensor_config parse_config(const Json::Value& root) { if (!root["phase_lock_offset"].empty()) config.phase_lock_offset = root["phase_lock_offset"].asInt(); - // deprecated params from 1.13. set 2.0 configs appropriately - if (!root["udp_ip"].empty()) config.udp_dest = root["udp_ip"].asString(); - if (!root["auto_start_flag"].empty()) - config.operating_mode = root["auto_start_flag"].asBool() - ? sensor::OPERATING_NORMAL - : sensor::OPERATING_STANDBY; - if (!root["columns_per_packet"].empty()) config.columns_per_packet = root["columns_per_packet"].asInt(); @@ -887,13 +912,11 @@ std::string to_string(const sensor_info& info) { return Json::writeString(builder, root); } -Json::Value to_json(const sensor_config& config, bool compat) { +Json::Value to_json(const sensor_config& config) { Json::Value root{Json::objectValue}; if (config.udp_dest) { - // use deprecated cofig for 1.13 compatibility - const char* key = compat ? "udp_ip" : "udp_dest"; - root[key] = config.udp_dest.value(); + root["udp_dest"] = config.udp_dest.value(); } if (config.udp_port_lidar) { @@ -913,14 +936,8 @@ Json::Value to_json(const sensor_config& config, bool compat) { } if (config.operating_mode) { - // use deprecated config for 1.13 compatibility auto mode = config.operating_mode.value(); - if (compat) { - root["auto_start_flag"] = - (mode == OperatingMode::OPERATING_NORMAL) ? 1 : 0; - } else { - root["operating_mode"] = to_string(mode); - } + root["operating_mode"] = to_string(mode); } if (config.multipurpose_io_mode) { @@ -1004,7 +1021,7 @@ Json::Value to_json(const sensor_config& config, bool compat) { } std::string to_string(const sensor_config& config) { - Json::Value root = to_json(config, false); + Json::Value root = to_json(config); Json::StreamWriterBuilder builder; builder["enableYAMLCompatibility"] = true; diff --git a/ouster_pcap/CMakeLists.txt b/ouster_pcap/CMakeLists.txt index 1a946dc6..3bd6b203 100644 --- a/ouster_pcap/CMakeLists.txt +++ b/ouster_pcap/CMakeLists.txt @@ -1,41 +1,27 @@ -cmake_minimum_required(VERSION 3.1.0) - -# ==== Project Name ==== -project(ouster_pcap) - # ==== Requirements ==== find_package(libtins REQUIRED) -set(TINS_LIB tins) -if(CONAN_EXPORTED) - # Conan requirements sourced, so use them - find_package(libpcap REQUIRED) - set(TINS_LIB libtins::libtins) -endif() # ==== Libraries ==== add_library(ouster_pcap src/os_pcap.cpp) -target_link_libraries(ouster_pcap PRIVATE ${TINS_LIB}) -# https://github.com/microsoft/vcpkg/issues/798 -find_path(TINS_INCLUDE_DIR tins/tins.h) -target_include_directories(ouster_pcap PRIVATE - ${TINS_INCLUDE_DIR} - ${libpcap_INCLUDE_DIR} - PUBLIC +target_include_directories(ouster_pcap SYSTEM PRIVATE + ${PCAP_INCLUDE_DIR} ${libtins_INCLUDE_DIRS}) +target_include_directories(ouster_pcap PUBLIC $ $) -add_library(OusterSDK::ouster_pcap ALIAS ouster_pcap) -# some warnings coming from libtins -if(MSVC) - target_compile_options(ouster_pcap PUBLIC /wd4251) +if(WIN32) + target_compile_options(ouster_pcap PRIVATE /wd4200) + target_link_libraries(ouster_pcap PUBLIC ws2_32) endif() +target_link_libraries(ouster_pcap PRIVATE libtins) +add_library(OusterSDK::ouster_pcap ALIAS ouster_pcap) # ==== Install ==== install(TARGETS ouster_pcap - EXPORT ouster-sdk-targets - LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib - RUNTIME DESTINATION bin - INCLUDES DESTINATION include) + EXPORT ouster-sdk-targets + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include) install(DIRECTORY include/ouster DESTINATION include) diff --git a/ouster_pcap/include/ouster/os_pcap.h b/ouster_pcap/include/ouster/os_pcap.h index a71a120f..8f240d34 100644 --- a/ouster_pcap/include/ouster/os_pcap.h +++ b/ouster_pcap/include/ouster/os_pcap.h @@ -97,7 +97,8 @@ bool next_packet_info(playback_handle& handle, packet_info& info); size_t read_packet(playback_handle& handle, uint8_t* buf, size_t buffer_size); /** - * Initialize the record handle for recording pcap files. + * Initialize the record handle for recording single sensor pcap files. Will be + * removed * * @param[in] file The file path to the target pcap to record to. * @param[in] src_ip The source address to label the packets with. @@ -105,11 +106,22 @@ size_t read_packet(playback_handle& handle, uint8_t* buf, size_t buffer_size); * @param[in] frag_size The size of the fragments for packet fragmentation. * @param[in] use_sll_encapsulation Whether to use sll encapsulation. */ -std::shared_ptr record_initialize( +[[deprecated]] std::shared_ptr record_initialize( const std::string& file, const std::string& src_ip, const std::string& dst_ip, int frag_size, bool use_sll_encapsulation = false); +/** + * Initialize the record handle for recording multi sensor pcap files. Source + * and destination IPs must be provided with each packet. + * + * @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. + */ +std::shared_ptr record_initialize( + const std::string& file, int frag_size, bool use_sll_encapsulation = false); + /** * Uninitialize the record handle, closing underlying file. * @@ -118,9 +130,28 @@ std::shared_ptr record_initialize( void record_uninitialize(record_handle& handle); /** - * Record a buffer to a record_handle pcap file. + * Record a buffer to a single sensor record_handle pcap file. Source + * and destination IPs must be provided during initialization. Will be removed. + * + * @param[in] handle The record handle that record_initialize has initted. + * @param[in] src_port The source port to label the packets with. + * @param[in] dst_port The destination port to label the packets with. + * @param[in] buf The buffer to record to the pcap file. + * @param[in] buffer_size The size of the buffer to record to the pcap file. + * @param[in] microsecond_timestamp The timestamp to record the packet as + * microseconds. + */ +[[deprecated]] void record_packet(record_handle& handle, int src_port, + int dst_port, const uint8_t* buf, + size_t buffer_size, + uint64_t microsecond_timestamp); + +/** + * Record a buffer to a multi sensor record_handle pcap file. * * @param[in] handle The record handle that record_initialize has initted. + * @param[in] src_ip The source address to label the packets with. + * @param[in] dst_ip The destination address to label the packets with. * @param[in] src_port The source port to label the packets with. * @param[in] dst_port The destination port to label the packets with. * @param[in] buf The buffer to record to the pcap file. @@ -128,7 +159,8 @@ void record_uninitialize(record_handle& handle); * @param[in] microsecond_timestamp The timestamp to record the packet as * microseconds. */ -void record_packet(record_handle& handle, int src_port, int dst_port, +void record_packet(record_handle& handle, const std::string& src_ip, + const std::string& dst_ip, int src_port, int dst_port, const uint8_t* buf, size_t buffer_size, uint64_t microsecond_timestamp); diff --git a/ouster_pcap/src/os_pcap.cpp b/ouster_pcap/src/os_pcap.cpp index 6838e778..06e450a4 100644 --- a/ouster_pcap/src/os_pcap.cpp +++ b/ouster_pcap/src/os_pcap.cpp @@ -6,9 +6,6 @@ #include "ouster/os_pcap.h" -#include -#include - #include #include #include @@ -16,6 +13,8 @@ #include #include +#include + using namespace Tins; namespace ouster { @@ -31,7 +30,6 @@ struct record_handle { std::unique_ptr pcap_file_writer; ///< Object that holds the pcap writer bool use_sll_encapsulation; - record_handle() {} ~record_handle() {} @@ -48,6 +46,8 @@ struct playback_handle { Tins::IPv4Reassembler reassembler; ///< The reassembler mainly for lidar packets + int encap_proto; + playback_handle() {} ~playback_handle() {} @@ -79,6 +79,7 @@ std::shared_ptr replay_initialize( result->file_name = file_name; result->pcap_reader.reset(new FileSniffer(file_name)); + result->encap_proto = result->pcap_reader->link_type(); return result; } @@ -114,7 +115,7 @@ bool next_packet_info(playback_handle& handle, packet_info& info) { info.fragments_in_packet = reassm_packets; reassm_packets = 0; - info.encapsulation_protocol = (int)pdu->pdu_type(); + info.encapsulation_protocol = handle.encap_proto; result = true; UDP* udp = pdu->find_pdu(); auto raw = pdu->find_pdu(); @@ -203,6 +204,13 @@ std::shared_ptr record_initialize(const std::string& file_name, return result; } +std::shared_ptr record_initialize(const std::string& file_name, + int frag_size, + bool use_sll_encapsulation) { + return record_initialize(file_name, "", "", frag_size, + use_sll_encapsulation); +} + void record_uninitialize(record_handle& handle) { if (handle.pcap_file_writer) handle.pcap_file_writer.reset(); } @@ -226,7 +234,9 @@ header. */ // SLL is the linux pcap capture container -std::vector buffer_to_frag_packets(record_handle& handle, int src_port, +std::vector buffer_to_frag_packets(record_handle& handle, + const std::string& src_ip, + const std::string& dst_ip, int src_port, int dst_port, const uint8_t* buf, size_t buf_size) { std::vector result; @@ -240,7 +250,7 @@ std::vector buffer_to_frag_packets(record_handle& handle, int src_port, while (i < buf_size) { // First create the ipv4 packet - IP pkt = IP(handle.src_ip, handle.dst_ip); + IP pkt = IP(dst_ip, src_ip); // Now figure out the size of the packet payload size_t size = std::min(handle.frag_size, (buf_size - i)); @@ -307,9 +317,21 @@ std::vector buffer_to_frag_packets(record_handle& handle, int src_port, void record_packet(record_handle& handle, int src_port, int dst_port, const uint8_t* buf, size_t buffer_size, uint64_t microsecond_timestamp) { + record_packet(handle, handle.src_ip, handle.dst_ip, src_port, dst_port, buf, + buffer_size, microsecond_timestamp); +} + +void record_packet(record_handle& handle, const std::string& src_ip, + const std::string& dst_ip, int src_port, int dst_port, + const uint8_t* buf, size_t buffer_size, + uint64_t microsecond_timestamp) { + // ensure IPs were provided + if (dst_ip.empty() || src_ip.empty()) { + throw std::invalid_argument("Invalid addresses provided for packet"); + } // For each of the packets write it to the pcap file - for (auto item : - buffer_to_frag_packets(handle, src_port, dst_port, buf, buffer_size)) { + for (auto item : buffer_to_frag_packets(handle, src_ip, dst_ip, src_port, + dst_port, buf, buffer_size)) { Packet packet; PDU* pdu; if (handle.use_sll_encapsulation) { diff --git a/ouster_ros/CMakeLists.txt b/ouster_ros/CMakeLists.txt index e749386d..8e296cbc 100644 --- a/ouster_ros/CMakeLists.txt +++ b/ouster_ros/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.1.0) +cmake_minimum_required(VERSION 3.10.0) list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/../cmake) include(DefaultBuildType) @@ -10,6 +10,8 @@ project(ouster_ros) find_package(Eigen3 REQUIRED) find_package(PCL REQUIRED COMPONENTS common) find_package(tf2_eigen REQUIRED) +find_package(CURL REQUIRED) +find_package(Boost REQUIRED) find_package( catkin REQUIRED @@ -20,16 +22,18 @@ find_package( pcl_conversions roscpp tf2 - tf2_ros) + tf2_ros + nodelet) # ==== Options ==== set(CMAKE_CXX_STANDARD 14) -add_compile_options(-Wall -Wextra -Werror -Wno-error=deprecated-declarations) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +add_compile_options(-Wall -Wextra) option(CMAKE_POSITION_INDEPENDENT_CODE "Build position independent code." ON) # ==== Catkin ==== add_message_files(FILES PacketMsg.msg) -add_service_files(FILES OSConfigSrv.srv) +add_service_files(FILES GetConfig.srv SetConfig.srv GetMetadata.srv) generate_messages(DEPENDENCIES std_msgs sensor_msgs geometry_msgs) set(_ouster_ros_INCLUDE_DIRS @@ -49,16 +53,27 @@ catkin_package( DEPENDS EIGEN3) +catkin_package( + LIBRARIES nodelet_os_cloud + CATKIN_DEPENDS nodelet roscpp std_msgs +) + +catkin_package( + LIBRARIES nodelet_os_image + CATKIN_DEPENDS nodelet roscpp std_msgs +) + # ==== Libraries ==== # Build static libraries and bundle them into ouster_ros using the `--whole-archive` flag. This is # necessary because catkin doesn't interoperate easily with target-based cmake builds. Object # libraries are the recommended way to do this, but require >=3.13 to propagate usage requirements. set(_SAVE_BUILD_SHARED_LIBS ${BUILD_SHARED_LIBS}) set(BUILD_SHARED_LIBS OFF) -add_subdirectory(../ouster_client ouster_client EXCLUDE_FROM_ALL) -target_compile_options(ouster_client PRIVATE -Wno-deprecated-declarations) -message(STATUS "Ouster SDK client: Using EIGEN_MAX_ALIGN_BYTES = 32") -target_compile_definitions(ouster_client PUBLIC EIGEN_MAX_ALIGN_BYTES=32) + +option(BUILD_VIZ "Enabled for Python build" OFF) +option(BUILD_PCAP "Enabled for Python build" OFF) +find_package(OusterSDK REQUIRED) + set(BUILD_SHARED_LIBS ${_SAVE_BUILD_SHARED_LIBS}) # catkin adds all include dirs to a single variable, don't try to use targets @@ -70,30 +85,39 @@ add_definitions(-DEIGEN_MPL2_ONLY) add_library(ouster_ros src/ros.cpp) target_link_libraries(ouster_ros PUBLIC ${catkin_LIBRARIES} ouster_build pcl_common PRIVATE -Wl,--whole-archive ouster_client -Wl,--no-whole-archive) -target_compile_definitions(ouster_ros PUBLIC EIGEN_MAX_ALIGN_BYTES=32) add_dependencies(ouster_ros ${PROJECT_NAME}_gencpp) # ==== Executables ==== -add_executable(os_node src/os_node.cpp) -target_link_libraries(os_node ouster_ros ${catkin_LIBRARIES}) -add_dependencies(os_node ${PROJECT_NAME}_gencpp) - -add_executable(os_cloud_node src/os_cloud_node.cpp) -target_link_libraries(os_cloud_node ouster_ros ${catkin_LIBRARIES}) -add_dependencies(os_cloud_node ${PROJECT_NAME}_gencpp) - -add_executable(img_node src/img_node.cpp) -target_link_libraries(img_node ouster_ros ${catkin_LIBRARIES}) -add_dependencies(img_node ${PROJECT_NAME}_gencpp) +add_library(nodelets_os + src/os_client_base_nodelet.cpp + src/os_sensor_nodelet.cpp + src/os_replay_nodelet.cpp + src/os_cloud_nodelet.cpp + src/os_image_nodelet.cpp) +target_link_libraries(nodelets_os ouster_ros ${catkin_LIBRARIES}) +add_dependencies(nodelets_os ${PROJECT_NAME}_gencpp) # ==== Install ==== install( - TARGETS ouster_ros os_node os_cloud_node img_node + TARGETS + ouster_ros + nodelets_os ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) install( DIRECTORY ${_ouster_ros_INCLUDE_DIRS} DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) -install(FILES ../LICENSE ../LICENSE-bin ouster.launch viz.rviz +install( + FILES + ../LICENSE + ../LICENSE-bin DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +install( + DIRECTORY + launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch) +install( + DIRECTORY + config/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config) diff --git a/ouster_ros/Dockerfile b/ouster_ros/Dockerfile index f404cd98..1095dc62 100644 --- a/ouster_ros/Dockerfile +++ b/ouster_ros/Dockerfile @@ -16,6 +16,8 @@ RUN set -xue \ fakeroot dpkg-dev debhelper \ $PY-rosdep $PY-rospkg $PY-bloom +RUN apt-get install -y curl libcurl4-openssl-dev + # Set up non-root build user ARG BUILD_UID=1000 ARG BUILD_GID=${BUILD_UID} @@ -46,6 +48,7 @@ RUN set -xe \ FROM build-env +ENV CXXFLAGS="-Werror -Wno-deprecated-declarations" RUN /opt/ros/${ROS_DISTRO}/env.sh catkin_make -DCMAKE_BUILD_TYPE=Release # Entrypoint for running Ouster ros: diff --git a/ouster_ros/README.md b/ouster_ros/README.md new file mode 100644 index 00000000..3ea0307f --- /dev/null +++ b/ouster_ros/README.md @@ -0,0 +1,60 @@ +# ouster_ros + +## Requirements +- Ubuntu 18|20 +- ROS Melodic|Noetic + +## Usage + +### Sensor Mode +```bash +roslaunch ouster_ros sensor.launch \ + sensor_hostname:= \ + metadata:= +``` + +### Replay Mode +```bash +roslaunch ouster_ros replay.launch \ + metadata:= \ + bag_file:= +``` + +### Recording Mode +```bash +roslaunch ouster_ros record.launch \ + sensor_hostname:= \ + metadata:= \ + bag_file:= +``` + +## Services +The ROS driver currently advertises three services `/ouster/get_metadata`, +`/ouster/get_config`, and `/ouster/set_config`. The first one is available +in all three modes of operation: Sensor, Replay, and Recording. The latter two, +however, are only available in Sensor and Recording modes. i.e. when connected +to a sensor. + +The usage of the three services is described below: +* `/ouster/get_metadata`: This service takes no parameters and returns the +current sensor metadata, you may use as follow: +```bash +rosservice call /ouster/get_metadata "{}" +``` +This will return a json string that contains the sensor metadata + +* `/ouster/get_config`: This service takes no parameters and returns the +current sensor configuration, you may use as follow: +```bash +rosservice call /ouster/get_config "{}" +``` +This will return a json string represting the current configuration + +* `/ouster/set_config`: Takes a single parameter and also returns the updated +sensor configuration. You may use as follows: +```bash +rosservice call /ouster/set_config "config_file: ''" +``` +It is not guranteed that all requested configuration are applied to the sensor, +thus it is the caller responsibilty to examine the returned json object and +check which of the sensor configuration parameters were successfully applied. \ No newline at end of file diff --git a/ouster_ros/viz.rviz b/ouster_ros/config/viz.rviz similarity index 96% rename from ouster_ros/viz.rviz rename to ouster_ros/config/viz.rviz index 05aeff64..5dc13c07 100644 --- a/ouster_ros/viz.rviz +++ b/ouster_ros/config/viz.rviz @@ -77,7 +77,7 @@ Visualization Manager: Size (Pixels): 2 Size (m): 0.029999999329447746 Style: Flat Squares - Topic: /os_cloud_node/points + Topic: /ouster/points Unreliable: false Use Fixed Frame: true Use rainbow: true @@ -107,14 +107,14 @@ Visualization Manager: Size (Pixels): 3 Size (m): 0.029999999329447746 Style: Flat Squares - Topic: /os_cloud_node/points2 + Topic: /ouster/points2 Unreliable: false Use Fixed Frame: true Use rainbow: true Value: true - Class: rviz/Image Enabled: true - Image Topic: /img_node/reflec_image + Image Topic: /ouster/reflec_image Max Value: 1 Median window: 5 Min Value: 0 @@ -126,7 +126,7 @@ Visualization Manager: Value: true - Class: rviz/Image Enabled: false - Image Topic: /img_node/range_image + Image Topic: /ouster/range_image Max Value: 1 Median window: 50 Min Value: 0 @@ -138,7 +138,7 @@ Visualization Manager: Value: false - Class: rviz/Image Enabled: false - Image Topic: /img_node/signal_image + Image Topic: /ouster/signal_image Max Value: 1 Median window: 5 Min Value: 0 @@ -150,7 +150,7 @@ Visualization Manager: Value: false - Class: rviz/Image Enabled: false - Image Topic: /img_node/nearir_image + Image Topic: /ouster/nearir_image Max Value: 1 Median window: 5 Min Value: 0 @@ -162,7 +162,7 @@ Visualization Manager: Value: false - Class: rviz/Image Enabled: false - Image Topic: /img_node/reflec_image2 + Image Topic: /ouster/reflec_image2 Max Value: 1 Median window: 5 Min Value: 0 @@ -174,7 +174,7 @@ Visualization Manager: Value: false - Class: rviz/Image Enabled: false - Image Topic: /img_node/range_image2 + Image Topic: /ouster/range_image2 Max Value: 1 Median window: 5 Min Value: 0 @@ -186,7 +186,7 @@ Visualization Manager: Value: false - Class: rviz/Image Enabled: false - Image Topic: /img_node/signal_image2 + Image Topic: /ouster/signal_image2 Max Value: 1 Median window: 5 Min Value: 0 diff --git a/ouster_ros/include/ouster_ros/os_client_base_nodelet.h b/ouster_ros/include/ouster_ros/os_client_base_nodelet.h new file mode 100644 index 00000000..287b0d55 --- /dev/null +++ b/ouster_ros/include/ouster_ros/os_client_base_nodelet.h @@ -0,0 +1,38 @@ +/** + * Copyright (c) 2018, Ouster, Inc. + * All rights reserved. + * + * @file + * @brief Example node to publish raw sensor output on ROS topics + * + * ROS Parameters + * sensor_hostname: hostname or IP in dotted decimal form of the sensor + * udp_dest: hostname or IP where the sensor will send data packets + * lidar_port: port to which the sensor should send lidar data + * imu_port: port to which the sensor should send imu data + */ + +#include +#include + +#include "ouster/impl/build.h" +#include "ouster/client.h" +#include "ouster/types.h" +#include "ouster_ros/ros.h" + +namespace nodelets_os { + +class OusterClientBase : public nodelet::Nodelet { + protected: + virtual void onInit() override; + + protected: + void display_lidar_info(const ouster::sensor::sensor_info& info); + + protected: + ouster::sensor::sensor_info info; + ros::ServiceServer get_metadata_srv; + std::string cached_metadata; +}; + +} // namespace nodelets_os \ No newline at end of file diff --git a/ouster_ros/include/ouster_ros/ros.h b/ouster_ros/include/ouster_ros/ros.h index d1bfd7f0..f420068e 100644 --- a/ouster_ros/include/ouster_ros/ros.h +++ b/ouster_ros/include/ouster_ros/ros.h @@ -32,9 +32,9 @@ using ns = std::chrono::nanoseconds; /** * Read an imu packet into a ROS message. Blocks for up to a second if no data * is available. - * @param cli the sensor client - * @param pm the destination packet message - * @param pf the packet format + * @param[in] cli the sensor client + * @param[out] pm the destination packet message + * @param[in] pf the packet format * @return whether reading was successful */ bool read_imu_packet(const sensor::client& cli, PacketMsg& pm, @@ -43,9 +43,9 @@ bool read_imu_packet(const sensor::client& cli, PacketMsg& pm, /** * Read a lidar packet into a ROS message. Blocks for up to a second if no data * is available. - * @param cli the sensor client - * @param pm the destination packet message - * @param pf the packet format + * @param[in] cli the sensor client + * @param[out] pm the destination packet message + * @param[in] pf the packet format * @return whether reading was successful */ bool read_lidar_packet(const sensor::client& cli, PacketMsg& pm, @@ -53,22 +53,36 @@ bool read_lidar_packet(const sensor::client& cli, PacketMsg& pm, /** * Parse an imu packet message into a ROS imu message - * @param pm packet message populated by read_imu_packet - * @param frame the frame to set in the resulting ROS message - * @param pf the packet format + * @param[in] pm packet message populated by read_imu_packet + * @param[in] timestamp the timestamp to give the resulting ROS message + * @param[in] frame the frame to set in the resulting ROS message + * @param[in] pf the packet format * @return ROS sensor message with fields populated from the packet */ sensor_msgs::Imu packet_to_imu_msg(const PacketMsg& pm, + const ros::Time& timestamp, const std::string& frame, const sensor::packet_format& pf); +/** + * Parse an imu packet message into a ROS imu message + * @param[in] pm packet message populated by read_imu_packet + * @param[in] frame the frame to set in the resulting ROS message + * @param[in] pf the packet format + * @return ROS sensor message with fields populated from the packet + */ +[[deprecated]] sensor_msgs::Imu packet_to_imu_msg( + const PacketMsg& pm, const std::string& frame, + const sensor::packet_format& pf); + /** * Populate a PCL point cloud from a LidarScan - * @param xyz_lut lookup table from sensor beam angles (see lidar_scan.h) - * @param scan_ts scan start used to caluclate relative timestamps for points - * @param ls input lidar data - * @param return_index index of return desired starting at 0 - * @param cloud output pcl pointcloud to populate + * @param[in] xyz_lut lookup table from sensor beam angles (see lidar_scan.h) + * @param[in] scan_ts scan start used to caluclate relative timestamps for + * points + * @param[in] ls input lidar data + * @param[out] cloud output pcl pointcloud to populate + * @param[in] return_index index of return desired starting at 0 */ void scan_to_cloud(const ouster::XYZLut& xyz_lut, ouster::LidarScan::ts_t scan_ts, const ouster::LidarScan& ls, @@ -76,19 +90,30 @@ void scan_to_cloud(const ouster::XYZLut& xyz_lut, /** * Serialize a PCL point cloud to a ROS message - * @param cloud the PCL point cloud to convert - * @param timestamp the timestamp to give the resulting ROS message - * @param frame the frame to set in the resulting ROS message + * @param[in] cloud the PCL point cloud to convert + * @param[in] timestamp the timestamp to apply to the resulting ROS message + * @param[in] frame the frame to set in the resulting ROS message * @return a ROS message containing the point cloud */ -sensor_msgs::PointCloud2 cloud_to_cloud_msg(const Cloud& cloud, ns timestamp, +sensor_msgs::PointCloud2 cloud_to_cloud_msg(const Cloud& cloud, + const ros::Time& timestamp, const std::string& frame); +/** + * Serialize a PCL point cloud to a ROS message + * @param[in] cloud the PCL point cloud to convert + * @param[in] timestamp the timestamp to apply to the resulting ROS message + * @param[in] frame the frame to set in the resulting ROS message + * @return a ROS message containing the point cloud + */ +[[deprecated]] sensor_msgs::PointCloud2 cloud_to_cloud_msg( + const Cloud& cloud, ns timestamp, const std::string& frame); + /** * Convert transformation matrix return by sensor to ROS transform - * @param mat transformation matrix return by sensor - * @param frame the parent frame of the published transform - * @param child_frame the child frame of the published transform + * @param[in] mat transformation matrix return by sensor + * @param[in] frame the parent frame of the published transform + * @param[in] child_frame the child frame of the published transform * @return ROS message suitable for publishing as a transform */ geometry_msgs::TransformStamped transform_to_tf_msg( diff --git a/ouster_ros/launch/common.launch b/ouster_ros/launch/common.launch new file mode 100644 index 00000000..df8d7108 --- /dev/null +++ b/ouster_ros/launch/common.launch @@ -0,0 +1,40 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ouster_ros/launch/record.launch b/ouster_ros/launch/record.launch new file mode 100644 index 00000000..e31b017d --- /dev/null +++ b/ouster_ros/launch/record.launch @@ -0,0 +1,75 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ouster_ros/launch/replay.launch b/ouster_ros/launch/replay.launch new file mode 100644 index 00000000..2802ab70 --- /dev/null +++ b/ouster_ros/launch/replay.launch @@ -0,0 +1,39 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ouster_ros/launch/sensor.launch b/ouster_ros/launch/sensor.launch new file mode 100644 index 00000000..6647e3a1 --- /dev/null +++ b/ouster_ros/launch/sensor.launch @@ -0,0 +1,63 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ouster_ros/nodelets_os.xml b/ouster_ros/nodelets_os.xml new file mode 100644 index 00000000..45c2162a --- /dev/null +++ b/ouster_ros/nodelets_os.xml @@ -0,0 +1,22 @@ + + + + A nodelet that connects to an Ouster sensor and publish incoming imu and lidar packets to other ros nodes. + + + + + A nodelet that can load up existing Ouster recordings and replay them. + + + + + A nodelet that process incoming Ouster lidar packets and publishes a corresponding point cloud. + + + + + A nodelet that processes Ouster point clouds and publish them as depth images. + + + diff --git a/ouster_ros/ouster.launch b/ouster_ros/ouster.launch deleted file mode 100644 index 2602b4ab..00000000 --- a/ouster_ros/ouster.launch +++ /dev/null @@ -1,46 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/ouster_ros/package.xml b/ouster_ros/package.xml index 47431754..e3a9a5ec 100644 --- a/ouster_ros/package.xml +++ b/ouster_ros/package.xml @@ -1,7 +1,7 @@ ouster_ros - 0.3.1 + 0.5.0 Ouster example ROS client ouster developers BSD @@ -13,6 +13,8 @@ geometry_msgs tf2_ros + boost + nodelet libjsoncpp-dev eigen message_generation @@ -20,11 +22,14 @@ libpcl-all-dev pcl_conversions + nodelet libjsoncpp message_runtime topic_tools gtest - + + + diff --git a/ouster_ros/src/img_node.cpp b/ouster_ros/src/img_node.cpp deleted file mode 100644 index 36b96589..00000000 --- a/ouster_ros/src/img_node.cpp +++ /dev/null @@ -1,197 +0,0 @@ -/** - * Copyright (c) 2020, Ouster, Inc. - * All rights reserved. - * - * @file - * @brief Example node to visualize range, near ir and signal images - * - * Publishes ~/range_image, ~/nearir_image, and ~/signal_image. Please bear - * in mind that there is rounding/clamping to display 8 bit images. For computer - * vision applications, use higher bit depth values in /os_cloud_node/points - */ - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -#include "ouster/client.h" -#include "ouster/image_processing.h" -#include "ouster/types.h" -#include "ouster_ros/OSConfigSrv.h" -#include "ouster_ros/ros.h" - -namespace sensor = ouster::sensor; -namespace viz = ouster::viz; - -using pixel_type = uint16_t; -const size_t pixel_value_max = std::numeric_limits::max(); - -sensor_msgs::ImagePtr make_image_msg(size_t H, size_t W, - const ros::Time& stamp) { - sensor_msgs::ImagePtr msg{new sensor_msgs::Image{}}; - msg->width = W; - msg->height = H; - msg->step = W * sizeof(pixel_type); - msg->encoding = sensor_msgs::image_encodings::MONO16; - msg->data.resize(W * H * sizeof(pixel_type)); - msg->header.stamp = stamp; - - return msg; -} - -int main(int argc, char** argv) { - ros::init(argc, argv, "img_node"); - ros::NodeHandle nh("~"); - - ouster_ros::OSConfigSrv cfg{}; - auto client = nh.serviceClient("os_config"); - client.waitForExistence(); - if (!client.call(cfg)) { - ROS_ERROR("img_node: Calling os config service failed"); - return EXIT_FAILURE; - } - - auto info = sensor::parse_metadata(cfg.response.metadata); - size_t H = info.format.pixels_per_column; - size_t W = info.format.columns_per_frame; - - auto udp_profile_lidar = info.format.udp_profile_lidar; - const int n_returns = - (udp_profile_lidar == sensor::UDPProfileLidar::PROFILE_LIDAR_LEGACY) - ? 1 - : 2; - - const auto& px_offset = info.format.pixel_shift_by_row; - - std::vector img_pubs; - std::vector aes; - - ros::Publisher nearir_image_pub = - nh.advertise("nearir_image", 100); - - std::vector range_image_pubs; - std::vector signal_image_pubs; - std::vector reflec_image_pubs; - - auto topic = [](auto base, int ind) { - if (ind == 0) return std::string(base); - return std::string(base) + - std::to_string(ind + 1); // need second return to return 2 - }; - for (int i = 0; i < n_returns; i++) { - ros::Publisher range_image_pub = - nh.advertise(topic("range_image", i), 100); - range_image_pubs.push_back(range_image_pub); - - ros::Publisher signal_image_pub = - nh.advertise(topic("signal_image", i), 100); - signal_image_pubs.push_back(signal_image_pub); - - ros::Publisher reflec_image_pub = - nh.advertise(topic("reflec_image", i), 100); - reflec_image_pubs.push_back(reflec_image_pub); - } - - ouster_ros::Cloud cloud{}; - - viz::AutoExposure nearir_ae, signal_ae, reflec_ae; - viz::BeamUniformityCorrector nearir_buc; - - sensor_msgs::ImagePtr nearir_image; - - auto base_cloud_handler = [&](const sensor_msgs::PointCloud2::ConstPtr& m, - int return_index) { - pcl::fromROSMsg(*m, cloud); - - auto range_image = make_image_msg(H, W, m->header.stamp); - auto signal_image = make_image_msg(H, W, m->header.stamp); - auto reflec_image = make_image_msg(H, W, m->header.stamp); - nearir_image = make_image_msg(H, W, m->header.stamp); - - ouster::img_t nearir_image_eigen(H, W); - ouster::img_t signal_image_eigen(H, W); - ouster::img_t reflec_image_eigen(H, W); - - // views into message data - auto range_image_map = Eigen::Map>( - (pixel_type*)range_image->data.data(), H, W); - auto signal_image_map = Eigen::Map>( - (pixel_type*)signal_image->data.data(), H, W); - auto reflec_image_map = Eigen::Map>( - (pixel_type*)reflec_image->data.data(), H, W); - auto nearir_image_map = Eigen::Map>( - (pixel_type*)nearir_image->data.data(), H, W); - - // copy data out of Cloud message, with destaggering - for (size_t u = 0; u < H; u++) { - for (size_t v = 0; v < W; v++) { - const size_t vv = (v + W - px_offset[u]) % W; - const auto& pt = cloud[u * W + vv]; - - // 16 bit img: use 4mm resolution and throw out returns > - // 260m - auto r = (pt.range + 0b10) >> 2; - range_image_map(u, v) = r > pixel_value_max ? 0 : r; - - signal_image_eigen(u, v) = pt.intensity; - reflec_image_eigen(u, v) = pt.reflectivity; - nearir_image_eigen(u, v) = pt.ambient; - } - } - - const bool first = (return_index == 0); - - signal_ae(signal_image_eigen, first); - reflec_ae(reflec_image_eigen, first); - nearir_buc(nearir_image_eigen); - nearir_ae(nearir_image_eigen, first); - nearir_image_eigen = nearir_image_eigen.sqrt(); - signal_image_eigen = signal_image_eigen.sqrt(); - - // copy data into image messages - signal_image_map = - (signal_image_eigen * pixel_value_max).cast(); - reflec_image_map = - (reflec_image_eigen * pixel_value_max).cast(); - if (first) - nearir_image_map = - (nearir_image_eigen * pixel_value_max).cast(); - - // publish at return index - range_image_pubs[return_index].publish(range_image); - signal_image_pubs[return_index].publish(signal_image); - reflec_image_pubs[return_index].publish(reflec_image); - }; - - auto first_cloud_handler = - [&](const sensor_msgs::PointCloud2::ConstPtr& m) { - base_cloud_handler(m, 0); - nearir_image_pub.publish(nearir_image); - }; - - auto second_cloud_handler = - [&](const sensor_msgs::PointCloud2::ConstPtr& m) { - base_cloud_handler(m, 1); - }; - - // image processing - auto pc1_sub = nh.subscribe( - topic("points", 0), 100, first_cloud_handler); - auto pc2_sub = nh.subscribe( - topic("points", 1), 100, second_cloud_handler); - - ros::spin(); - return EXIT_SUCCESS; -} diff --git a/ouster_ros/src/os_client_base_nodelet.cpp b/ouster_ros/src/os_client_base_nodelet.cpp new file mode 100644 index 00000000..fed0d382 --- /dev/null +++ b/ouster_ros/src/os_client_base_nodelet.cpp @@ -0,0 +1,38 @@ +/** + * Copyright (c) 2022, Ouster, Inc. + * All rights reserved. + * + * @file os_client_base_nodelet.cpp + * @brief Implementatin of OusterClientBase + */ + +#include "ouster_ros/os_client_base_nodelet.h" + +#include "ouster_ros/GetMetadata.h" + +namespace sensor = ouster::sensor; +using ouster_ros::GetMetadata; + +namespace nodelets_os { + +void OusterClientBase::onInit() { + auto& nh = getNodeHandle(); + get_metadata_srv = + nh.advertiseService( + "get_metadata", + [this](GetMetadata::Request&, GetMetadata::Response& res) { + res.metadata = cached_metadata; + return cached_metadata.size() > 0; + }); + + NODELET_INFO("get_metadata service created"); +} + +void OusterClientBase::display_lidar_info(const sensor::sensor_info& info) { + NODELET_WARN("Client version: %s", ouster::SDK_VERSION_FULL); + NODELET_WARN("Using lidar_mode: %s", sensor::to_string(info.mode).c_str()); + NODELET_WARN("%s sn: %s firmware rev: %s", info.prod_line.c_str(), + info.sn.c_str(), info.fw_rev.c_str()); +} + +} // namespace nodelets_os diff --git a/ouster_ros/src/os_cloud_node.cpp b/ouster_ros/src/os_cloud_node.cpp deleted file mode 100644 index bb7f6faf..00000000 --- a/ouster_ros/src/os_cloud_node.cpp +++ /dev/null @@ -1,118 +0,0 @@ -/** - * Copyright (c) 2019, Ouster, Inc. - * All rights reserved. - * - * @file - * @brief Example node to publish point clouds and imu topics - */ - -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include "ouster/lidar_scan.h" -#include "ouster/types.h" -#include "ouster_ros/OSConfigSrv.h" -#include "ouster_ros/PacketMsg.h" -#include "ouster_ros/ros.h" - -using PacketMsg = ouster_ros::PacketMsg; -using Cloud = ouster_ros::Cloud; -using Point = ouster_ros::Point; -namespace sensor = ouster::sensor; - -int main(int argc, char** argv) { - ros::init(argc, argv, "os_cloud_node"); - ros::NodeHandle nh("~"); - - auto tf_prefix = nh.param("tf_prefix", std::string{}); - if (!tf_prefix.empty() && tf_prefix.back() != '/') tf_prefix.append("/"); - auto sensor_frame = tf_prefix + "os_sensor"; - auto imu_frame = tf_prefix + "os_imu"; - auto lidar_frame = tf_prefix + "os_lidar"; - - ouster_ros::OSConfigSrv cfg{}; - auto client = nh.serviceClient("os_config"); - client.waitForExistence(); - if (!client.call(cfg)) { - ROS_ERROR("os_cloud_node: Calling config service failed"); - return EXIT_FAILURE; - } - - auto info = sensor::parse_metadata(cfg.response.metadata); - uint32_t H = info.format.pixels_per_column; - uint32_t W = info.format.columns_per_frame; - auto udp_profile_lidar = info.format.udp_profile_lidar; - - const int n_returns = - (udp_profile_lidar == sensor::UDPProfileLidar::PROFILE_LIDAR_LEGACY) - ? 1 - : 2; - auto pf = sensor::get_format(info); - - auto imu_pub = nh.advertise("imu", 100); - - auto img_suffix = [](int ind) { - if (ind == 0) return std::string(); - return std::to_string(ind + 1); // need second return to return 2 - }; - - auto lidar_pubs = std::vector(); - for (int i = 0; i < n_returns; i++) { - auto pub = nh.advertise( - std::string("points") + img_suffix(i), 10); - lidar_pubs.push_back(pub); - } - - auto xyz_lut = ouster::make_xyz_lut(info); - - ouster::LidarScan ls{W, H, udp_profile_lidar}; - Cloud cloud{W, H}; - - ouster::ScanBatcher batch(W, pf); - - auto lidar_handler = [&](const PacketMsg& pm) mutable { - if (batch(pm.buf.data(), ls)) { - auto h = std::find_if( - ls.headers.begin(), ls.headers.end(), [](const auto& h) { - return h.timestamp != std::chrono::nanoseconds{0}; - }); - if (h != ls.headers.end()) { - for (int i = 0; i < n_returns; i++) { - scan_to_cloud(xyz_lut, h->timestamp, ls, cloud, i); - lidar_pubs[i].publish(ouster_ros::cloud_to_cloud_msg( - cloud, h->timestamp, sensor_frame)); - } - } - } - }; - - auto imu_handler = [&](const PacketMsg& p) { - imu_pub.publish(ouster_ros::packet_to_imu_msg(p, imu_frame, pf)); - }; - - auto lidar_packet_sub = nh.subscribe( - "lidar_packets", 2048, lidar_handler); - auto imu_packet_sub = nh.subscribe( - "imu_packets", 100, imu_handler); - - // publish transforms - tf2_ros::StaticTransformBroadcaster tf_bcast{}; - - tf_bcast.sendTransform(ouster_ros::transform_to_tf_msg( - info.imu_to_sensor_transform, sensor_frame, imu_frame)); - - tf_bcast.sendTransform(ouster_ros::transform_to_tf_msg( - info.lidar_to_sensor_transform, sensor_frame, lidar_frame)); - - ros::spin(); - - return EXIT_SUCCESS; -} diff --git a/ouster_ros/src/os_cloud_nodelet.cpp b/ouster_ros/src/os_cloud_nodelet.cpp new file mode 100644 index 00000000..eb49b5f1 --- /dev/null +++ b/ouster_ros/src/os_cloud_nodelet.cpp @@ -0,0 +1,191 @@ +/** + * Copyright (c) 2022, Ouster, Inc. + * All rights reserved. + * + * @file os_cloud_nodelet.cpp + * @brief A nodelet to publish point clouds and imu topics + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "ouster/lidar_scan.h" +#include "ouster/types.h" +#include "ouster_ros/GetMetadata.h" +#include "ouster_ros/PacketMsg.h" +#include "ouster_ros/ros.h" + +namespace sensor = ouster::sensor; +using ouster_ros::PacketMsg; +using sensor::UDPProfileLidar; +using namespace std::chrono_literals; + +namespace nodelets_os { +class OusterCloud : public nodelet::Nodelet { + private: + virtual void onInit() override { + auto& pnh = getPrivateNodeHandle(); + + auto tf_prefix = pnh.param("tf_prefix", std::string{}); + if (!tf_prefix.empty() && tf_prefix.back() != '/') + tf_prefix.append("/"); + sensor_frame = tf_prefix + "os_sensor"; + imu_frame = tf_prefix + "os_imu"; + lidar_frame = tf_prefix + "os_lidar"; + auto timestamp_mode_arg = pnh.param("timestamp_mode", std::string{}); + use_ros_time = (timestamp_mode_arg == "TIME_FROM_ROS_TIME"); + + auto& nh = getNodeHandle(); + ouster_ros::GetMetadata metadata{}; + auto client = nh.serviceClient("get_metadata"); + client.waitForExistence(); + if (!client.call(metadata)) { + auto error_msg = "OusterCloud: Calling get_metadata service failed"; + NODELET_ERROR_STREAM(error_msg); + throw std::runtime_error(error_msg); + } + + NODELET_INFO("OusterCloud: retrieved sensor metadata!"); + + info = sensor::parse_metadata(metadata.response.metadata); + uint32_t H = info.format.pixels_per_column; + uint32_t W = info.format.columns_per_frame; + + n_returns = info.format.udp_profile_lidar == + UDPProfileLidar::PROFILE_RNG19_RFL8_SIG16_NIR16_DUAL + ? 2 + : 1; + + NODELET_INFO_STREAM("Profile has " << n_returns << " return(s)"); + + imu_pub = nh.advertise("imu", 100); + + auto img_suffix = [](int ind) { + if (ind == 0) return std::string(); + return std::to_string(ind + 1); // need second return to return 2 + }; + + lidar_pubs.resize(n_returns); + for (int i = 0; i < n_returns; i++) { + auto pub = nh.advertise( + std::string("points") + img_suffix(i), 10); + lidar_pubs[i] = pub; + } + + xyz_lut = ouster::make_xyz_lut(info); + + ls = ouster::LidarScan{W, H, info.format.udp_profile_lidar}; + cloud = ouster_ros::Cloud{W, H}; + + scan_batcher = std::make_unique(info); + + auto lidar_handler = use_ros_time + ? &OusterCloud::lidar_handler_ros_time + : &OusterCloud::lidar_handler_sensor_time; + + lidar_packet_sub = + nh.subscribe("lidar_packets", 2048, lidar_handler, this); + imu_packet_sub = nh.subscribe( + "imu_packets", 100, &OusterCloud::imu_handler, this); + + // publish transforms + tf_bcast.sendTransform(ouster_ros::transform_to_tf_msg( + info.imu_to_sensor_transform, sensor_frame, imu_frame)); + + tf_bcast.sendTransform(ouster_ros::transform_to_tf_msg( + info.lidar_to_sensor_transform, sensor_frame, lidar_frame)); + } + + void convert_scan_to_pointcloud_publish(std::chrono::nanoseconds scan_ts, + const ros::Time& msg_ts) { + for (int i = 0; i < n_returns; ++i) { + scan_to_cloud(xyz_lut, scan_ts, ls, cloud, i); + sensor_msgs::PointCloud2 pc = + ouster_ros::cloud_to_cloud_msg(cloud, msg_ts, sensor_frame); + sensor_msgs::PointCloud2Ptr pc_ptr = + boost::make_shared(pc); + lidar_pubs[i].publish(pc_ptr); + } + } + + void lidar_handler_sensor_time(const PacketMsg::ConstPtr& packet) { + if (!(*scan_batcher)(packet->buf.data(), ls)) return; + auto ts_v = ls.timestamp(); + auto idx = std::find_if(ts_v.data(), ts_v.data() + ts_v.size(), + [](uint64_t h) { return h != 0; }); + if (idx == ts_v.data() + ts_v.size()) return; + auto scan_ts = std::chrono::nanoseconds{ts_v(idx - ts_v.data())}; + convert_scan_to_pointcloud_publish(scan_ts, to_ros_time(scan_ts)); + } + + void lidar_handler_ros_time(const PacketMsg::ConstPtr& packet) { + auto packet_receive_time = ros::Time::now(); + static auto frame_ts = packet_receive_time; // first point cloud time + if (!(*scan_batcher)(packet->buf.data(), ls)) return; + auto ts_v = ls.timestamp(); + auto idx = std::find_if(ts_v.data(), ts_v.data() + ts_v.size(), + [](uint64_t h) { return h != 0; }); + if (idx == ts_v.data() + ts_v.size()) return; + auto scan_ts = std::chrono::nanoseconds{ts_v(idx - ts_v.data())}; + convert_scan_to_pointcloud_publish(scan_ts, frame_ts); + frame_ts = packet_receive_time; // set time for next point cloud msg + } + + void imu_handler(const PacketMsg::ConstPtr& packet) { + auto pf = sensor::get_format(info); + ros::Time msg_ts = + use_ros_time ? ros::Time::now() + : to_ros_time(pf.imu_gyro_ts(packet->buf.data())); + sensor_msgs::Imu imu_msg = + ouster_ros::packet_to_imu_msg(*packet, msg_ts, imu_frame, pf); + sensor_msgs::ImuPtr imu_msg_ptr = + boost::make_shared(imu_msg); + imu_pub.publish(imu_msg_ptr); + }; + + inline ros::Time to_ros_time(uint64_t ts) { + ros::Time t; + t.fromNSec(ts); + return t; + } + + inline ros::Time to_ros_time(std::chrono::nanoseconds ts) { + return to_ros_time(ts.count()); + } + + private: + ros::Subscriber lidar_packet_sub; + std::vector lidar_pubs; + ros::Subscriber imu_packet_sub; + ros::Publisher imu_pub; + + sensor::sensor_info info; + int n_returns = 0; + + ouster::XYZLut xyz_lut; + ouster::LidarScan ls; + ouster_ros::Cloud cloud; + std::unique_ptr scan_batcher; + + std::string sensor_frame; + std::string imu_frame; + std::string lidar_frame; + + tf2_ros::StaticTransformBroadcaster tf_bcast; + + bool use_ros_time; +}; +} // namespace nodelets_os + +PLUGINLIB_EXPORT_CLASS(nodelets_os::OusterCloud, nodelet::Nodelet) diff --git a/ouster_ros/src/os_image_nodelet.cpp b/ouster_ros/src/os_image_nodelet.cpp new file mode 100644 index 00000000..3b400aa9 --- /dev/null +++ b/ouster_ros/src/os_image_nodelet.cpp @@ -0,0 +1,217 @@ +/** + * Copyright (c) 2022, Ouster, Inc. + * All rights reserved. + * + * @file os_image_nodelet.cpp + * @brief A nodelet to decode range, near ir and signal images from ouster + * point cloud + * + * Publishes ~/range_image, ~/nearir_image, and ~/signal_image. Please bear + * in mind that there is rounding/clamping to display 8 bit images. For computer + * vision applications, use higher bit depth values in /os_cloud_node/points + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "ouster/client.h" +#include "ouster/image_processing.h" +#include "ouster/types.h" +#include "ouster_ros/GetMetadata.h" +#include "ouster_ros/ros.h" + +namespace sensor = ouster::sensor; +namespace viz = ouster::viz; +using sensor::UDPProfileLidar; + +using pixel_type = uint16_t; +const size_t pixel_value_max = std::numeric_limits::max(); + +namespace nodelets_os { +class OusterImage : public nodelet::Nodelet { + private: + virtual void onInit() override { + auto& nh = getNodeHandle(); + + ouster_ros::GetMetadata metadata{}; + auto client = nh.serviceClient("get_metadata"); + client.waitForExistence(); + if (!client.call(metadata)) { + auto error_msg = "OusterImage: Calling get_metadata service failed"; + NODELET_ERROR_STREAM(error_msg); + throw std::runtime_error(error_msg); + } + + NODELET_INFO("OusterImage: retrieved sensor metadata!"); + + info = sensor::parse_metadata(metadata.response.metadata); + + const int n_returns = + info.format.udp_profile_lidar == + UDPProfileLidar::PROFILE_RNG19_RFL8_SIG16_NIR16_DUAL + ? 2 + : 1; + + nearir_image_pub = + nh.advertise("nearir_image", 100); + + auto topic = [](auto base, int ind) { + if (ind == 0) return std::string(base); + return std::string(base) + + std::to_string(ind + 1); // need second return to return 2 + }; + + ros::Publisher a_pub; + for (int i = 0; i < n_returns; i++) { + a_pub = + nh.advertise(topic("range_image", i), 100); + range_image_pubs.push_back(a_pub); + + a_pub = + nh.advertise(topic("signal_image", i), 100); + signal_image_pubs.push_back(a_pub); + + a_pub = + nh.advertise(topic("reflec_image", i), 100); + reflec_image_pubs.push_back(a_pub); + } + + uint32_t H = info.format.pixels_per_column; + uint32_t W = info.format.columns_per_frame; + cloud = ouster_ros::Cloud{W, H}; + + // image processing + pc1_sub = nh.subscribe( + topic("points", 0), 100, &OusterImage::first_cloud_handler, this); + + if (n_returns > 1) { + pc2_sub = nh.subscribe( + topic("points", 1), 100, &OusterImage::second_cloud_handler, + this); + } + } + + void base_cloud_handler(const sensor_msgs::PointCloud2::ConstPtr& m, + int return_index) { + pcl::fromROSMsg(*m, cloud); + + const bool first = (return_index == 0); + uint32_t H = info.format.pixels_per_column; + uint32_t W = info.format.columns_per_frame; + + auto range_image = make_image_msg(H, W, m->header.stamp); + auto signal_image = make_image_msg(H, W, m->header.stamp); + auto reflec_image = make_image_msg(H, W, m->header.stamp); + auto nearir_image = make_image_msg(H, W, m->header.stamp); + + ouster::img_t nearir_image_eigen(H, W); + ouster::img_t signal_image_eigen(H, W); + ouster::img_t reflec_image_eigen(H, W); + + // views into message data + auto range_image_map = Eigen::Map>( + (pixel_type*)range_image->data.data(), H, W); + auto signal_image_map = Eigen::Map>( + (pixel_type*)signal_image->data.data(), H, W); + auto reflec_image_map = Eigen::Map>( + (pixel_type*)reflec_image->data.data(), H, W); + + auto nearir_image_map = Eigen::Map>( + (pixel_type*)nearir_image->data.data(), H, W); + + const auto& px_offset = info.format.pixel_shift_by_row; + + // copy data out of Cloud message, with destaggering + for (size_t u = 0; u < H; u++) { + for (size_t v = 0; v < W; v++) { + const size_t vv = (v + W - px_offset[u]) % W; + const auto& pt = cloud[u * W + vv]; + + // 16 bit img: use 4mm resolution and throw out returns > + // 260m + auto r = (pt.range + 0b10) >> 2; + range_image_map(u, v) = r > pixel_value_max ? 0 : r; + + signal_image_eigen(u, v) = pt.intensity; + reflec_image_eigen(u, v) = pt.reflectivity; + nearir_image_eigen(u, v) = pt.ambient; + } + } + + signal_ae(signal_image_eigen, first); + reflec_ae(reflec_image_eigen, first); + nearir_buc(nearir_image_eigen); + nearir_ae(nearir_image_eigen, first); + nearir_image_eigen = nearir_image_eigen.sqrt(); + signal_image_eigen = signal_image_eigen.sqrt(); + + // copy data into image messages + signal_image_map = + (signal_image_eigen * pixel_value_max).cast(); + reflec_image_map = + (reflec_image_eigen * pixel_value_max).cast(); + if (first) { + nearir_image_map = + (nearir_image_eigen * pixel_value_max).cast(); + nearir_image_pub.publish(nearir_image); + } + + // publish at return index + range_image_pubs[return_index].publish(range_image); + signal_image_pubs[return_index].publish(signal_image); + reflec_image_pubs[return_index].publish(reflec_image); + } + + void first_cloud_handler(const sensor_msgs::PointCloud2::ConstPtr& m) { + base_cloud_handler(m, 0); + } + + void second_cloud_handler(const sensor_msgs::PointCloud2::ConstPtr& m) { + base_cloud_handler(m, 1); + } + + static sensor_msgs::ImagePtr make_image_msg(size_t H, size_t W, + const ros::Time& stamp) { + auto msg = boost::make_shared(); + msg->width = W; + msg->height = H; + msg->step = W * sizeof(pixel_type); + msg->encoding = sensor_msgs::image_encodings::MONO16; + msg->data.resize(W * H * sizeof(pixel_type)); + msg->header.stamp = stamp; + return msg; + } + + private: + ros::Publisher nearir_image_pub; + std::vector range_image_pubs; + std::vector signal_image_pubs; + std::vector reflec_image_pubs; + + ros::Subscriber pc1_sub; + ros::Subscriber pc2_sub; + + sensor::sensor_info info; + + ouster_ros::Cloud cloud; + viz::AutoExposure nearir_ae, signal_ae, reflec_ae; + viz::BeamUniformityCorrector nearir_buc; +}; +} // namespace nodelets_os + +PLUGINLIB_EXPORT_CLASS(nodelets_os::OusterImage, nodelet::Nodelet) diff --git a/ouster_ros/src/os_node.cpp b/ouster_ros/src/os_node.cpp deleted file mode 100644 index 0c6ab162..00000000 --- a/ouster_ros/src/os_node.cpp +++ /dev/null @@ -1,291 +0,0 @@ -/** - * Copyright (c) 2018, Ouster, Inc. - * All rights reserved. - * - * @file - * @brief Example node to publish raw sensor output on ROS topics - * - * ROS Parameters - * sensor_hostname: hostname or IP in dotted decimal form of the sensor - * udp_dest: hostname or IP where the sensor will send data packets - * lidar_port: port to which the sensor should send lidar data - * imu_port: port to which the sensor should send imu data - */ - -#include -#include - -#include -#include -#include - -#include "ouster/build.h" -#include "ouster/types.h" -#include "ouster_ros/OSConfigSrv.h" -#include "ouster_ros/PacketMsg.h" -#include "ouster_ros/ros.h" - -using PacketMsg = ouster_ros::PacketMsg; -using OSConfigSrv = ouster_ros::OSConfigSrv; -using nonstd::optional; -namespace sensor = ouster::sensor; - -// fill in values that could not be parsed from metadata -void populate_metadata_defaults(sensor::sensor_info& info, - sensor::lidar_mode specified_lidar_mode) { - if (!info.name.size()) info.name = "UNKNOWN"; - - if (!info.sn.size()) info.sn = "UNKNOWN"; - - ouster::util::version v = ouster::util::version_of_string(info.fw_rev); - if (v == ouster::util::invalid_version) - ROS_WARN("Unknown sensor firmware version; output may not be reliable"); - else if (v < sensor::min_version) - ROS_WARN("Firmware < %s not supported; output may not be reliable", - to_string(sensor::min_version).c_str()); - - if (!info.mode) { - ROS_WARN( - "Lidar mode not found in metadata; output may not be reliable"); - info.mode = specified_lidar_mode; - } - - if (!info.prod_line.size()) info.prod_line = "UNKNOWN"; - - if (info.beam_azimuth_angles.empty() || info.beam_altitude_angles.empty()) { - ROS_WARN("Beam angles not found in metadata; using design values"); - info.beam_azimuth_angles = sensor::gen1_azimuth_angles; - info.beam_altitude_angles = sensor::gen1_altitude_angles; - } -} - -// try to write metadata file -bool write_metadata(const std::string& meta_file, const std::string& metadata) { - std::ofstream ofs; - ofs.open(meta_file); - ofs << metadata << std::endl; - ofs.close(); - if (ofs) { - ROS_INFO("Wrote metadata to %s", meta_file.c_str()); - } else { - ROS_WARN( - "Failed to write metadata to %s; check that the path is valid. If " - "you provided a relative path, please note that the working " - "directory of all ROS nodes is set by default to $ROS_HOME", - meta_file.c_str()); - return false; - } - return true; -} - -int connection_loop(ros::NodeHandle& nh, sensor::client& cli, - const sensor::sensor_info& info) { - auto lidar_packet_pub = nh.advertise("lidar_packets", 1280); - auto imu_packet_pub = nh.advertise("imu_packets", 100); - - auto pf = sensor::get_format(info); - - PacketMsg lidar_packet, imu_packet; - lidar_packet.buf.resize(pf.lidar_packet_size + 1); - imu_packet.buf.resize(pf.imu_packet_size + 1); - - while (ros::ok()) { - auto state = sensor::poll_client(cli); - if (state == sensor::EXIT) { - ROS_INFO("poll_client: caught signal, exiting"); - return EXIT_SUCCESS; - } - if (state & sensor::CLIENT_ERROR) { - ROS_ERROR("poll_client: returned error"); - return EXIT_FAILURE; - } - if (state & sensor::LIDAR_DATA) { - if (sensor::read_lidar_packet(cli, lidar_packet.buf.data(), pf)) - lidar_packet_pub.publish(lidar_packet); - } - if (state & sensor::IMU_DATA) { - if (sensor::read_imu_packet(cli, imu_packet.buf.data(), pf)) - imu_packet_pub.publish(imu_packet); - } - ros::spinOnce(); - } - return EXIT_SUCCESS; -} - -int main(int argc, char** argv) { - ros::init(argc, argv, "os_node"); - ros::NodeHandle nh("~"); - - std::string published_metadata; - auto srv = nh.advertiseService( - "os_config", [&](OSConfigSrv::Request&, OSConfigSrv::Response& res) { - if (published_metadata.size()) { - res.metadata = published_metadata; - return true; - } else - return false; - }); - - // empty indicates "not set" since roslaunch xml can't optionally set params - auto hostname = nh.param("sensor_hostname", std::string{}); - auto udp_dest = nh.param("udp_dest", std::string{}); - auto lidar_port = nh.param("lidar_port", 0); - auto imu_port = nh.param("imu_port", 0); - auto replay = nh.param("replay", false); - auto lidar_mode_arg = nh.param("lidar_mode", std::string{}); - auto timestamp_mode_arg = nh.param("timestamp_mode", std::string{}); - - std::string udp_profile_lidar_arg; - nh.param("udp_profile_lidar", udp_profile_lidar_arg, ""); - - optional udp_profile_lidar; - if (udp_profile_lidar_arg.size()) { - if (replay) - ROS_WARN("UDP Profile Lidar set in replay mode. Will be ignored."); - - // set lidar profile from param - udp_profile_lidar = - sensor::udp_profile_lidar_of_string(udp_profile_lidar_arg); - if (!udp_profile_lidar) { - ROS_ERROR("Invalid udp profile lidar: %s", - udp_profile_lidar_arg.c_str()); - return EXIT_FAILURE; - } - } - - // set lidar mode from param - sensor::lidar_mode lidar_mode = sensor::MODE_UNSPEC; - if (lidar_mode_arg.size()) { - if (replay) ROS_WARN("Lidar mode set in replay mode. May be ignored"); - - lidar_mode = sensor::lidar_mode_of_string(lidar_mode_arg); - if (!lidar_mode) { - ROS_ERROR("Invalid lidar mode %s", lidar_mode_arg.c_str()); - return EXIT_FAILURE; - } - } - - // set timestamp mode from param - sensor::timestamp_mode timestamp_mode = sensor::TIME_FROM_UNSPEC; - if (timestamp_mode_arg.size()) { - if (replay) - ROS_WARN("Timestamp mode set in replay mode. Will be ignored"); - - timestamp_mode = sensor::timestamp_mode_of_string(timestamp_mode_arg); - if (!timestamp_mode) { - ROS_ERROR("Invalid timestamp mode %s", timestamp_mode_arg.c_str()); - return EXIT_FAILURE; - } - } - - auto meta_file = nh.param("metadata", std::string{}); - if (!meta_file.size()) { - if (replay) { - ROS_ERROR("Must specify metadata file in replay mode"); - } else { - ROS_ERROR("Must specify path for metadata output"); - } - return EXIT_FAILURE; - } - - if (!replay && !hostname.size()) { - ROS_ERROR("Must specify a sensor hostname"); - return EXIT_FAILURE; - } - - ROS_INFO("Client version: %s", ouster::CLIENT_VERSION_FULL); - - if (replay) { - ROS_INFO("Running in replay mode"); - - // populate info for config service - try { - auto info = sensor::metadata_from_json(meta_file); - published_metadata = to_string(info); - - ROS_INFO("Using lidar_mode: %s", - sensor::to_string(info.mode).c_str()); - ROS_INFO("%s sn: %s firmware rev: %s", info.prod_line.c_str(), - info.sn.c_str(), info.fw_rev.c_str()); - - // just serve config service - ros::spin(); - return EXIT_SUCCESS; - } catch (const std::runtime_error& e) { - ROS_ERROR("Error when running in replay mode: %s", e.what()); - } - } else { - ROS_INFO("Starting sensor %s initialization...", hostname.c_str()); - - // use no-config version of init_client to allow for random ports - auto cli = sensor::init_client(hostname, lidar_port, imu_port); - if (!cli) { - ROS_ERROR("Failed to initialize client"); - return EXIT_FAILURE; - } - - sensor::sensor_config config; - config.udp_port_imu = get_imu_port(*cli); - config.udp_port_lidar = get_lidar_port(*cli); - config.udp_profile_lidar = udp_profile_lidar; - config.operating_mode = sensor::OPERATING_NORMAL; - if (lidar_mode) config.ld_mode = lidar_mode; - if (timestamp_mode) config.ts_mode = timestamp_mode; - - uint8_t config_flags = 0; - - if (udp_dest.size()) { - ROS_INFO("Will send UDP data to %s", udp_dest.c_str()); - config.udp_dest = udp_dest; - } else { - ROS_INFO("Will use automatic UDP destination"); - config_flags |= ouster::sensor::CONFIG_UDP_DEST_AUTO; - } - - try { - if (!set_config(hostname, config, config_flags)) { - ROS_ERROR("Error connecting to sensor %s", hostname.c_str()); - return EXIT_FAILURE; - } - ROS_INFO("Sensor %s configured successfully", hostname.c_str()); - } catch (const std::runtime_error& e) { - ROS_ERROR("Error setting config: %s", e.what()); - return EXIT_FAILURE; - } catch (const std::invalid_argument& ia) { - ROS_ERROR("Error setting config: %s", ia.what()); - return EXIT_FAILURE; - } - - // fetch metadata for client after setting configs - auto metadata = sensor::get_metadata(*cli); - if (metadata.empty()) { - ROS_ERROR("Failed to collect metadata"); - return EXIT_FAILURE; - } - - if (!cli) { - ROS_ERROR("Failed to initialize sensor at %s", hostname.c_str()); - return EXIT_FAILURE; - } - ROS_INFO("Sensor %s initialized successfully", hostname.c_str()); - - // write metadata file. If metadata_path is relative, will use cwd - // (usually ~/.ros) - if (!write_metadata(meta_file, metadata)) { - ROS_ERROR("Exiting because of failure to write metadata path"); - return EXIT_FAILURE; - } - - // populate sensor info - auto info = sensor::parse_metadata(metadata); - populate_metadata_defaults(info, sensor::MODE_UNSPEC); - published_metadata = to_string(info); - - ROS_INFO("Using lidar_mode: %s", sensor::to_string(info.mode).c_str()); - ROS_INFO("%s sn: %s firmware rev: %s", info.prod_line.c_str(), - info.sn.c_str(), info.fw_rev.c_str()); - - // publish packet messages from the sensor - return connection_loop(nh, *cli, info); - } -} diff --git a/ouster_ros/src/os_replay_nodelet.cpp b/ouster_ros/src/os_replay_nodelet.cpp new file mode 100644 index 00000000..32d8fb98 --- /dev/null +++ b/ouster_ros/src/os_replay_nodelet.cpp @@ -0,0 +1,45 @@ +/** + * Copyright (c) 2022, Ouster, Inc. + * All rights reserved. + * + * @file os_replay_nodelet.cpp + * @brief This nodelet mainly handles publishing saved metadata + * + */ + +#include + +#include "ouster_ros/os_client_base_nodelet.h" + +namespace sensor = ouster::sensor; + +namespace nodelets_os { + +class OusterReplay : public OusterClientBase { + private: + virtual void onInit() override { + auto& pnh = getPrivateNodeHandle(); + auto meta_file = pnh.param("metadata", std::string{}); + if (!meta_file.size()) { + NODELET_ERROR("Must specify metadata file in replay mode"); + throw std::runtime_error("metadata no specificed"); + } + + NODELET_INFO("Running in replay mode"); + + // populate info for config service + try { + info = sensor::metadata_from_json(meta_file); + cached_metadata = to_string(info); + display_lidar_info(info); + } catch (const std::runtime_error& e) { + NODELET_ERROR("Error when running in replay mode: %s", e.what()); + } + + OusterClientBase::onInit(); + } +}; + +} // namespace nodelets_os + +PLUGINLIB_EXPORT_CLASS(nodelets_os::OusterReplay, nodelet::Nodelet) \ No newline at end of file diff --git a/ouster_ros/src/os_sensor_nodelet.cpp b/ouster_ros/src/os_sensor_nodelet.cpp new file mode 100644 index 00000000..5f8fc5c7 --- /dev/null +++ b/ouster_ros/src/os_sensor_nodelet.cpp @@ -0,0 +1,387 @@ +/** + * Copyright (c) 2022, Ouster, Inc. + * All rights reserved. + * + * @file os_sensor_nodelet.cpp + * @brief A nodelet that connects to a live ouster sensor + */ + +#include + +#include +#include + +#include "ouster_ros/GetConfig.h" +#include "ouster_ros/PacketMsg.h" +#include "ouster_ros/SetConfig.h" +#include "ouster_ros/os_client_base_nodelet.h" + +namespace sensor = ouster::sensor; +using nonstd::optional; +using ouster_ros::GetConfig; +using ouster_ros::PacketMsg; +using ouster_ros::SetConfig; + +namespace nodelets_os { + +class OusterSensor : public OusterClientBase { + private: + virtual void onInit() override { + auto& pnh = getPrivateNodeHandle(); + hostname = pnh.param("sensor_hostname", std::string{}); + auto lidar_port = pnh.param("lidar_port", 0); + auto imu_port = pnh.param("imu_port", 0); + auto sensor_conf = create_sensor_config_rosparams(pnh); + configure_sensor(hostname, sensor_conf.first, sensor_conf.second); + sensor_client = create_client(hostname, lidar_port, imu_port); + update_config_and_metadata(*sensor_client); + save_metadata(pnh); + OusterClientBase::onInit(); + create_get_config_service(); + create_set_config_service(); + start_connection_loop(); + } + + bool update_config_and_metadata(sensor::client& cli) { + sensor::sensor_config config; + auto success = get_config(hostname, config); + if (!success) { + NODELET_ERROR("Failed to collect sensor config"); + cached_config.clear(); + cached_metadata.clear(); + return false; + } + + cached_config = to_string(config); + + try { + cached_metadata = sensor::get_metadata(cli); + } catch (const std::exception& e) { + NODELET_ERROR_STREAM( + "sensor::get_metadata exception: " << e.what()); + cached_metadata.clear(); + } + + if (cached_metadata.empty()) { + NODELET_ERROR("Failed to collect sensor metadata"); + return false; + } + + info = sensor::parse_metadata(cached_metadata); + // TODO: revist when *min_version* is changed + populate_metadata_defaults(info, sensor::MODE_UNSPEC); + display_lidar_info(info); + + return cached_config.size() > 0 && cached_metadata.size() > 0; + } + + void save_metadata(ros::NodeHandle& nh) { + auto meta_file = nh.param("metadata", std::string{}); + if (!meta_file.size()) { + meta_file = + hostname.substr(0, hostname.rfind('.')) + "-metadata.json"; + NODELET_WARN_STREAM( + "No metadata file was specified, using: " << meta_file); + } + + // write metadata file. If metadata_path is relative, will use cwd + // (usually ~/.ros) + if (!write_metadata(meta_file, cached_metadata)) { + NODELET_ERROR("Exiting because of failure to write metadata path"); + throw std::runtime_error("Failure to write metadata path"); + } + } + + void create_get_config_service() { + auto& nh = getNodeHandle(); + get_config_srv = + nh.advertiseService( + "get_config", + [this](GetConfig::Request&, GetConfig::Response& response) { + response.config = cached_config; + return cached_config.size() > 0; + }); + + NODELET_INFO("get_config service created"); + } + + void create_set_config_service() { + auto& nh = getNodeHandle(); + set_config_srv = + nh.advertiseService( + "set_config", [this](SetConfig::Request& request, + SetConfig::Response& response) { + sensor::sensor_config config; + response.config = ""; + auto success = + load_config_file(request.config_file, config); + if (!success) { + NODELET_ERROR_STREAM("Failed to load and parse file: " + << request.config_file); + return false; + } + + try { + configure_sensor(hostname, config, 0); + } catch (const std::runtime_error& e) { + return false; + } catch (const std::invalid_argument& ia) { + return false; + } + success = update_config_and_metadata(*sensor_client); + response.config = cached_config; + return success; + }); + + NODELET_INFO("set_config service created"); + } + + std::shared_ptr create_client(const std::string& hostname, + int lidar_port, + int imu_port) { + if (!hostname.size()) { + auto error_msg = "Must specify a sensor hostname"; + NODELET_ERROR_STREAM(error_msg); + throw std::runtime_error(error_msg); + } + + NODELET_INFO_STREAM("Starting sensor " << hostname + << " initialization..."); + + // use no-config version of init_client to allow for random ports + auto cli = + sensor::init_client(hostname, "", sensor::MODE_UNSPEC, + sensor::TIME_FROM_UNSPEC, lidar_port, imu_port); + + if (!cli) { + auto error_msg = "Failed to initialize client"; + NODELET_ERROR_STREAM(error_msg); + throw std::runtime_error(error_msg); + } + + return cli; + } + + std::pair create_sensor_config_rosparams( + ros::NodeHandle& nh) { + auto udp_dest = nh.param("udp_dest", std::string{}); + auto lidar_mode_arg = nh.param("lidar_mode", std::string{}); + auto timestamp_mode_arg = nh.param("timestamp_mode", std::string{}); + auto lidar_port = nh.param("lidar_port", 0); + auto imu_port = nh.param("imu_port", 0); + + std::string udp_profile_lidar_arg; + nh.param("udp_profile_lidar", udp_profile_lidar_arg, ""); + + optional udp_profile_lidar; + if (!udp_profile_lidar_arg.empty()) { + // set lidar profile from param + udp_profile_lidar = + sensor::udp_profile_lidar_of_string(udp_profile_lidar_arg); + if (!udp_profile_lidar) { + auto error_msg = + "Invalid udp profile lidar: " + udp_profile_lidar_arg; + NODELET_ERROR_STREAM(error_msg); + throw std::runtime_error(error_msg); + } + } + + // set lidar mode from param + sensor::lidar_mode lidar_mode = sensor::MODE_UNSPEC; + if (!lidar_mode_arg.empty()) { + lidar_mode = sensor::lidar_mode_of_string(lidar_mode_arg); + if (!lidar_mode) { + auto error_msg = "Invalid lidar mode: " + lidar_mode_arg; + NODELET_ERROR_STREAM(error_msg); + throw std::runtime_error(error_msg); + } + } + + // set timestamp mode from param + sensor::timestamp_mode timestamp_mode = sensor::TIME_FROM_UNSPEC; + if (!timestamp_mode_arg.empty()) { + // In case the option TIME_FROM_ROS_TIME is set then leave the + // sensor timestamp_mode unmodified + if (timestamp_mode_arg == "TIME_FROM_ROS_TIME") { + NODELET_INFO( + "TIME_FROM_ROS_TIME timestamp mode specified." + " IMU and pointcloud messages will use ros time"); + } else { + timestamp_mode = + sensor::timestamp_mode_of_string(timestamp_mode_arg); + if (!timestamp_mode) { + auto error_msg = + "Invalid timestamp mode: " + timestamp_mode_arg; + NODELET_ERROR_STREAM(error_msg); + throw std::runtime_error(error_msg); + } + } + } + + sensor::sensor_config config; + config.udp_port_imu = imu_port; + config.udp_port_lidar = lidar_port; + config.udp_profile_lidar = udp_profile_lidar; + config.operating_mode = sensor::OPERATING_NORMAL; + if (lidar_mode) config.ld_mode = lidar_mode; + if (timestamp_mode) config.ts_mode = timestamp_mode; + + uint8_t config_flags = 0; + + if (udp_dest.size()) { + NODELET_INFO("Will send UDP data to %s", udp_dest.c_str()); + config.udp_dest = udp_dest; + } else { + NODELET_INFO("Will use automatic UDP destination"); + config_flags |= ouster::sensor::CONFIG_UDP_DEST_AUTO; + } + + return std::make_pair(config, config_flags); + } + + void configure_sensor(const std::string& hostname, + const sensor::sensor_config& config, + int config_flags) { + try { + if (!set_config(hostname, config, config_flags)) { + auto err_msg = "Error connecting to sensor " + hostname; + NODELET_ERROR_STREAM(err_msg); + throw std::runtime_error(err_msg); + } + } catch (const std::runtime_error& e) { + NODELET_ERROR("Error setting config: %s", e.what()); + throw; + } catch (const std::invalid_argument& ia) { + NODELET_ERROR("Error setting config: %s", ia.what()); + throw; + } + + NODELET_WARN_STREAM("Sensor " << hostname + << " configured successfully"); + } + + bool load_config_file(const std::string& config_file, + sensor::sensor_config& out_config) { + std::ifstream ifs{}; + ifs.open(config_file); + if (ifs.fail()) return false; + std::stringstream buf; + buf << ifs.rdbuf(); + out_config = sensor::parse_config(buf.str()); + return true; + } + + private: + // fill in values that could not be parsed from metadata + void populate_metadata_defaults(sensor::sensor_info& info, + sensor::lidar_mode specified_lidar_mode) { + if (!info.name.size()) info.name = "UNKNOWN"; + + if (!info.sn.size()) info.sn = "UNKNOWN"; + + ouster::util::version v = ouster::util::version_of_string(info.fw_rev); + if (v == ouster::util::invalid_version) + NODELET_WARN( + "Unknown sensor firmware version; output may not be reliable"); + else if (v < sensor::min_version) + NODELET_WARN( + "Firmware < %s not supported; output may not be reliable", + to_string(sensor::min_version).c_str()); + + if (!info.mode) { + NODELET_WARN( + "Lidar mode not found in metadata; output may not be reliable"); + info.mode = specified_lidar_mode; + } + + if (!info.prod_line.size()) info.prod_line = "UNKNOWN"; + + if (info.beam_azimuth_angles.empty() || + info.beam_altitude_angles.empty()) { + NODELET_ERROR( + "Beam angles not found in metadata; using design values"); + info.beam_azimuth_angles = sensor::gen1_azimuth_angles; + info.beam_altitude_angles = sensor::gen1_altitude_angles; + } + } + + // try to write metadata file + bool write_metadata(const std::string& meta_file, + const std::string& metadata) { + std::ofstream ofs; + ofs.open(meta_file); + ofs << metadata << std::endl; + ofs.close(); + if (ofs) { + NODELET_INFO("Wrote metadata to %s", meta_file.c_str()); + } else { + NODELET_WARN( + "Failed to write metadata to %s; check that the path is valid. " + "If " + "you provided a relative path, please note that the working " + "directory of all ROS nodes is set by default to $ROS_HOME", + meta_file.c_str()); + return false; + } + return true; + } + + void start_connection_loop() { + auto& nh = getNodeHandle(); + + auto pf = sensor::get_format(info); + lidar_packet.buf.resize(pf.lidar_packet_size + 1); + imu_packet.buf.resize(pf.imu_packet_size + 1); + + lidar_packet_pub = nh.advertise("lidar_packets", 1280); + imu_packet_pub = nh.advertise("imu_packets", 100); + + timer_ = nh.createTimer( + ros::Duration(0), + boost::bind(&OusterSensor::timer_callback, this, _1), true); + } + + void connection_loop(sensor::client& cli, const sensor::sensor_info& info) { + auto pf = sensor::get_format(info); + + auto state = sensor::poll_client(cli); + if (state == sensor::EXIT) { + NODELET_INFO("poll_client: caught signal, exiting"); + return; + } + if (state & sensor::CLIENT_ERROR) { + NODELET_ERROR("poll_client: returned error"); + return; + } + if (state & sensor::LIDAR_DATA) { + if (sensor::read_lidar_packet(cli, lidar_packet.buf.data(), pf)) + lidar_packet_pub.publish(lidar_packet); + } + if (state & sensor::IMU_DATA) { + if (sensor::read_imu_packet(cli, imu_packet.buf.data(), pf)) + imu_packet_pub.publish(imu_packet); + } + } + + void timer_callback(const ros::TimerEvent&) { + connection_loop(*sensor_client, info); + timer_.stop(); + timer_.start(); + } + + private: + PacketMsg lidar_packet; + PacketMsg imu_packet; + ros::Publisher lidar_packet_pub; + ros::Publisher imu_packet_pub; + std::shared_ptr sensor_client; + ros::Timer timer_; + std::string hostname; + ros::ServiceServer get_config_srv; + ros::ServiceServer set_config_srv; + std::string cached_config; +}; + +} // namespace nodelets_os + +PLUGINLIB_EXPORT_CLASS(nodelets_os::OusterSensor, nodelet::Nodelet) diff --git a/ouster_ros/src/ros.cpp b/ouster_ros/src/ros.cpp index b263cc9f..a8396b2f 100644 --- a/ouster_ros/src/ros.cpp +++ b/ouster_ros/src/ros.cpp @@ -21,25 +21,27 @@ namespace ouster_ros { namespace sensor = ouster::sensor; -bool read_imu_packet(const sensor::client& cli, PacketMsg& m, +bool read_imu_packet(const sensor::client& cli, PacketMsg& pm, const sensor::packet_format& pf) { - m.buf.resize(pf.imu_packet_size + 1); - return read_imu_packet(cli, m.buf.data(), pf); + pm.buf.resize(pf.imu_packet_size + 1); + return read_imu_packet(cli, pm.buf.data(), pf); } -bool read_lidar_packet(const sensor::client& cli, PacketMsg& m, +bool read_lidar_packet(const sensor::client& cli, PacketMsg& pm, const sensor::packet_format& pf) { - m.buf.resize(pf.lidar_packet_size + 1); - return read_lidar_packet(cli, m.buf.data(), pf); + pm.buf.resize(pf.lidar_packet_size + 1); + return read_lidar_packet(cli, pm.buf.data(), pf); } -sensor_msgs::Imu packet_to_imu_msg(const PacketMsg& p, const std::string& frame, +sensor_msgs::Imu packet_to_imu_msg(const PacketMsg& pm, + const ros::Time& timestamp, + const std::string& frame, const sensor::packet_format& pf) { const double standard_g = 9.80665; sensor_msgs::Imu m; - const uint8_t* buf = p.buf.data(); + const uint8_t* buf = pm.buf.data(); - m.header.stamp.fromNSec(pf.imu_gyro_ts(buf)); + m.header.stamp = timestamp; m.header.frame_id = frame; m.orientation.x = 0; @@ -68,6 +70,14 @@ sensor_msgs::Imu packet_to_imu_msg(const PacketMsg& p, const std::string& frame, return m; } +sensor_msgs::Imu packet_to_imu_msg(const PacketMsg& pm, + const std::string& frame, + const sensor::packet_format& pf) { + ros::Time timestamp; + timestamp.fromNSec(pf.imu_gyro_ts(pm.buf.data())); + return packet_to_imu_msg(pm, timestamp, frame, pf); +} + struct read_and_cast { template void operator()(Eigen::Ref> field, @@ -129,11 +139,13 @@ void scan_to_cloud(const ouster::XYZLut& xyz_lut, suitable_return(sensor::ChanField::REFLECTIVITY, second), ls); auto points = ouster::cartesian(range, xyz_lut); + auto timestamp = ls.timestamp(); for (auto u = 0; u < ls.h; u++) { for (auto v = 0; v < ls.w; v++) { const auto xyz = points.row(u * ls.w + v); - const auto ts = (ls.header(v).timestamp - scan_ts).count(); + const auto ts = + (std::chrono::nanoseconds(timestamp[v]) - scan_ts).count(); cloud(v, u) = ouster_ros::Point{ {{static_cast(xyz(0)), static_cast(xyz(1)), static_cast(xyz(2)), 1.0f}}, @@ -147,15 +159,23 @@ void scan_to_cloud(const ouster::XYZLut& xyz_lut, } } -sensor_msgs::PointCloud2 cloud_to_cloud_msg(const Cloud& cloud, ns timestamp, +sensor_msgs::PointCloud2 cloud_to_cloud_msg(const Cloud& cloud, + const ros::Time& timestamp, const std::string& frame) { sensor_msgs::PointCloud2 msg{}; pcl::toROSMsg(cloud, msg); msg.header.frame_id = frame; - msg.header.stamp.fromNSec(timestamp.count()); + msg.header.stamp = timestamp; return msg; } +sensor_msgs::PointCloud2 cloud_to_cloud_msg(const Cloud& cloud, ns ts, + const std::string& frame) { + ros::Time timestamp; + timestamp.fromNSec(ts.count()); + return cloud_to_cloud_msg(cloud, timestamp, frame); +} + geometry_msgs::TransformStamped transform_to_tf_msg( const ouster::mat4d& mat, const std::string& frame, const std::string& child_frame) { diff --git a/ouster_ros/srv/GetConfig.srv b/ouster_ros/srv/GetConfig.srv new file mode 100644 index 00000000..5c667dbc --- /dev/null +++ b/ouster_ros/srv/GetConfig.srv @@ -0,0 +1,2 @@ +--- +string config diff --git a/ouster_ros/srv/OSConfigSrv.srv b/ouster_ros/srv/GetMetadata.srv similarity index 100% rename from ouster_ros/srv/OSConfigSrv.srv rename to ouster_ros/srv/GetMetadata.srv diff --git a/ouster_ros/srv/SetConfig.srv b/ouster_ros/srv/SetConfig.srv new file mode 100644 index 00000000..22ebc103 --- /dev/null +++ b/ouster_ros/srv/SetConfig.srv @@ -0,0 +1,3 @@ +string config_file +--- +string config \ No newline at end of file diff --git a/ouster_viz/CMakeLists.txt b/ouster_viz/CMakeLists.txt index 1ce66bb7..f7380ad3 100644 --- a/ouster_viz/CMakeLists.txt +++ b/ouster_viz/CMakeLists.txt @@ -1,21 +1,19 @@ -cmake_minimum_required(VERSION 3.1.0) - -# ==== Project Name ==== -project(ouster_viz) - # ==== Requirements ==== set(OpenGL_GL_PREFERENCE LEGACY) find_package(OpenGL REQUIRED) +# default to glad, if found. Note: this can be overridden from the command line find_package(glad QUIET) -if(glad_FOUND) - message(STATUS "Found glad ${glad_CONFIG}") +option(OUSTER_VIZ_USE_GLAD "Use GLAD instead of GLEW." ${glad_FOUND}) +if(OUSTER_VIZ_USE_GLAD) + message(STATUS "Configured GL loader: glad") + find_package(glad REQUIRED) set(GL_LOADER glad::glad) + add_definitions("-DOUSTER_VIZ_USE_GLAD") else() - message(STATUS "glad NOT found, falling back to GLEW") + message(STATUS "Configured GL loader: GLEW") find_package(GLEW REQUIRED) set(GL_LOADER GLEW::GLEW) - add_definitions("-DOUSTER_VIZ_GLEW") endif() find_package(glfw3 REQUIRED) diff --git a/ouster_viz/src/cloud.cpp b/ouster_viz/src/cloud.cpp index df40129e..ef015e97 100644 --- a/ouster_viz/src/cloud.cpp +++ b/ouster_viz/src/cloud.cpp @@ -46,7 +46,7 @@ struct CloudIds { }; bool GLCloud::initialized = false; -GLfloat GLCloud::program_id; +GLuint GLCloud::program_id; CloudIds GLCloud::cloud_ids; GLCloud::GLCloud(const Cloud& cloud) : point_size{cloud.point_size_} { diff --git a/ouster_viz/src/cloud.h b/ouster_viz/src/cloud.h index 14f2a527..d079ceb2 100644 --- a/ouster_viz/src/cloud.h +++ b/ouster_viz/src/cloud.h @@ -27,7 +27,7 @@ struct CloudIds; class GLCloud { // global gl state static bool initialized; - static GLfloat program_id; + static GLuint program_id; static CloudIds cloud_ids; private: diff --git a/ouster_viz/src/colormaps.h b/ouster_viz/src/colormaps.h index 81801fc1..8210dc59 100644 --- a/ouster_viz/src/colormaps.h +++ b/ouster_viz/src/colormaps.h @@ -25,521 +25,521 @@ inline float** genPalette(const int n, const float from[3], const float to[3]) { // https://daniel.lawrence.lu/public/colortransform/#0_2423_964_352_6_2624_1000_513_11_3248_1000_617_15_415_1000_774 const size_t spezia_n = 256; const float spezia_palette[spezia_n][3] = { - {0.04890922165917825, 0.34265700288230266, 0.5139042200196196}, - {0.04895672077739804, 0.34399228711079705, 0.5173325088859984}, - {0.04899969158023907, 0.34532432182766976, 0.5207851330769154}, - {0.049038068929181285, 0.34665300013643424, 0.5242624999557384}, - {0.0490717860366443, 0.3479782119131098, 0.5277650273921529}, - {0.04910077440233592, 0.34929984367863964, 0.5312931441090918}, - {0.04912496374647964, 0.35061777846523556, 0.5348472900437968}, - {0.049144281939685876, 0.35193189567631167, 0.5384279167237124}, - {0.04915865492929047, 0.3532420709396423, 0.5420354876579142}, - {0.04916800666192803, 0.3545481759533582, 0.5456704787448663}, - {0.04917225900211732, 0.3558500783243678, 0.5493333786972924}, - {0.04917133164659893, 0.35714764139876426, 0.553024689485032}, - {0.0491651420341628, 0.35844072408375016, 0.5567449267967906}, - {0.049153605250673076, 0.35972918066057785, 0.5604946205217287}, - {0.04913663392897654, 0.36101286058797066, 0.5642743152519267}, - {0.04911413814335756, 0.36229160829545354, 0.5680845708067875}, - {0.04908602529819959, 0.36356526296598163, 0.5719259627805287}, - {0.04905220001042406, 0.36483365830721187, 0.5757990831139734}, - {0.04901256398533129, 0.36609662231071893, 0.5797045406919258}, - {0.04896701588534969, 0.36735397699840217, 0.5836429619674972}, - {0.04891545119124254, 0.36860553815528246, 0.5876149916148347}, - {0.04885776205520153, 0.36985111504782353, 0.5916212932117864}, - {0.048793837145294165, 0.371090510126853, 0.5956625499541581}, - {0.048723561480604215, 0.37232351871408936, 0.5997394654032839}, - {0.04864681625641982, 0.37354992867120285, 0.6038527642687842}, - {0.0485634786587359, 0.37476952005026626, 0.6080031932284756}, - {0.04847342166723854, 0.3759820647243526, 0.6121915217875443}, - {0.04837651384597603, 0.37718732599695254, 0.6164185431792271}, - {0.04827261912068898, 0.3783850581887729, 0.6206850753093874}, - {0.04816159654185025, 0.37957500620037093, 0.6249919617475522}, - {0.04804330003224206, 0.38075690504895116, 0.6293400727671268}, - {0.047917578117875524, 0.3819304793775204, 0.633730306437712}, - {0.04778427364089425, 0.38309544293445374, 0.6381635897726399}, - {0.04764322345301101, 0.38425149802135766, 0.6426408799350484}, - {0.04749425808786458, 0.385398334906948, 0.6471631655060938}, - {0.04733720141054259, 0.3865356312044689, 0.6517314678190856}, - {0.04717187024231324, 0.3876630512099673, 0.6563468423636755}, - {0.046998073958454976, 0.38878024519851034, 0.6610103802644818}, - {0.046815614056824016, 0.3898868486751851, 0.6657232098388559}, - {0.04662428369457814, 0.3909824815774357, 0.6704864982388766}, - {0.04642386719018477, 0.39206674742499825, 0.6753014531830023}, - {0.04621413948754389, 0.39313923241335524, 0.6801693247832367}, - {0.045994865578738504, 0.3941995044462622, 0.6850914074741193}, - {0.04576579988147745, 0.39524711210249736, 0.6900690420503143}, - {0.04552668556693947, 0.3962815835315315, 0.6951036178201221}, - {0.04527725383318241, 0.39730242527232407, 0.7001965748827989}, - {0.04501722311872807, 0.39830912098889804, 0.7053494065382041}, - {0.04474629825033485, 0.39930113011574186, 0.7105636618379779}, - {0.044464169518219306, 0.4002778864054065, 0.7158409482881979}, - {0.044170511671191286, 0.4012387963699213, 0.7211829347142875}, - {0.04386498282321687, 0.4021832376068135, 0.7265913542998228}, - {0.04354722326188234, 0.4031105569995846, 0.7320680078119023}, - {0.04321685414797862, 0.40402006878146585, 0.7376147670267773}, - {0.0428734760940282, 0.40491105245010933, 0.743233578370643}, - {0.042516667607970175, 0.40578275051957646, 0.748926466791789}, - {0.04214598338630927, 0.4066343660945334, 0.7546955398817109}, - {0.04176095243886018, 0.40746506024993384, 0.7605429922643745}, - {0.04136107602475044, 0.40827394919762916, 0.766471110274553}, - {0.04094582537627162, 0.4090601012192915, 0.7724822769480404}, - {0.04051463918382638, 0.40982253334270374, 0.7785789773486957}, - {0.040061502782456945, 0.4105602077358398, 0.7847638042595603}, - {0.03959294089889664, 0.41127202779018696, 0.7910394642679004}, - {0.039109793546916495, 0.4119568338613871, 0.7974087842769024}, - {0.03861172210191932, 0.41261339863144436, 0.803874718479878}, - {0.0380983735795864, 0.4132404220523802, 0.8104403558364525}, - {0.03756937968562651, 0.4138365258262561, 0.8171089280940507}, - {0.03702435578736771, 0.4144002473707861, 0.8238838184024792}, - {0.0364628997996382, 0.4149300332132621, 0.8307685705742502}, - {0.03588459097638143, 0.4154242317480496, 0.8377668990487521}, - {0.035288988598694025, 0.4158810852842974, 0.844882699624589}, - {0.03467563054866628, 0.4162987213006144, 0.8521200610312002}, - {0.03404403175731731, 0.41667514281199364, 0.8594832774186676}, - {0.033393682513460185, 0.41700821774098445, 0.8669768618532854}, - {0.03272404661867004, 0.41729566716967786, 0.8746055609162682}, - {0.032034559371859575, 0.4175350523310705, 0.8823743705140761}, - {0.03132462536474723, 0.41772376017735885, 0.8902885530212784}, - {0.03059361606719027, 0.417858987338036, 0.8983536558911435}, - {0.029840867178669222, 0.41793772225168413, 0.9065755318852089}, - {0.02906567571902483, 0.4179567252211435, 0.9149603610913213}, - {0.028267296828018075, 0.41791250610119823, 0.9235146749206897}, - {0.027444940239127507, 0.41780129927982523, 0.9322453822980893}, - {0.026597766388240202, 0.4176190355565933, 0.9411597982868389}, - {0.02572488211232861, 0.41736131045306674, 0.9502656754213602}, - {0.02482533588680886, 0.41702334840740857, 0.9595712380560552}, - {0.023898112542860842, 0.416599962205498, 0.9690852200808441}, - {0.02294212739712791, 0.41608550687982504, 0.9788169064013666}, - {0.02195621971619119, 0.4154738271597193, 0.9887761786374855}, - {0.03533572637548167, 0.4150344767837667, 0.9966419438918287}, - {0.08206748636661013, 0.4154760610454022, 0.996875442497312}, - {0.1131664468320158, 0.4159292422424467, 0.9971067037505105}, - {0.1377759789309851, 0.4163940123475041, 0.9973357493609963}, - {0.1586260932452447, 0.4168703621191211, 0.9975626007042689}, - {0.17695881259992585, 0.41735828111703227, 0.997787278826484}, - {0.19346029551091778, 0.4178577577177723, 0.9980098044491156}, - {0.2085556849234767, 0.4183687791306285, 0.9982301979735458}, - {0.22252938052310162, 0.41889133141394447, 0.9984484794855942}, - {0.2355824089832244, 0.4194253994917421, 0.9986646687599702}, - {0.24786290560296725, 0.4199709671706614, 0.9988787852646682}, - {0.25948364869956886, 0.42052801715720073, 0.9990908481652964}, - {0.2705327829044692, 0.42109653107524325, 0.9993008763293371}, - {0.2810807045979947, 0.4216764894838623, 0.9995088883303488}, - {0.2911846624744039, 0.4222678718953844, 0.9997149024521047}, - {0.30089193496804306, 0.4228706567937021, 0.9999189366926701}, - {0.3199598560384707, 0.4211529467871777, 1.0000000000000044}, - {0.3436114893370144, 0.4178742172053897, 1.0000000000000047}, - {0.36539676089694495, 0.41458308629177515, 1.0000000000000044}, - {0.3856661632570949, 0.41127775518053283, 1.0000000000000042}, - {0.404675301565696, 0.407956362084171, 1.0000000000000044}, - {0.4226172861700883, 0.4046169767859018, 1.0000000000000047}, - {0.43964219386021874, 0.40125759469274436, 1.0000000000000047}, - {0.45586938841351193, 0.3978761303980185, 1.0000000000000047}, - {0.47139565849043324, 0.39447041069519134, 1.0000000000000047}, - {0.4863007849418988, 0.3910381669772773, 1.0000000000000047}, - {0.5006514638539757, 0.3875770269469873, 1.0000000000000044}, - {0.5145041416968924, 0.3840845055522841, 1.0000000000000047}, - {0.5279071095300848, 0.3805579950497078, 1.0000000000000047}, - {0.5409020797263486, 0.3769947540834305, 1.0000000000000044}, - {0.5535253932438766, 0.3733918956509583, 1.0000000000000044}, - {0.5658089579546876, 0.3697463738064324, 1.0000000000000042}, - {0.577780987780821, 0.366054968928604, 1.0000000000000049}, - {0.589466591997403, 0.3623142713523205, 1.0000000000000047}, - {0.6008882502481963, 0.35852066312849035, 1.0000000000000044}, - {0.6120661992793963, 0.3546702976368881, 1.0000000000000047}, - {0.6230187506929341, 0.35075907672718176, 1.0000000000000047}, - {0.6337625542333337, 0.34678262500419443, 1.0000000000000047}, - {0.6443128176539651, 0.3427362608011279, 1.0000000000000044}, - {0.6546834916623888, 0.33861496329592544, 1.0000000000000047}, - {0.664887426552217, 0.3344133351169368, 1.0000000000000044}, - {0.6749365057066918, 0.3301255596489445, 1.0000000000000047}, - {0.6848417600790246, 0.32574535208217403, 1.0000000000000047}, - {0.6946134669261637, 0.32126590303548275, 1.0000000000000049}, - {0.7042612354316643, 0.31667981331755896, 1.0000000000000047}, - {0.7137940813531695, 0.3119790180493533, 1.0000000000000049}, - {0.7232204924365964, 0.3071546979334297, 1.0000000000000049}, - {0.7325484860275505, 0.30219717488892517, 1.0000000000000047}, - {0.7417856600618409, 0.2970957885292609, 1.000000000000005}, - {0.7509392384175178, 0.2918387489798506, 1.0000000000000047}, - {0.760016111449703, 0.28641296022435003, 1.0000000000000047}, - {0.7690228723986646, 0.2808038063993306, 1.0000000000000049}, - {0.7779658502549104, 0.27499489103633235, 1.0000000000000049}, - {0.7868511395774846, 0.2689677158905533, 1.0000000000000047}, - {0.7956846276897148, 0.26270128126132847, 1.0000000000000047}, - {0.804472019617065, 0.2561715829275765, 1.0000000000000047}, - {0.8132188610824966, 0.2493509709254887, 1.0000000000000047}, - {0.8219305598337341, 0.24220732066040862, 1.0000000000000049}, - {0.8306124055427538, 0.23470294440057987, 1.0000000000000049}, - {0.8392695884894237, 0.2267931361345682, 1.0000000000000047}, - {0.847907217217596, 0.21842418639150069, 1.0000000000000047}, - {0.8565303353323375, 0.20953060994411976, 1.0000000000000049}, - {0.8651439375907393, 0.20003116767718654, 1.0000000000000049}, - {0.8737529854254381, 0.18982297245453064, 1.0000000000000049}, - {0.8823624220291222, 0.17877241522237444, 1.0000000000000047}, - {0.8909771871196978, 0.1667005280966983, 1.0000000000000047}, - {0.8996022314990386, 0.15335795616479617, 1.000000000000005}, - {0.9082425315133318, 0.13837882372526109, 1.0000000000000049}, - {0.9169031035195819, 0.12118667725012405, 1.0000000000000049}, - {0.9255890184609986, 0.10077304980525353, 1.0000000000000047}, - {0.9343054166534386, 0.07504334998300113, 1.0000000000000049}, - {0.9430575228859241, 0.03781952178921804, 1.000000000000005}, - {0.9509350420238839, 1.4218570765223148e-13, 0.9989984483716071}, - {0.9554497353124459, 1.4191675612451605e-13, 0.9943640499109371}, - {0.9599176427714787, 1.4433731987395504e-13, 0.9897799632511853}, - {0.9643412154073002, 1.4245465917994694e-13, 0.9852425190239346}, - {0.9687227616942858, 1.4191675612451605e-13, 0.9807481714229297}, - {0.9730644583865243, 1.411995520506082e-13, 0.9762934885028384}, - {0.9773683603724937, 1.3931689135660008e-13, 0.9718751430792824}, - {0.9816364096714153, 1.3886863881040766e-13, 0.9674899041721569}, - {0.9858704436584534, 1.4039269746746187e-13, 0.9631346289394122}, - {0.9900722025959202, 1.4397871783700112e-13, 0.9588062550529955}, - {0.9942433365389557, 1.4155815408756212e-13, 0.954501793472642}, - {0.9983854116765075, 1.3752388117183045e-13, 0.9502183215767478}, - {0.9999999999999819, 0.02804423714351181, 0.9437140548413381}, - {0.9999999999999823, 0.0675265531658979, 0.9359017685954015}, - {0.9999999999999826, 0.09447578037166751, 0.9282451825736049}, - {0.9999999999999823, 0.11567880450339993, 0.920737795368809}, - {0.9999999999999826, 0.13352190503381375, 0.9133734552831144}, - {0.9999999999999823, 0.1491028314594674, 0.906146335428585}, - {0.9999999999999826, 0.16303259275115084, 0.8990509109121838}, - {0.9999999999999826, 0.17569199214531872, 0.8920819378992011}, - {0.9999999999999826, 0.18733702217610845, 0.8852344343724449}, - {0.9999999999999826, 0.19814940356609517, 0.8785036624245576}, - {0.9999999999999823, 0.20826355122506324, 0.8718851119384158}, - {0.9999999999999823, 0.21778214249596284, 0.8653744855260821}, - {0.9999999999999826, 0.22678566871532468, 0.8589676846103573}, - {0.9999999999999823, 0.2353385863611125, 0.8526607965450058}, - {0.9999999999999828, 0.24349343831907827, 0.8464500826803465}, - {0.9999999999999826, 0.2512937077092952, 0.840331967290248}, - {0.9999999999999826, 0.2587758499993201, 0.8343030272849384}, - {0.999999999999983, 0.26739099502162367, 0.8275538904243963}, - {0.999999999999983, 0.2793555475103376, 0.8187524096848618}, - {0.9999999999999828, 0.29067538241472596, 0.810154074771914}, - {0.999999999999983, 0.3014349177286362, 0.8017491111724352}, - {0.9999999999999826, 0.31170258039783083, 0.7935283442712853}, - {0.9999999999999826, 0.3215347049761315, 0.7854831467895685}, - {0.9999999999999826, 0.3309782925632311, 0.7776053911816436}, - {0.9999999999999826, 0.3400730122474594, 0.7698874064041857}, - {0.9999999999999826, 0.34885268450644075, 0.7623219385454285}, - {0.999999999999983, 0.35734640143399626, 0.7549021148665397}, - {0.9999999999999826, 0.3655793867737775, 0.7476214108616114}, - {0.9999999999999826, 0.3735736659274856, 0.7404736199894286}, - {0.9999999999999828, 0.381348594792351, 0.7334528257702123}, - {0.9999999999999826, 0.38892128210540905, 0.7265533759748873}, - {0.9999999999999823, 0.3963069303390571, 0.7197698586639263}, - {0.9999999999999823, 0.4035191135203492, 0.7130970798581467}, - {0.9999999999999823, 0.410570005644612, 0.7065300426455539}, - {0.9999999999999821, 0.4174705699878856, 0.700063927546916}, - {0.9999999999999819, 0.4242307171780247, 0.6936940739785828}, - {0.9999999999999821, 0.4308594380852102, 0.6874159626644994}, - {0.9999999999999821, 0.4373649162525338, 0.6812251988606219}, - {0.9999999999999819, 0.44375462357781925, 0.6751174962642902}, - {0.9999999999999819, 0.4500354021895003, 0.6690886614886871}, - {0.9999999999999821, 0.45621353486890187, 0.6631345789884755}, - {0.9999999999999817, 0.4622948059133914, 0.657251196327135}, - {0.9999999999999817, 0.4682845539768576, 0.6514345096795133}, - {0.9999999999999817, 0.474187718141824, 0.645680549464667}, - {0.9999999999999817, 0.4800088782535285, 0.6399853660042518}, - {0.9999999999999815, 0.4857522903672667, 0.6343450151004509}, - {0.9999999999999815, 0.4914219180162633, 0.6287555434246979}, - {0.9999999999999815, 0.497021459890778, 0.6232129736041581}, - {0.9999999999999815, 0.5025543744242497, 0.6177132888869281}, - {0.9999999999999815, 0.5080239017046412, 0.6122524172590773}, - {0.999999999999981, 0.5134330830652836, 0.606826214876734}, - {0.9999999999999808, 0.518784778656747, 0.6014304486641499}, - {0.9999999999999808, 0.5240816832574693, 0.5960607779137368}, - {0.9999999999999806, 0.5293263405443853, 0.5907127347060119}, - {0.9999999999999806, 0.5345211560142691, 0.5853817029456958}, - {0.9999999999999808, 0.5396684087209026, 0.580062895784249}, - {0.9999999999999808, 0.5447702619716198, 0.5747513311680923}, - {0.9999999999999806, 0.5498287731085955, 0.5694418052146554}, - {0.9999999999999803, 0.5548459024848833, 0.5641288630740176}, - {0.9999999999999801, 0.5598235217321937, 0.5588067668806895}, - {0.9999999999999799, 0.5647634214064047, 0.5534694603362047}, - {0.9999999999999799, 0.569667318087479, 0.5481105293861371}, - {0.9999999999999801, 0.5745368610026079, 0.5427231583620321}, - {0.9999999999999797, 0.5793736382348097, 0.5373000808456486}, - {0.9999999999999797, 0.5841791825736894, 0.5318335243749407}, - {0.9999999999999797, 0.58895497706055, 0.5263151479421893}, - {0.9999999999999795, 0.5937024602763533, 0.5207359710263567}, - {0.9999999999999795, 0.5984230314181602, 0.5150862926436902}, - {0.9999999999999792, 0.6031180552074987, 0.5093555985787912}, - {0.9999999999999792, 0.607788866672662, 0.5035324545546109}, - {0.999999999999979, 0.6124367758461117, 0.4976043825895365}, - {0.999999999999979, 0.6170630724180334, 0.4915577171399405}, - {0.9999999999999788, 0.6216690303876014, 0.48537743679248463}, - {0.9999999999999788, 0.6262559127547657, 0.4790469661903673}, - {0.9999999999999784, 0.6308249762973255, 0.4725479414659382}, - {0.9999999999999786, 0.6353774764808859, 0.46585993058805514}, - {0.9999999999999784, 0.6399146725529954, 0.45896009754439654}, - {0.9999999999999784, 0.644437832877538, 0.45182279591800384}, - {0.9999999999999781, 0.6489482405714118, 0.4444190728188997}, - {0.9999999999999779, 0.6534471995128909, 0.4367160577509657}, - {0.9999999999999779, 0.6579360408000906, 0.4286762020035964}, - {0.9999999999999779, 0.6624161297489367, 0.42025632127341656}, - {0.9999999999999777, 0.6668888735333959, 0.41140637540952824}, - {0.9999999999999777, 0.6713557295869282, 0.40206789113388525}, - {0.9999999999999775, 0.6758182149038043, 0.3921718908087272}}; + {0.04890922165917825f, 0.34265700288230266f, 0.5139042200196196f}, + {0.04895672077739804f, 0.34399228711079705f, 0.5173325088859984f}, + {0.04899969158023907f, 0.34532432182766976f, 0.5207851330769154f}, + {0.049038068929181285f, 0.34665300013643424f, 0.5242624999557384f}, + {0.0490717860366443f, 0.3479782119131098f, 0.5277650273921529f}, + {0.04910077440233592f, 0.34929984367863964f, 0.5312931441090918f}, + {0.04912496374647964f, 0.35061777846523556f, 0.5348472900437968f}, + {0.049144281939685876f, 0.35193189567631167f, 0.5384279167237124f}, + {0.04915865492929047f, 0.3532420709396423f, 0.5420354876579142f}, + {0.04916800666192803f, 0.3545481759533582f, 0.5456704787448663f}, + {0.04917225900211732f, 0.3558500783243678f, 0.5493333786972924f}, + {0.04917133164659893f, 0.35714764139876426f, 0.553024689485032f}, + {0.0491651420341628f, 0.35844072408375016f, 0.5567449267967906f}, + {0.049153605250673076f, 0.35972918066057785f, 0.5604946205217287f}, + {0.04913663392897654f, 0.36101286058797066f, 0.5642743152519267f}, + {0.04911413814335756f, 0.36229160829545354f, 0.5680845708067875f}, + {0.04908602529819959f, 0.36356526296598163f, 0.5719259627805287f}, + {0.04905220001042406f, 0.36483365830721187f, 0.5757990831139734f}, + {0.04901256398533129f, 0.36609662231071893f, 0.5797045406919258f}, + {0.04896701588534969f, 0.36735397699840217f, 0.5836429619674972f}, + {0.04891545119124254f, 0.36860553815528246f, 0.5876149916148347f}, + {0.04885776205520153f, 0.36985111504782353f, 0.5916212932117864f}, + {0.048793837145294165f, 0.371090510126853f, 0.5956625499541581f}, + {0.048723561480604215f, 0.37232351871408936f, 0.5997394654032839f}, + {0.04864681625641982f, 0.37354992867120285f, 0.6038527642687842f}, + {0.0485634786587359f, 0.37476952005026626f, 0.6080031932284756f}, + {0.04847342166723854f, 0.3759820647243526f, 0.6121915217875443f}, + {0.04837651384597603f, 0.37718732599695254f, 0.6164185431792271f}, + {0.04827261912068898f, 0.3783850581887729f, 0.6206850753093874f}, + {0.04816159654185025f, 0.37957500620037093f, 0.6249919617475522f}, + {0.04804330003224206f, 0.38075690504895116f, 0.6293400727671268f}, + {0.047917578117875524f, 0.3819304793775204f, 0.633730306437712f}, + {0.04778427364089425f, 0.38309544293445374f, 0.6381635897726399f}, + {0.04764322345301101f, 0.38425149802135766f, 0.6426408799350484f}, + {0.04749425808786458f, 0.385398334906948f, 0.6471631655060938f}, + {0.04733720141054259f, 0.3865356312044689f, 0.6517314678190856f}, + {0.04717187024231324f, 0.3876630512099673f, 0.6563468423636755f}, + {0.046998073958454976f, 0.38878024519851034f, 0.6610103802644818f}, + {0.046815614056824016f, 0.3898868486751851f, 0.6657232098388559f}, + {0.04662428369457814f, 0.3909824815774357f, 0.6704864982388766f}, + {0.04642386719018477f, 0.39206674742499825f, 0.6753014531830023f}, + {0.04621413948754389f, 0.39313923241335524f, 0.6801693247832367f}, + {0.045994865578738504f, 0.3941995044462622f, 0.6850914074741193f}, + {0.04576579988147745f, 0.39524711210249736f, 0.6900690420503143f}, + {0.04552668556693947f, 0.3962815835315315f, 0.6951036178201221f}, + {0.04527725383318241f, 0.39730242527232407f, 0.7001965748827989f}, + {0.04501722311872807f, 0.39830912098889804f, 0.7053494065382041f}, + {0.04474629825033485f, 0.39930113011574186f, 0.7105636618379779f}, + {0.044464169518219306f, 0.4002778864054065f, 0.7158409482881979f}, + {0.044170511671191286f, 0.4012387963699213f, 0.7211829347142875f}, + {0.04386498282321687f, 0.4021832376068135f, 0.7265913542998228f}, + {0.04354722326188234f, 0.4031105569995846f, 0.7320680078119023f}, + {0.04321685414797862f, 0.40402006878146585f, 0.7376147670267773f}, + {0.0428734760940282f, 0.40491105245010933f, 0.743233578370643f}, + {0.042516667607970175f, 0.40578275051957646f, 0.748926466791789f}, + {0.04214598338630927f, 0.4066343660945334f, 0.7546955398817109f}, + {0.04176095243886018f, 0.40746506024993384f, 0.7605429922643745f}, + {0.04136107602475044f, 0.40827394919762916f, 0.766471110274553f}, + {0.04094582537627162f, 0.4090601012192915f, 0.7724822769480404f}, + {0.04051463918382638f, 0.40982253334270374f, 0.7785789773486957f}, + {0.040061502782456945f, 0.4105602077358398f, 0.7847638042595603f}, + {0.03959294089889664f, 0.41127202779018696f, 0.7910394642679004f}, + {0.039109793546916495f, 0.4119568338613871f, 0.7974087842769024f}, + {0.03861172210191932f, 0.41261339863144436f, 0.803874718479878f}, + {0.0380983735795864f, 0.4132404220523802f, 0.8104403558364525f}, + {0.03756937968562651f, 0.4138365258262561f, 0.8171089280940507f}, + {0.03702435578736771f, 0.4144002473707861f, 0.8238838184024792f}, + {0.0364628997996382f, 0.4149300332132621f, 0.8307685705742502f}, + {0.03588459097638143f, 0.4154242317480496f, 0.8377668990487521f}, + {0.035288988598694025f, 0.4158810852842974f, 0.844882699624589f}, + {0.03467563054866628f, 0.4162987213006144f, 0.8521200610312002f}, + {0.03404403175731731f, 0.41667514281199364f, 0.8594832774186676f}, + {0.033393682513460185f, 0.41700821774098445f, 0.8669768618532854f}, + {0.03272404661867004f, 0.41729566716967786f, 0.8746055609162682f}, + {0.032034559371859575f, 0.4175350523310705f, 0.8823743705140761f}, + {0.03132462536474723f, 0.41772376017735885f, 0.8902885530212784f}, + {0.03059361606719027f, 0.417858987338036f, 0.8983536558911435f}, + {0.029840867178669222f, 0.41793772225168413f, 0.9065755318852089f}, + {0.02906567571902483f, 0.4179567252211435f, 0.9149603610913213f}, + {0.028267296828018075f, 0.41791250610119823f, 0.9235146749206897f}, + {0.027444940239127507f, 0.41780129927982523f, 0.9322453822980893f}, + {0.026597766388240202f, 0.4176190355565933f, 0.9411597982868389f}, + {0.02572488211232861f, 0.41736131045306674f, 0.9502656754213602f}, + {0.02482533588680886f, 0.41702334840740857f, 0.9595712380560552f}, + {0.023898112542860842f, 0.416599962205498f, 0.9690852200808441f}, + {0.02294212739712791f, 0.41608550687982504f, 0.9788169064013666f}, + {0.02195621971619119f, 0.4154738271597193f, 0.9887761786374855f}, + {0.03533572637548167f, 0.4150344767837667f, 0.9966419438918287f}, + {0.08206748636661013f, 0.4154760610454022f, 0.996875442497312f}, + {0.1131664468320158f, 0.4159292422424467f, 0.9971067037505105f}, + {0.1377759789309851f, 0.4163940123475041f, 0.9973357493609963f}, + {0.1586260932452447f, 0.4168703621191211f, 0.9975626007042689f}, + {0.17695881259992585f, 0.41735828111703227f, 0.997787278826484f}, + {0.19346029551091778f, 0.4178577577177723f, 0.9980098044491156f}, + {0.2085556849234767f, 0.4183687791306285f, 0.9982301979735458f}, + {0.22252938052310162f, 0.41889133141394447f, 0.9984484794855942f}, + {0.2355824089832244f, 0.4194253994917421f, 0.9986646687599702f}, + {0.24786290560296725f, 0.4199709671706614f, 0.9988787852646682f}, + {0.25948364869956886f, 0.42052801715720073f, 0.9990908481652964f}, + {0.2705327829044692f, 0.42109653107524325f, 0.9993008763293371f}, + {0.2810807045979947f, 0.4216764894838623f, 0.9995088883303488f}, + {0.2911846624744039f, 0.4222678718953844f, 0.9997149024521047f}, + {0.30089193496804306f, 0.4228706567937021f, 0.9999189366926701f}, + {0.3199598560384707f, 0.4211529467871777f, 1.0000000000000044f}, + {0.3436114893370144f, 0.4178742172053897f, 1.0000000000000047f}, + {0.36539676089694495f, 0.41458308629177515f, 1.0000000000000044f}, + {0.3856661632570949f, 0.41127775518053283f, 1.0000000000000042f}, + {0.404675301565696f, 0.407956362084171f, 1.0000000000000044f}, + {0.4226172861700883f, 0.4046169767859018f, 1.0000000000000047f}, + {0.43964219386021874f, 0.40125759469274436f, 1.0000000000000047f}, + {0.45586938841351193f, 0.3978761303980185f, 1.0000000000000047f}, + {0.47139565849043324f, 0.39447041069519134f, 1.0000000000000047f}, + {0.4863007849418988f, 0.3910381669772773f, 1.0000000000000047f}, + {0.5006514638539757f, 0.3875770269469873f, 1.0000000000000044f}, + {0.5145041416968924f, 0.3840845055522841f, 1.0000000000000047f}, + {0.5279071095300848f, 0.3805579950497078f, 1.0000000000000047f}, + {0.5409020797263486f, 0.3769947540834305f, 1.0000000000000044f}, + {0.5535253932438766f, 0.3733918956509583f, 1.0000000000000044f}, + {0.5658089579546876f, 0.3697463738064324f, 1.0000000000000042f}, + {0.577780987780821f, 0.366054968928604f, 1.0000000000000049f}, + {0.589466591997403f, 0.3623142713523205f, 1.0000000000000047f}, + {0.6008882502481963f, 0.35852066312849035f, 1.0000000000000044f}, + {0.6120661992793963f, 0.3546702976368881f, 1.0000000000000047f}, + {0.6230187506929341f, 0.35075907672718176f, 1.0000000000000047f}, + {0.6337625542333337f, 0.34678262500419443f, 1.0000000000000047f}, + {0.6443128176539651f, 0.3427362608011279f, 1.0000000000000044f}, + {0.6546834916623888f, 0.33861496329592544f, 1.0000000000000047f}, + {0.664887426552217f, 0.3344133351169368f, 1.0000000000000044f}, + {0.6749365057066918f, 0.3301255596489445f, 1.0000000000000047f}, + {0.6848417600790246f, 0.32574535208217403f, 1.0000000000000047f}, + {0.6946134669261637f, 0.32126590303548275f, 1.0000000000000049f}, + {0.7042612354316643f, 0.31667981331755896f, 1.0000000000000047f}, + {0.7137940813531695f, 0.3119790180493533f, 1.0000000000000049f}, + {0.7232204924365964f, 0.3071546979334297f, 1.0000000000000049f}, + {0.7325484860275505f, 0.30219717488892517f, 1.0000000000000047f}, + {0.7417856600618409f, 0.2970957885292609f, 1.000000000000005f}, + {0.7509392384175178f, 0.2918387489798506f, 1.0000000000000047f}, + {0.760016111449703f, 0.28641296022435003f, 1.0000000000000047f}, + {0.7690228723986646f, 0.2808038063993306f, 1.0000000000000049f}, + {0.7779658502549104f, 0.27499489103633235f, 1.0000000000000049f}, + {0.7868511395774846f, 0.2689677158905533f, 1.0000000000000047f}, + {0.7956846276897148f, 0.26270128126132847f, 1.0000000000000047f}, + {0.804472019617065f, 0.2561715829275765f, 1.0000000000000047f}, + {0.8132188610824966f, 0.2493509709254887f, 1.0000000000000047f}, + {0.8219305598337341f, 0.24220732066040862f, 1.0000000000000049f}, + {0.8306124055427538f, 0.23470294440057987f, 1.0000000000000049f}, + {0.8392695884894237f, 0.2267931361345682f, 1.0000000000000047f}, + {0.847907217217596f, 0.21842418639150069f, 1.0000000000000047f}, + {0.8565303353323375f, 0.20953060994411976f, 1.0000000000000049f}, + {0.8651439375907393f, 0.20003116767718654f, 1.0000000000000049f}, + {0.8737529854254381f, 0.18982297245453064f, 1.0000000000000049f}, + {0.8823624220291222f, 0.17877241522237444f, 1.0000000000000047f}, + {0.8909771871196978f, 0.1667005280966983f, 1.0000000000000047f}, + {0.8996022314990386f, 0.15335795616479617f, 1.000000000000005f}, + {0.9082425315133318f, 0.13837882372526109f, 1.0000000000000049f}, + {0.9169031035195819f, 0.12118667725012405f, 1.0000000000000049f}, + {0.9255890184609986f, 0.10077304980525353f, 1.0000000000000047f}, + {0.9343054166534386f, 0.07504334998300113f, 1.0000000000000049f}, + {0.9430575228859241f, 0.03781952178921804f, 1.000000000000005f}, + {0.9509350420238839f, 1.4218570765223148e-13f, 0.9989984483716071f}, + {0.9554497353124459f, 1.4191675612451605e-13f, 0.9943640499109371f}, + {0.9599176427714787f, 1.4433731987395504e-13f, 0.9897799632511853f}, + {0.9643412154073002f, 1.4245465917994694e-13f, 0.9852425190239346f}, + {0.9687227616942858f, 1.4191675612451605e-13f, 0.9807481714229297f}, + {0.9730644583865243f, 1.411995520506082e-13f, 0.9762934885028384f}, + {0.9773683603724937f, 1.3931689135660008e-13f, 0.9718751430792824f}, + {0.9816364096714153f, 1.3886863881040766e-13f, 0.9674899041721569f}, + {0.9858704436584534f, 1.4039269746746187e-13f, 0.9631346289394122f}, + {0.9900722025959202f, 1.4397871783700112e-13f, 0.9588062550529955f}, + {0.9942433365389557f, 1.4155815408756212e-13f, 0.954501793472642f}, + {0.9983854116765075f, 1.3752388117183045e-13f, 0.9502183215767478f}, + {0.9999999999999819f, 0.02804423714351181f, 0.9437140548413381f}, + {0.9999999999999823f, 0.0675265531658979f, 0.9359017685954015f}, + {0.9999999999999826f, 0.09447578037166751f, 0.9282451825736049f}, + {0.9999999999999823f, 0.11567880450339993f, 0.920737795368809f}, + {0.9999999999999826f, 0.13352190503381375f, 0.9133734552831144f}, + {0.9999999999999823f, 0.1491028314594674f, 0.906146335428585f}, + {0.9999999999999826f, 0.16303259275115084f, 0.8990509109121838f}, + {0.9999999999999826f, 0.17569199214531872f, 0.8920819378992011f}, + {0.9999999999999826f, 0.18733702217610845f, 0.8852344343724449f}, + {0.9999999999999826f, 0.19814940356609517f, 0.8785036624245576f}, + {0.9999999999999823f, 0.20826355122506324f, 0.8718851119384158f}, + {0.9999999999999823f, 0.21778214249596284f, 0.8653744855260821f}, + {0.9999999999999826f, 0.22678566871532468f, 0.8589676846103573f}, + {0.9999999999999823f, 0.2353385863611125f, 0.8526607965450058f}, + {0.9999999999999828f, 0.24349343831907827f, 0.8464500826803465f}, + {0.9999999999999826f, 0.2512937077092952f, 0.840331967290248f}, + {0.9999999999999826f, 0.2587758499993201f, 0.8343030272849384f}, + {0.999999999999983f, 0.26739099502162367f, 0.8275538904243963f}, + {0.999999999999983f, 0.2793555475103376f, 0.8187524096848618f}, + {0.9999999999999828f, 0.29067538241472596f, 0.810154074771914f}, + {0.999999999999983f, 0.3014349177286362f, 0.8017491111724352f}, + {0.9999999999999826f, 0.31170258039783083f, 0.7935283442712853f}, + {0.9999999999999826f, 0.3215347049761315f, 0.7854831467895685f}, + {0.9999999999999826f, 0.3309782925632311f, 0.7776053911816436f}, + {0.9999999999999826f, 0.3400730122474594f, 0.7698874064041857f}, + {0.9999999999999826f, 0.34885268450644075f, 0.7623219385454285f}, + {0.999999999999983f, 0.35734640143399626f, 0.7549021148665397f}, + {0.9999999999999826f, 0.3655793867737775f, 0.7476214108616114f}, + {0.9999999999999826f, 0.3735736659274856f, 0.7404736199894286f}, + {0.9999999999999828f, 0.381348594792351f, 0.7334528257702123f}, + {0.9999999999999826f, 0.38892128210540905f, 0.7265533759748873f}, + {0.9999999999999823f, 0.3963069303390571f, 0.7197698586639263f}, + {0.9999999999999823f, 0.4035191135203492f, 0.7130970798581467f}, + {0.9999999999999823f, 0.410570005644612f, 0.7065300426455539f}, + {0.9999999999999821f, 0.4174705699878856f, 0.700063927546916f}, + {0.9999999999999819f, 0.4242307171780247f, 0.6936940739785828f}, + {0.9999999999999821f, 0.4308594380852102f, 0.6874159626644994f}, + {0.9999999999999821f, 0.4373649162525338f, 0.6812251988606219f}, + {0.9999999999999819f, 0.44375462357781925f, 0.6751174962642902f}, + {0.9999999999999819f, 0.4500354021895003f, 0.6690886614886871f}, + {0.9999999999999821f, 0.45621353486890187f, 0.6631345789884755f}, + {0.9999999999999817f, 0.4622948059133914f, 0.657251196327135f}, + {0.9999999999999817f, 0.4682845539768576f, 0.6514345096795133f}, + {0.9999999999999817f, 0.474187718141824f, 0.645680549464667f}, + {0.9999999999999817f, 0.4800088782535285f, 0.6399853660042518f}, + {0.9999999999999815f, 0.4857522903672667f, 0.6343450151004509f}, + {0.9999999999999815f, 0.4914219180162633f, 0.6287555434246979f}, + {0.9999999999999815f, 0.497021459890778f, 0.6232129736041581f}, + {0.9999999999999815f, 0.5025543744242497f, 0.6177132888869281f}, + {0.9999999999999815f, 0.5080239017046412f, 0.6122524172590773f}, + {0.999999999999981f, 0.5134330830652836f, 0.606826214876734f}, + {0.9999999999999808f, 0.518784778656747f, 0.6014304486641499f}, + {0.9999999999999808f, 0.5240816832574693f, 0.5960607779137368f}, + {0.9999999999999806f, 0.5293263405443853f, 0.5907127347060119f}, + {0.9999999999999806f, 0.5345211560142691f, 0.5853817029456958f}, + {0.9999999999999808f, 0.5396684087209026f, 0.580062895784249f}, + {0.9999999999999808f, 0.5447702619716198f, 0.5747513311680923f}, + {0.9999999999999806f, 0.5498287731085955f, 0.5694418052146554f}, + {0.9999999999999803f, 0.5548459024848833f, 0.5641288630740176f}, + {0.9999999999999801f, 0.5598235217321937f, 0.5588067668806895f}, + {0.9999999999999799f, 0.5647634214064047f, 0.5534694603362047f}, + {0.9999999999999799f, 0.569667318087479f, 0.5481105293861371f}, + {0.9999999999999801f, 0.5745368610026079f, 0.5427231583620321f}, + {0.9999999999999797f, 0.5793736382348097f, 0.5373000808456486f}, + {0.9999999999999797f, 0.5841791825736894f, 0.5318335243749407f}, + {0.9999999999999797f, 0.58895497706055f, 0.5263151479421893f}, + {0.9999999999999795f, 0.5937024602763533f, 0.5207359710263567f}, + {0.9999999999999795f, 0.5984230314181602f, 0.5150862926436902f}, + {0.9999999999999792f, 0.6031180552074987f, 0.5093555985787912f}, + {0.9999999999999792f, 0.607788866672662f, 0.5035324545546109f}, + {0.999999999999979f, 0.6124367758461117f, 0.4976043825895365f}, + {0.999999999999979f, 0.6170630724180334f, 0.4915577171399405f}, + {0.9999999999999788f, 0.6216690303876014f, 0.48537743679248463f}, + {0.9999999999999788f, 0.6262559127547657f, 0.4790469661903673f}, + {0.9999999999999784f, 0.6308249762973255f, 0.4725479414659382f}, + {0.9999999999999786f, 0.6353774764808859f, 0.46585993058805514f}, + {0.9999999999999784f, 0.6399146725529954f, 0.45896009754439654f}, + {0.9999999999999784f, 0.644437832877538f, 0.45182279591800384f}, + {0.9999999999999781f, 0.6489482405714118f, 0.4444190728188997f}, + {0.9999999999999779f, 0.6534471995128909f, 0.4367160577509657f}, + {0.9999999999999779f, 0.6579360408000906f, 0.4286762020035964f}, + {0.9999999999999779f, 0.6624161297489367f, 0.42025632127341656f}, + {0.9999999999999777f, 0.6668888735333959f, 0.41140637540952824f}, + {0.9999999999999777f, 0.6713557295869282f, 0.40206789113388525f}, + {0.9999999999999775f, 0.6758182149038043f, 0.3921718908087272f}}; const size_t calref_n = 256; const float calref_palette[calref_n][3] = { - {0.1, 0.1, 0.1}, - {0.36862745098039246, 0.30980392156862746, 0.6352941176470588}, - {0.3618608227604765, 0.31856978085351784, 0.6394463667820068}, - {0.3550941945405613, 0.3273356401384083, 0.643598615916955}, - {0.3483275663206459, 0.3361014994232987, 0.647750865051903}, - {0.3415609381007305, 0.3448673587081891, 0.6519031141868512}, - {0.33479430988081516, 0.35363321799307956, 0.6560553633217993}, - {0.3280276816608997, 0.36239907727796994, 0.6602076124567474}, - {0.3212610534409842, 0.3711649365628603, 0.6643598615916955}, - {0.31449442522106885, 0.3799307958477509, 0.6685121107266436}, - {0.3077277970011534, 0.38869665513264134, 0.6726643598615917}, - {0.300961168781238, 0.3974625144175317, 0.6768166089965398}, - {0.29419454056132255, 0.4062283737024219, 0.6809688581314879}, - {0.2874279123414072, 0.4149942329873126, 0.685121107266436}, - {0.2806612841214917, 0.4237600922722031, 0.6892733564013841}, - {0.27389465590157624, 0.4325259515570933, 0.6934256055363321}, - {0.2671280276816609, 0.4412918108419839, 0.6975778546712803}, - {0.2603613994617455, 0.45005767012687425, 0.7017301038062282}, - {0.25359477124183005, 0.4588235294117643, 0.7058823529411765}, - {0.24682814302191458, 0.46758938869665506, 0.7100346020761246}, - {0.24006151480199922, 0.4763552479815456, 0.7141868512110727}, - {0.23329488658208386, 0.485121107266436, 0.7183391003460207}, - {0.2265282583621684, 0.49388696655132636, 0.7224913494809689}, - {0.21976163014225292, 0.5026528258362168, 0.726643598615917}, - {0.21299500192233756, 0.5114186851211073, 0.7307958477508651}, - {0.20622837370242209, 0.5201845444059976, 0.7349480968858132}, - {0.19946174548250672, 0.5289504036908883, 0.7391003460207612}, - {0.20007689350249913, 0.5377931564782777, 0.7393310265282583}, - {0.2080738177623992, 0.5467128027681663, 0.7356401384083046}, - {0.21607074202229903, 0.5556324490580544, 0.7319492502883508}, - {0.2240676662821992, 0.5645520953479432, 0.7282583621683968}, - {0.23206459054209927, 0.5734717416378313, 0.7245674740484429}, - {0.24006151480199933, 0.58239138792772, 0.720876585928489}, - {0.2480584390618994, 0.5913110342176088, 0.7171856978085351}, - {0.25605536332179935, 0.6002306805074966, 0.7134948096885814}, - {0.2640522875816994, 0.6091503267973857, 0.7098039215686275}, - {0.27204921184159947, 0.6180699730872741, 0.7061130334486736}, - {0.28004613610149953, 0.6269896193771626, 0.7024221453287197}, - {0.2880430603613995, 0.6359092656670511, 0.6987312572087658}, - {0.29603998462129966, 0.6448289119569397, 0.695040369088812}, - {0.3040369088811996, 0.6537485582468282, 0.6913494809688581}, - {0.3120338331410998, 0.6626682045367166, 0.6876585928489042}, - {0.32003075740099973, 0.671587850826605, 0.6839677047289503}, - {0.3280276816608998, 0.6805074971164937, 0.6802768166089965}, - {0.33602460592079986, 0.6894271434063821, 0.6765859284890426}, - {0.3440215301806999, 0.6983467896962707, 0.6728950403690888}, - {0.35201845444059976, 0.7072664359861591, 0.6692041522491351}, - {0.36001537870050004, 0.7161860822760477, 0.6655132641291811}, - {0.3680123029604, 0.7251057285659362, 0.6618223760092272}, - {0.37600922722029995, 0.7340253748558248, 0.6581314878892734}, - {0.3840061514802, 0.7429450211457131, 0.6544405997693193}, - {0.39200307574010007, 0.7518646674356018, 0.6507497116493657}, - {0.40000000000000036, 0.7607843137254902, 0.6470588235294117}, - {0.4106113033448675, 0.7649365628604383, 0.6469050365244137}, - {0.42122260668973477, 0.7690888119953864, 0.6467512495194156}, - {0.43183391003460214, 0.7732410611303345, 0.6465974625144175}, - {0.4424452133794696, 0.7773933102652826, 0.6464436755094196}, - {0.4530565167243371, 0.7815455594002306, 0.6462898885044215}, - {0.46366782006920415, 0.7856978085351789, 0.6461361014994234}, - {0.4742791234140715, 0.7898500576701271, 0.6459823144944252}, - {0.4848904267589389, 0.794002306805075, 0.6458285274894271}, - {0.49550173010380627, 0.7981545559400232, 0.645674740484429}, - {0.5061130334486739, 0.8023068050749711, 0.6455209534794312}, - {0.5167243367935411, 0.8064590542099194, 0.645367166474433}, - {0.5273356401384084, 0.8106113033448674, 0.6452133794694349}, - {0.5379469434832758, 0.8147635524798154, 0.6450595924644369}, - {0.548558246828143, 0.8189158016147636, 0.6449058054594388}, - {0.5591695501730105, 0.8230680507497117, 0.6447520184544406}, - {0.5697808535178779, 0.8272202998846598, 0.6445982314494427}, - {0.5803921568627453, 0.831372549019608, 0.6444444444444446}, - {0.5910034602076126, 0.8355247981545562, 0.6442906574394465}, - {0.60161476355248, 0.8396770472895041, 0.6441368704344483}, - {0.6122260668973473, 0.8438292964244521, 0.6439830834294502}, - {0.6228373702422147, 0.8479815455594002, 0.6438292964244523}, - {0.633448673587082, 0.8521337946943485, 0.6436755094194541}, - {0.6440599769319493, 0.8562860438292964, 0.6435217224144562}, - {0.6546712802768165, 0.8604382929642447, 0.6433679354094579}, - {0.6652825836216838, 0.8645905420991928, 0.6432141484044598}, - {0.675124951941561, 0.8685121107266438, 0.6422145328719724}, - {0.6841983852364476, 0.8722029988465975, 0.6403690888119954}, - {0.6932718185313342, 0.8758938869665513, 0.6385236447520186}, - {0.7023452518262208, 0.8795847750865051, 0.6366782006920415}, - {0.7114186851211074, 0.8832756632064591, 0.6348327566320646}, - {0.7204921184159938, 0.8869665513264131, 0.6329873125720877}, - {0.7295655517108806, 0.890657439446367, 0.6311418685121105}, - {0.7386389850057672, 0.8943483275663208, 0.6292964244521339}, - {0.7477124183006536, 0.8980392156862746, 0.6274509803921569}, - {0.7567858515955403, 0.9017301038062284, 0.62560553633218}, - {0.7658592848904268, 0.9054209919261822, 0.6237600922722031}, - {0.7749327181853134, 0.909111880046136, 0.6219146482122262}, - {0.7840061514802001, 0.9128027681660901, 0.6200692041522492}, - {0.7930795847750867, 0.916493656286044, 0.618223760092272}, - {0.8021530180699734, 0.920184544405998, 0.6163783160322951}, - {0.8112264513648599, 0.9238754325259518, 0.6145328719723183}, - {0.8202998846597466, 0.9275663206459055, 0.6126874279123413}, - {0.8293733179546331, 0.9312572087658594, 0.6108419838523645}, - {0.8384467512495197, 0.9349480968858133, 0.6089965397923875}, - {0.8475201845444063, 0.9386389850057671, 0.6071510957324106}, - {0.8565936178392928, 0.9423298731257211, 0.6053056516724337}, - {0.8656670511341793, 0.9460207612456747, 0.6034602076124568}, - {0.874740484429066, 0.9497116493656288, 0.6016147635524798}, - {0.8838139177239525, 0.9534025374855826, 0.5997693194925027}, - {0.8928873510188393, 0.9570934256055367, 0.5979238754325257}, - {0.9019607843137256, 0.9607843137254903, 0.5960784313725491}, - {0.9058054594386773, 0.962322183775471, 0.6020761245674742}, - {0.9096501345636295, 0.9638600538254517, 0.6080738177623993}, - {0.9134948096885813, 0.9653979238754326, 0.6140715109573244}, - {0.9173394848135333, 0.9669357939254133, 0.6200692041522493}, - {0.9211841599384853, 0.9684736639753941, 0.6260668973471741}, - {0.9250288350634372, 0.9700115340253751, 0.6320645905420991}, - {0.9288735101883892, 0.9715494040753557, 0.6380622837370243}, - {0.932718185313341, 0.9730872741253366, 0.6440599769319492}, - {0.9365628604382931, 0.9746251441753172, 0.6500576701268744}, - {0.9404075355632451, 0.9761630142252982, 0.6560553633217994}, - {0.9442522106881969, 0.9777008842752788, 0.6620530565167244}, - {0.9480968858131487, 0.9792387543252595, 0.6680507497116493}, - {0.9519415609381008, 0.9807766243752404, 0.6740484429065746}, - {0.9557862360630527, 0.9823144944252212, 0.6800461361014994}, - {0.9596309111880046, 0.9838523644752019, 0.6860438292964245}, - {0.9634755863129567, 0.9853902345251826, 0.6920415224913494}, - {0.9673202614379086, 0.9869281045751634, 0.6980392156862747}, - {0.9711649365628605, 0.9884659746251442, 0.7040369088811996}, - {0.9750096116878124, 0.9900038446751249, 0.7100346020761246}, - {0.9788542868127644, 0.9915417147251058, 0.7160322952710494}, - {0.9826989619377164, 0.9930795847750866, 0.7220299884659747}, - {0.9865436370626683, 0.9946174548250674, 0.7280276816608996}, - {0.9903883121876201, 0.9961553248750481, 0.7340253748558248}, - {0.9942329873125721, 0.9976931949250287, 0.7400230680507498}, - {0.9980776624375239, 0.9992310649750095, 0.746020761245675}, - {0.9999231064975008, 0.9976163014225297, 0.7450211457131873}, - {0.9997693194925027, 0.9928489042675892, 0.7370242214532873}, - {0.9996155324875048, 0.988081507112649, 0.729027297193387}, - {0.9994617454825068, 0.9833141099577085, 0.7210303729334873}, - {0.9993079584775085, 0.9785467128027682, 0.7130334486735873}, - {0.9991541714725107, 0.9737793156478278, 0.7050365244136869}, - {0.9990003844675125, 0.9690119184928874, 0.697039600153787}, - {0.9988465974625144, 0.9642445213379468, 0.6890426758938869}, - {0.9986928104575163, 0.9594771241830067, 0.681045751633987}, - {0.9985390234525182, 0.9547097270280661, 0.6730488273740869}, - {0.9983852364475202, 0.9499423298731258, 0.6650519031141869}, - {0.9982314494425222, 0.9451749327181854, 0.6570549788542868}, - {0.998077662437524, 0.9404075355632449, 0.6490580545943867}, - {0.9979238754325258, 0.9356401384083044, 0.6410611303344868}, - {0.9977700884275279, 0.930872741253364, 0.6330642060745867}, - {0.9976163014225298, 0.9261053440984237, 0.6250672818146866}, - {0.9974625144175316, 0.9213379469434833, 0.6170703575547868}, - {0.9973087274125335, 0.9165705497885427, 0.6090734332948867}, - {0.9971549404075356, 0.9118031526336023, 0.6010765090349864}, - {0.9970011534025374, 0.907035755478662, 0.5930795847750865}, - {0.9968473663975395, 0.9022683583237218, 0.5850826605151866}, - {0.9966935793925413, 0.8975009611687812, 0.5770857362552864}, - {0.9965397923875433, 0.892733564013841, 0.5690888119953864}, - {0.9963860053825454, 0.8879661668589005, 0.5610918877354861}, - {0.9962322183775473, 0.88319876970396, 0.5530949634755861}, - {0.996078431372549, 0.8784313725490196, 0.5450980392156861}, - {0.9959246443675508, 0.8707420222991156, 0.538638985005767}, - {0.9957708573625528, 0.8630526720492118, 0.5321799307958477}, - {0.9956170703575548, 0.855363321799308, 0.5257208765859284}, - {0.9954632833525567, 0.847673971549404, 0.519261822376009}, - {0.9953094963475586, 0.8399846212995001, 0.5128027681660898}, - {0.9951557093425605, 0.8322952710495963, 0.5063437139561706}, - {0.9950019223375625, 0.8246059207996924, 0.4998846597462513}, - {0.9948481353325646, 0.8169165705497885, 0.4934256055363321}, - {0.9946943483275664, 0.8092272202998847, 0.48696655132641264}, - {0.9945405613225683, 0.8015378700499808, 0.48050749711649365}, - {0.9943867743175702, 0.7938485198000771, 0.47404844290657466}, - {0.9942329873125721, 0.7861591695501731, 0.4675893886966551}, - {0.994079200307574, 0.7784698193002692, 0.4611303344867359}, - {0.993925413302576, 0.7707804690503652, 0.4546712802768166}, - {0.9937716262975778, 0.7630911188004613, 0.4482122260668975}, - {0.99361783929258, 0.7554017685505575, 0.44175317185697793}, - {0.9934640522875816, 0.7477124183006536, 0.43529411764705894}, - {0.9933102652825835, 0.7400230680507496, 0.4288350634371395}, - {0.9931564782775857, 0.7323337178008458, 0.4223760092272202}, - {0.9930026912725872, 0.724644367550942, 0.4159169550173013}, - {0.9928489042675894, 0.716955017301038, 0.40945790080738176}, - {0.9926951172625912, 0.7092656670511341, 0.40299884659746266}, - {0.9925413302575933, 0.7015763168012303, 0.3965397923875432}, - {0.9923875432525952, 0.6938869665513263, 0.390080738177624}, - {0.992233756247597, 0.6861976163014225, 0.3836216839677048}, - {0.9914648212226067, 0.677354863514033, 0.3780853517877738}, - {0.990080738177624, 0.6673587081891583, 0.3734717416378317}, - {0.9886966551326414, 0.6573625528642829, 0.36885813148788904}, - {0.9873125720876587, 0.647366397539408, 0.36424452133794694}, - {0.985928489042676, 0.6373702422145329, 0.3596309111880045}, - {0.9845444059976933, 0.6273740868896578, 0.35501730103806217}, - {0.9831603229527106, 0.6173779315647828, 0.35040369088811985}, - {0.981776239907728, 0.6073817762399077, 0.3457900807381774}, - {0.9803921568627451, 0.5973856209150328, 0.3411764705882355}, - {0.9790080738177624, 0.5873894655901575, 0.33656286043829287}, - {0.9776239907727798, 0.5773933102652827, 0.33194925028835065}, - {0.976239907727797, 0.5673971549404075, 0.3273356401384082}, - {0.9748558246828143, 0.5574009996155325, 0.3227220299884661}, - {0.9734717416378316, 0.5474048442906574, 0.3181084198385238}, - {0.9720876585928488, 0.5374086889657824, 0.31349480968858146}, - {0.9707035755478661, 0.5274125336409075, 0.30888119953863913}, - {0.9693194925028835, 0.5174163783160323, 0.3042675893886967}, - {0.9679354094579009, 0.5074202229911575, 0.2996539792387545}, - {0.9665513264129182, 0.4974240676662822, 0.2950403690888119}, - {0.9651672433679354, 0.4874279123414072, 0.2904267589388697}, - {0.9637831603229527, 0.47743175701653207, 0.2858131487889273}, - {0.9623990772779699, 0.4674356016916571, 0.28119953863898506}, - {0.9610149942329872, 0.4574394463667821, 0.27658592848904273}, - {0.9596309111880046, 0.447443291041907, 0.2719723183391005}, - {0.958246828143022, 0.43744713571703187, 0.2673587081891581}, - {0.9568627450980394, 0.42745098039215673, 0.26274509803921564}, - {0.9520953479430986, 0.42022299115724726, 0.26459054209919286}, - {0.9473279507881583, 0.4129950019223377, 0.26643598615916975}, - {0.9425605536332179, 0.40576701268742793, 0.26828143021914663}, - {0.9377931564782777, 0.39853902345251835, 0.2701268742791235}, - {0.9330257593233372, 0.3913110342176086, 0.2719723183391004}, - {0.928258362168397, 0.38408304498269874, 0.27381776239907707}, - {0.9234909650134564, 0.3768550557477892, 0.2756632064590543}, - {0.9187235678585162, 0.36962706651287985, 0.2775086505190312}, - {0.9139561707035755, 0.36239907727797027, 0.2793540945790083}, - {0.9091887735486351, 0.3551710880430604, 0.28119953863898506}, - {0.9044213763936948, 0.34794309880815055, 0.28304498269896183}, - {0.8996539792387543, 0.340715109573241, 0.28489042675893894}, - {0.8948865820838141, 0.3334871203383314, 0.28673587081891583}, - {0.8901191849288733, 0.326259131103422, 0.28858131487889305}, - {0.8853517877739333, 0.319031141868512, 0.2904267589388695}, - {0.8805843906189926, 0.3118031526336025, 0.2922722029988466}, - {0.8758169934640523, 0.30457516339869284, 0.2941176470588236}, - {0.871049596309112, 0.2973471741637831, 0.2959630911188005}, - {0.8662821991541714, 0.29011918492887356, 0.2978085351787776}, - {0.8615148019992313, 0.2828911956939638, 0.2996539792387542}, - {0.8567474048442908, 0.2756632064590542, 0.30149942329873114}, - {0.8519800076893502, 0.26843521722414465, 0.3033448673587078}, - {0.84721261053441, 0.2612072279892348, 0.3051903114186846}, - {0.8424452133794698, 0.2539792387543254, 0.3070357554786618}, - {0.8376778162245291, 0.24675124951941552, 0.3088811995386387}, - {0.8310649750096116, 0.23844675124951953, 0.30880430603613984}, - {0.8226066897347173, 0.22906574394463686, 0.3068050749711647}, - {0.8141484044598231, 0.21968473663975407, 0.30480584390618953}, - {0.8056901191849288, 0.2103037293348713, 0.30280661284121513}, - {0.7972318339100345, 0.20092272202998862, 0.30080738177624006}, - {0.7887735486351404, 0.19154171472510573, 0.2988081507112643}, - {0.7803152633602461, 0.18216070742022294, 0.2968089196462894}, - {0.7718569780853518, 0.17277970011534027, 0.2948096885813144}, - {0.7633986928104576, 0.1633986928104575, 0.29281045751633983}, - {0.7549404075355632, 0.15401768550557482, 0.29081122645136503}, - {0.746482122260669, 0.14463667820069204, 0.2888119953863893}, - {0.7380238369857748, 0.13525567089580925, 0.2868127643214149}, - {0.7295655517108804, 0.12587466359092658, 0.2848135332564399}, - {0.7211072664359862, 0.1164936562860438, 0.28281430219146436}, - {0.7126489811610921, 0.10711264898116135, 0.2808150711264894}, - {0.7041906958861976, 0.09773164167627835, 0.2788158400615146}, - {0.6957324106113034, 0.08835063437139556, 0.27681660899653987}, - {0.6872741253364091, 0.07896962706651289, 0.2748173779315646}, - {0.6788158400615149, 0.06958861976163011, 0.27281814686658995}, - {0.6703575547866205, 0.06020761245674744, 0.2708189158016141}, - {0.6618992695117263, 0.05082660515186466, 0.26881968473663936}, - {0.6534409842368321, 0.041445597846981874, 0.2668204536716644}, - {0.6449826989619377, 0.0320645905420992, 0.26482122260668983}, - {0.6365244136870435, 0.02268358323721642, 0.2628219915417146}, - {0.6280661284121491, 0.013302575932333749, 0.26082276047673913}}; + {0.1f, 0.1f, 0.1f}, + {0.36862745098039246f, 0.30980392156862746f, 0.6352941176470588f}, + {0.3618608227604765f, 0.31856978085351784f, 0.6394463667820068f}, + {0.3550941945405613f, 0.3273356401384083f, 0.643598615916955f}, + {0.3483275663206459f, 0.3361014994232987f, 0.647750865051903f}, + {0.3415609381007305f, 0.3448673587081891f, 0.6519031141868512f}, + {0.33479430988081516f, 0.35363321799307956f, 0.6560553633217993f}, + {0.3280276816608997f, 0.36239907727796994f, 0.6602076124567474f}, + {0.3212610534409842f, 0.3711649365628603f, 0.6643598615916955f}, + {0.31449442522106885f, 0.3799307958477509f, 0.6685121107266436f}, + {0.3077277970011534f, 0.38869665513264134f, 0.6726643598615917f}, + {0.300961168781238f, 0.3974625144175317f, 0.6768166089965398f}, + {0.29419454056132255f, 0.4062283737024219f, 0.6809688581314879f}, + {0.2874279123414072f, 0.4149942329873126f, 0.685121107266436f}, + {0.2806612841214917f, 0.4237600922722031f, 0.6892733564013841f}, + {0.27389465590157624f, 0.4325259515570933f, 0.6934256055363321f}, + {0.2671280276816609f, 0.4412918108419839f, 0.6975778546712803f}, + {0.2603613994617455f, 0.45005767012687425f, 0.7017301038062282f}, + {0.25359477124183005f, 0.4588235294117643f, 0.7058823529411765f}, + {0.24682814302191458f, 0.46758938869665506f, 0.7100346020761246f}, + {0.24006151480199922f, 0.4763552479815456f, 0.7141868512110727f}, + {0.23329488658208386f, 0.485121107266436f, 0.7183391003460207f}, + {0.2265282583621684f, 0.49388696655132636f, 0.7224913494809689f}, + {0.21976163014225292f, 0.5026528258362168f, 0.726643598615917f}, + {0.21299500192233756f, 0.5114186851211073f, 0.7307958477508651f}, + {0.20622837370242209f, 0.5201845444059976f, 0.7349480968858132f}, + {0.19946174548250672f, 0.5289504036908883f, 0.7391003460207612f}, + {0.20007689350249913f, 0.5377931564782777f, 0.7393310265282583f}, + {0.2080738177623992f, 0.5467128027681663f, 0.7356401384083046f}, + {0.21607074202229903f, 0.5556324490580544f, 0.7319492502883508f}, + {0.2240676662821992f, 0.5645520953479432f, 0.7282583621683968f}, + {0.23206459054209927f, 0.5734717416378313f, 0.7245674740484429f}, + {0.24006151480199933f, 0.58239138792772f, 0.720876585928489f}, + {0.2480584390618994f, 0.5913110342176088f, 0.7171856978085351f}, + {0.25605536332179935f, 0.6002306805074966f, 0.7134948096885814f}, + {0.2640522875816994f, 0.6091503267973857f, 0.7098039215686275f}, + {0.27204921184159947f, 0.6180699730872741f, 0.7061130334486736f}, + {0.28004613610149953f, 0.6269896193771626f, 0.7024221453287197f}, + {0.2880430603613995f, 0.6359092656670511f, 0.6987312572087658f}, + {0.29603998462129966f, 0.6448289119569397f, 0.695040369088812f}, + {0.3040369088811996f, 0.6537485582468282f, 0.6913494809688581f}, + {0.3120338331410998f, 0.6626682045367166f, 0.6876585928489042f}, + {0.32003075740099973f, 0.671587850826605f, 0.6839677047289503f}, + {0.3280276816608998f, 0.6805074971164937f, 0.6802768166089965f}, + {0.33602460592079986f, 0.6894271434063821f, 0.6765859284890426f}, + {0.3440215301806999f, 0.6983467896962707f, 0.6728950403690888f}, + {0.35201845444059976f, 0.7072664359861591f, 0.6692041522491351f}, + {0.36001537870050004f, 0.7161860822760477f, 0.6655132641291811f}, + {0.3680123029604f, 0.7251057285659362f, 0.6618223760092272f}, + {0.37600922722029995f, 0.7340253748558248f, 0.6581314878892734f}, + {0.3840061514802f, 0.7429450211457131f, 0.6544405997693193f}, + {0.39200307574010007f, 0.7518646674356018f, 0.6507497116493657f}, + {0.40000000000000036f, 0.7607843137254902f, 0.6470588235294117f}, + {0.4106113033448675f, 0.7649365628604383f, 0.6469050365244137f}, + {0.42122260668973477f, 0.7690888119953864f, 0.6467512495194156f}, + {0.43183391003460214f, 0.7732410611303345f, 0.6465974625144175f}, + {0.4424452133794696f, 0.7773933102652826f, 0.6464436755094196f}, + {0.4530565167243371f, 0.7815455594002306f, 0.6462898885044215f}, + {0.46366782006920415f, 0.7856978085351789f, 0.6461361014994234f}, + {0.4742791234140715f, 0.7898500576701271f, 0.6459823144944252f}, + {0.4848904267589389f, 0.794002306805075f, 0.6458285274894271f}, + {0.49550173010380627f, 0.7981545559400232f, 0.645674740484429f}, + {0.5061130334486739f, 0.8023068050749711f, 0.6455209534794312f}, + {0.5167243367935411f, 0.8064590542099194f, 0.645367166474433f}, + {0.5273356401384084f, 0.8106113033448674f, 0.6452133794694349f}, + {0.5379469434832758f, 0.8147635524798154f, 0.6450595924644369f}, + {0.548558246828143f, 0.8189158016147636f, 0.6449058054594388f}, + {0.5591695501730105f, 0.8230680507497117f, 0.6447520184544406f}, + {0.5697808535178779f, 0.8272202998846598f, 0.6445982314494427f}, + {0.5803921568627453f, 0.831372549019608f, 0.6444444444444446f}, + {0.5910034602076126f, 0.8355247981545562f, 0.6442906574394465f}, + {0.60161476355248f, 0.8396770472895041f, 0.6441368704344483f}, + {0.6122260668973473f, 0.8438292964244521f, 0.6439830834294502f}, + {0.6228373702422147f, 0.8479815455594002f, 0.6438292964244523f}, + {0.633448673587082f, 0.8521337946943485f, 0.6436755094194541f}, + {0.6440599769319493f, 0.8562860438292964f, 0.6435217224144562f}, + {0.6546712802768165f, 0.8604382929642447f, 0.6433679354094579f}, + {0.6652825836216838f, 0.8645905420991928f, 0.6432141484044598f}, + {0.675124951941561f, 0.8685121107266438f, 0.6422145328719724f}, + {0.6841983852364476f, 0.8722029988465975f, 0.6403690888119954f}, + {0.6932718185313342f, 0.8758938869665513f, 0.6385236447520186f}, + {0.7023452518262208f, 0.8795847750865051f, 0.6366782006920415f}, + {0.7114186851211074f, 0.8832756632064591f, 0.6348327566320646f}, + {0.7204921184159938f, 0.8869665513264131f, 0.6329873125720877f}, + {0.7295655517108806f, 0.890657439446367f, 0.6311418685121105f}, + {0.7386389850057672f, 0.8943483275663208f, 0.6292964244521339f}, + {0.7477124183006536f, 0.8980392156862746f, 0.6274509803921569f}, + {0.7567858515955403f, 0.9017301038062284f, 0.62560553633218f}, + {0.7658592848904268f, 0.9054209919261822f, 0.6237600922722031f}, + {0.7749327181853134f, 0.909111880046136f, 0.6219146482122262f}, + {0.7840061514802001f, 0.9128027681660901f, 0.6200692041522492f}, + {0.7930795847750867f, 0.916493656286044f, 0.618223760092272f}, + {0.8021530180699734f, 0.920184544405998f, 0.6163783160322951f}, + {0.8112264513648599f, 0.9238754325259518f, 0.6145328719723183f}, + {0.8202998846597466f, 0.9275663206459055f, 0.6126874279123413f}, + {0.8293733179546331f, 0.9312572087658594f, 0.6108419838523645f}, + {0.8384467512495197f, 0.9349480968858133f, 0.6089965397923875f}, + {0.8475201845444063f, 0.9386389850057671f, 0.6071510957324106f}, + {0.8565936178392928f, 0.9423298731257211f, 0.6053056516724337f}, + {0.8656670511341793f, 0.9460207612456747f, 0.6034602076124568f}, + {0.874740484429066f, 0.9497116493656288f, 0.6016147635524798f}, + {0.8838139177239525f, 0.9534025374855826f, 0.5997693194925027f}, + {0.8928873510188393f, 0.9570934256055367f, 0.5979238754325257f}, + {0.9019607843137256f, 0.9607843137254903f, 0.5960784313725491f}, + {0.9058054594386773f, 0.962322183775471f, 0.6020761245674742f}, + {0.9096501345636295f, 0.9638600538254517f, 0.6080738177623993f}, + {0.9134948096885813f, 0.9653979238754326f, 0.6140715109573244f}, + {0.9173394848135333f, 0.9669357939254133f, 0.6200692041522493f}, + {0.9211841599384853f, 0.9684736639753941f, 0.6260668973471741f}, + {0.9250288350634372f, 0.9700115340253751f, 0.6320645905420991f}, + {0.9288735101883892f, 0.9715494040753557f, 0.6380622837370243f}, + {0.932718185313341f, 0.9730872741253366f, 0.6440599769319492f}, + {0.9365628604382931f, 0.9746251441753172f, 0.6500576701268744f}, + {0.9404075355632451f, 0.9761630142252982f, 0.6560553633217994f}, + {0.9442522106881969f, 0.9777008842752788f, 0.6620530565167244f}, + {0.9480968858131487f, 0.9792387543252595f, 0.6680507497116493f}, + {0.9519415609381008f, 0.9807766243752404f, 0.6740484429065746f}, + {0.9557862360630527f, 0.9823144944252212f, 0.6800461361014994f}, + {0.9596309111880046f, 0.9838523644752019f, 0.6860438292964245f}, + {0.9634755863129567f, 0.9853902345251826f, 0.6920415224913494f}, + {0.9673202614379086f, 0.9869281045751634f, 0.6980392156862747f}, + {0.9711649365628605f, 0.9884659746251442f, 0.7040369088811996f}, + {0.9750096116878124f, 0.9900038446751249f, 0.7100346020761246f}, + {0.9788542868127644f, 0.9915417147251058f, 0.7160322952710494f}, + {0.9826989619377164f, 0.9930795847750866f, 0.7220299884659747f}, + {0.9865436370626683f, 0.9946174548250674f, 0.7280276816608996f}, + {0.9903883121876201f, 0.9961553248750481f, 0.7340253748558248f}, + {0.9942329873125721f, 0.9976931949250287f, 0.7400230680507498f}, + {0.9980776624375239f, 0.9992310649750095f, 0.746020761245675f}, + {0.9999231064975008f, 0.9976163014225297f, 0.7450211457131873f}, + {0.9997693194925027f, 0.9928489042675892f, 0.7370242214532873f}, + {0.9996155324875048f, 0.988081507112649f, 0.729027297193387f}, + {0.9994617454825068f, 0.9833141099577085f, 0.7210303729334873f}, + {0.9993079584775085f, 0.9785467128027682f, 0.7130334486735873f}, + {0.9991541714725107f, 0.9737793156478278f, 0.7050365244136869f}, + {0.9990003844675125f, 0.9690119184928874f, 0.697039600153787f}, + {0.9988465974625144f, 0.9642445213379468f, 0.6890426758938869f}, + {0.9986928104575163f, 0.9594771241830067f, 0.681045751633987f}, + {0.9985390234525182f, 0.9547097270280661f, 0.6730488273740869f}, + {0.9983852364475202f, 0.9499423298731258f, 0.6650519031141869f}, + {0.9982314494425222f, 0.9451749327181854f, 0.6570549788542868f}, + {0.998077662437524f, 0.9404075355632449f, 0.6490580545943867f}, + {0.9979238754325258f, 0.9356401384083044f, 0.6410611303344868f}, + {0.9977700884275279f, 0.930872741253364f, 0.6330642060745867f}, + {0.9976163014225298f, 0.9261053440984237f, 0.6250672818146866f}, + {0.9974625144175316f, 0.9213379469434833f, 0.6170703575547868f}, + {0.9973087274125335f, 0.9165705497885427f, 0.6090734332948867f}, + {0.9971549404075356f, 0.9118031526336023f, 0.6010765090349864f}, + {0.9970011534025374f, 0.907035755478662f, 0.5930795847750865f}, + {0.9968473663975395f, 0.9022683583237218f, 0.5850826605151866f}, + {0.9966935793925413f, 0.8975009611687812f, 0.5770857362552864f}, + {0.9965397923875433f, 0.892733564013841f, 0.5690888119953864f}, + {0.9963860053825454f, 0.8879661668589005f, 0.5610918877354861f}, + {0.9962322183775473f, 0.88319876970396f, 0.5530949634755861f}, + {0.996078431372549f, 0.8784313725490196f, 0.5450980392156861f}, + {0.9959246443675508f, 0.8707420222991156f, 0.538638985005767f}, + {0.9957708573625528f, 0.8630526720492118f, 0.5321799307958477f}, + {0.9956170703575548f, 0.855363321799308f, 0.5257208765859284f}, + {0.9954632833525567f, 0.847673971549404f, 0.519261822376009f}, + {0.9953094963475586f, 0.8399846212995001f, 0.5128027681660898f}, + {0.9951557093425605f, 0.8322952710495963f, 0.5063437139561706f}, + {0.9950019223375625f, 0.8246059207996924f, 0.4998846597462513f}, + {0.9948481353325646f, 0.8169165705497885f, 0.4934256055363321f}, + {0.9946943483275664f, 0.8092272202998847f, 0.48696655132641264f}, + {0.9945405613225683f, 0.8015378700499808f, 0.48050749711649365f}, + {0.9943867743175702f, 0.7938485198000771f, 0.47404844290657466f}, + {0.9942329873125721f, 0.7861591695501731f, 0.4675893886966551f}, + {0.994079200307574f, 0.7784698193002692f, 0.4611303344867359f}, + {0.993925413302576f, 0.7707804690503652f, 0.4546712802768166f}, + {0.9937716262975778f, 0.7630911188004613f, 0.4482122260668975f}, + {0.99361783929258f, 0.7554017685505575f, 0.44175317185697793f}, + {0.9934640522875816f, 0.7477124183006536f, 0.43529411764705894f}, + {0.9933102652825835f, 0.7400230680507496f, 0.4288350634371395f}, + {0.9931564782775857f, 0.7323337178008458f, 0.4223760092272202f}, + {0.9930026912725872f, 0.724644367550942f, 0.4159169550173013f}, + {0.9928489042675894f, 0.716955017301038f, 0.40945790080738176f}, + {0.9926951172625912f, 0.7092656670511341f, 0.40299884659746266f}, + {0.9925413302575933f, 0.7015763168012303f, 0.3965397923875432f}, + {0.9923875432525952f, 0.6938869665513263f, 0.390080738177624f}, + {0.992233756247597f, 0.6861976163014225f, 0.3836216839677048f}, + {0.9914648212226067f, 0.677354863514033f, 0.3780853517877738f}, + {0.990080738177624f, 0.6673587081891583f, 0.3734717416378317f}, + {0.9886966551326414f, 0.6573625528642829f, 0.36885813148788904f}, + {0.9873125720876587f, 0.647366397539408f, 0.36424452133794694f}, + {0.985928489042676f, 0.6373702422145329f, 0.3596309111880045f}, + {0.9845444059976933f, 0.6273740868896578f, 0.35501730103806217f}, + {0.9831603229527106f, 0.6173779315647828f, 0.35040369088811985f}, + {0.981776239907728f, 0.6073817762399077f, 0.3457900807381774f}, + {0.9803921568627451f, 0.5973856209150328f, 0.3411764705882355f}, + {0.9790080738177624f, 0.5873894655901575f, 0.33656286043829287f}, + {0.9776239907727798f, 0.5773933102652827f, 0.33194925028835065f}, + {0.976239907727797f, 0.5673971549404075f, 0.3273356401384082f}, + {0.9748558246828143f, 0.5574009996155325f, 0.3227220299884661f}, + {0.9734717416378316f, 0.5474048442906574f, 0.3181084198385238f}, + {0.9720876585928488f, 0.5374086889657824f, 0.31349480968858146f}, + {0.9707035755478661f, 0.5274125336409075f, 0.30888119953863913f}, + {0.9693194925028835f, 0.5174163783160323f, 0.3042675893886967f}, + {0.9679354094579009f, 0.5074202229911575f, 0.2996539792387545f}, + {0.9665513264129182f, 0.4974240676662822f, 0.2950403690888119f}, + {0.9651672433679354f, 0.4874279123414072f, 0.2904267589388697f}, + {0.9637831603229527f, 0.47743175701653207f, 0.2858131487889273f}, + {0.9623990772779699f, 0.4674356016916571f, 0.28119953863898506f}, + {0.9610149942329872f, 0.4574394463667821f, 0.27658592848904273f}, + {0.9596309111880046f, 0.447443291041907f, 0.2719723183391005f}, + {0.958246828143022f, 0.43744713571703187f, 0.2673587081891581f}, + {0.9568627450980394f, 0.42745098039215673f, 0.26274509803921564f}, + {0.9520953479430986f, 0.42022299115724726f, 0.26459054209919286f}, + {0.9473279507881583f, 0.4129950019223377f, 0.26643598615916975f}, + {0.9425605536332179f, 0.40576701268742793f, 0.26828143021914663f}, + {0.9377931564782777f, 0.39853902345251835f, 0.2701268742791235f}, + {0.9330257593233372f, 0.3913110342176086f, 0.2719723183391004f}, + {0.928258362168397f, 0.38408304498269874f, 0.27381776239907707f}, + {0.9234909650134564f, 0.3768550557477892f, 0.2756632064590543f}, + {0.9187235678585162f, 0.36962706651287985f, 0.2775086505190312f}, + {0.9139561707035755f, 0.36239907727797027f, 0.2793540945790083f}, + {0.9091887735486351f, 0.3551710880430604f, 0.28119953863898506f}, + {0.9044213763936948f, 0.34794309880815055f, 0.28304498269896183f}, + {0.8996539792387543f, 0.340715109573241f, 0.28489042675893894f}, + {0.8948865820838141f, 0.3334871203383314f, 0.28673587081891583f}, + {0.8901191849288733f, 0.326259131103422f, 0.28858131487889305f}, + {0.8853517877739333f, 0.319031141868512f, 0.2904267589388695f}, + {0.8805843906189926f, 0.3118031526336025f, 0.2922722029988466f}, + {0.8758169934640523f, 0.30457516339869284f, 0.2941176470588236f}, + {0.871049596309112f, 0.2973471741637831f, 0.2959630911188005f}, + {0.8662821991541714f, 0.29011918492887356f, 0.2978085351787776f}, + {0.8615148019992313f, 0.2828911956939638f, 0.2996539792387542f}, + {0.8567474048442908f, 0.2756632064590542f, 0.30149942329873114f}, + {0.8519800076893502f, 0.26843521722414465f, 0.3033448673587078f}, + {0.84721261053441f, 0.2612072279892348f, 0.3051903114186846f}, + {0.8424452133794698f, 0.2539792387543254f, 0.3070357554786618f}, + {0.8376778162245291f, 0.24675124951941552f, 0.3088811995386387f}, + {0.8310649750096116f, 0.23844675124951953f, 0.30880430603613984f}, + {0.8226066897347173f, 0.22906574394463686f, 0.3068050749711647f}, + {0.8141484044598231f, 0.21968473663975407f, 0.30480584390618953f}, + {0.8056901191849288f, 0.2103037293348713f, 0.30280661284121513f}, + {0.7972318339100345f, 0.20092272202998862f, 0.30080738177624006f}, + {0.7887735486351404f, 0.19154171472510573f, 0.2988081507112643f}, + {0.7803152633602461f, 0.18216070742022294f, 0.2968089196462894f}, + {0.7718569780853518f, 0.17277970011534027f, 0.2948096885813144f}, + {0.7633986928104576f, 0.1633986928104575f, 0.29281045751633983f}, + {0.7549404075355632f, 0.15401768550557482f, 0.29081122645136503f}, + {0.746482122260669f, 0.14463667820069204f, 0.2888119953863893f}, + {0.7380238369857748f, 0.13525567089580925f, 0.2868127643214149f}, + {0.7295655517108804f, 0.12587466359092658f, 0.2848135332564399f}, + {0.7211072664359862f, 0.1164936562860438f, 0.28281430219146436f}, + {0.7126489811610921f, 0.10711264898116135f, 0.2808150711264894f}, + {0.7041906958861976f, 0.09773164167627835f, 0.2788158400615146f}, + {0.6957324106113034f, 0.08835063437139556f, 0.27681660899653987f}, + {0.6872741253364091f, 0.07896962706651289f, 0.2748173779315646f}, + {0.6788158400615149f, 0.06958861976163011f, 0.27281814686658995f}, + {0.6703575547866205f, 0.06020761245674744f, 0.2708189158016141f}, + {0.6618992695117263f, 0.05082660515186466f, 0.26881968473663936f}, + {0.6534409842368321f, 0.041445597846981874f, 0.2668204536716644f}, + {0.6449826989619377f, 0.0320645905420992f, 0.26482122260668983f}, + {0.6365244136870435f, 0.02268358323721642f, 0.2628219915417146f}, + {0.6280661284121491f, 0.013302575932333749f, 0.26082276047673913f}}; } // namespace viz } // namespace ouster diff --git a/ouster_viz/src/glfw.cpp b/ouster_viz/src/glfw.cpp index 3f411256..e78652c0 100644 --- a/ouster_viz/src/glfw.cpp +++ b/ouster_viz/src/glfw.cpp @@ -156,15 +156,15 @@ GLFWContext::GLFWContext(const std::string& name, bool fix_aspect, } glfwMakeContextCurrent(window); -#ifdef OUSTER_VIZ_GLEW - if (glewInit() != GLEW_OK) { +#ifdef OUSTER_VIZ_USE_GLAD + if (!gladLoadGLLoader((GLADloadproc)glfwGetProcAddress)) { glfwTerminate(); - throw std::runtime_error("Failed to initialize GLEW"); + throw std::runtime_error("Failed to initialize GLAD"); } #else - if (!gladLoadGLLoader((GLADloadproc)glfwGetProcAddress)) { + if (glewInit() != GLEW_OK) { glfwTerminate(); - throw std::runtime_error("Failed to initialize GLAD"); + throw std::runtime_error("Failed to initialize GLEW"); } #endif @@ -207,6 +207,9 @@ GLFWContext::GLFWContext(const std::string& name, bool fix_aspect, gltViewport(viewport_width, viewport_height); window_context.viewport_width = viewport_width; window_context.viewport_height = viewport_height; + + // release context in case subsequent calls are done from another thread + glfwMakeContextCurrent(nullptr); } GLFWContext::~GLFWContext() { glfwDestroyWindow(window); } diff --git a/ouster_viz/src/glfw.h b/ouster_viz/src/glfw.h index 4d31c7bd..4dc75b90 100644 --- a/ouster_viz/src/glfw.h +++ b/ouster_viz/src/glfw.h @@ -5,10 +5,10 @@ #pragma once -#ifdef OUSTER_VIZ_GLEW -#include -#else +#ifdef OUSTER_VIZ_USE_GLAD #include +#else +#include #endif #include diff --git a/ouster_viz/src/point_viz.cpp b/ouster_viz/src/point_viz.cpp index c9e296b1..26894cd2 100644 --- a/ouster_viz/src/point_viz.cpp +++ b/ouster_viz/src/point_viz.cpp @@ -74,7 +74,8 @@ class Indexed { void draw(const WindowCtx& ctx, const impl::CameraData& camera) { for (auto& f : front) { if (!f.state) continue; // skip deleted - if (!f.gl) f.gl.reset(new GL{*f.state}); // init GL for added + if (!f.gl) + f.gl = std::make_unique(*f.state); // init GL for added f.gl->draw(ctx, camera, *f.state); } } @@ -90,7 +91,7 @@ class Indexed { if (back[i] && front[i].state) { std::swap(*front[i].state, *back[i]); } else if (back[i] && !front[i].state) { - front[i].state.reset(new T{*back[i]}); + front[i].state = std::make_unique(*back[i]); back[i]->clear(); } else if (!back[i] && front[i].state) { front[i].state.reset(); @@ -105,7 +106,7 @@ class Indexed { * PointViz implementation */ struct PointViz::Impl { - GLFWContext glfw; + std::unique_ptr glfw; GLuint vao; // state for drawing @@ -130,9 +131,7 @@ struct PointViz::Impl { Handlers scroll_handlers; Handlers mouse_pos_handlers; - Impl(const std::string& name, bool fix_aspect, int window_width, - int window_height) - : glfw{name, fix_aspect, window_width, window_height} {} + Impl(std::unique_ptr&& glfw) : glfw{std::move(glfw)} {} }; /* @@ -141,13 +140,15 @@ struct PointViz::Impl { PointViz::PointViz(const std::string& name, bool fix_aspect, int window_width, int window_height) { - // TODO initialization (and opengl API usage) still pretty messed up due to - // single shared vao - pimpl = std::unique_ptr{ - new Impl{name, fix_aspect, window_width, window_height}}; + auto glfw = std::make_unique(name, fix_aspect, window_width, + window_height); + + // set context for GL initialization + glfwMakeContextCurrent(glfw->window); + + pimpl = std::make_unique(std::move(glfw)); // top-level gl state for point viz - glfwMakeContextCurrent(pimpl->glfw.window); glGenVertexArrays(1, &pimpl->vao); glBindVertexArray(pimpl->vao); @@ -161,30 +162,33 @@ PointViz::PointViz(const std::string& name, bool fix_aspect, int window_width, impl::GLRings::initialize(); impl::GLCuboid::initialize(); + // release context in case subsequent calls are done from another thread + glfwMakeContextCurrent(nullptr); + // add user-setable input handlers - pimpl->glfw.key_handler = [this](const WindowCtx& ctx, int key, int mods) { + pimpl->glfw->key_handler = [this](const WindowCtx& ctx, int key, int mods) { for (auto& f : pimpl->key_handlers) if (!f(ctx, key, mods)) break; }; - pimpl->glfw.mouse_button_handler = [this](const WindowCtx& ctx, int button, - int mods) { + pimpl->glfw->mouse_button_handler = [this](const WindowCtx& ctx, int button, + int mods) { for (auto& f : pimpl->mouse_button_handlers) if (!f(ctx, button, mods)) break; }; - pimpl->glfw.scroll_handler = [this](const WindowCtx& ctx, double x, - double y) { + pimpl->glfw->scroll_handler = [this](const WindowCtx& ctx, double x, + double y) { for (auto& f : pimpl->scroll_handlers) if (!f(ctx, x, y)) break; }; - pimpl->glfw.mouse_pos_handler = [this](const WindowCtx& ctx, double x, - double y) { + pimpl->glfw->mouse_pos_handler = [this](const WindowCtx& ctx, double x, + double y) { for (auto& f : pimpl->mouse_pos_handlers) if (!f(ctx, x, y)) break; }; // glfwPollEvents blocks during resize on macos. Keep rendering to avoid // artifacts during resize - pimpl->glfw.resize_handler = [this]() { + pimpl->glfw->resize_handler = [this]() { (void)this; #ifdef __APPLE__ draw(); @@ -195,23 +199,24 @@ PointViz::PointViz(const std::string& name, bool fix_aspect, int window_width, PointViz::~PointViz() { glDeleteVertexArrays(1, &pimpl->vao); } void PointViz::run() { - pimpl->glfw.running(true); - pimpl->glfw.visible(true); + pimpl->glfw->running(true); + pimpl->glfw->visible(true); while (running()) run_once(); - pimpl->glfw.visible(false); + pimpl->glfw->visible(false); } void PointViz::run_once() { - glfwMakeContextCurrent(pimpl->glfw.window); + if (glfwGetCurrentContext() != pimpl->glfw->window) + glfwMakeContextCurrent(pimpl->glfw->window); draw(); glfwPollEvents(); } -bool PointViz::running() { return pimpl->glfw.running(); } +bool PointViz::running() { return pimpl->glfw->running(); } -void PointViz::running(bool state) { pimpl->glfw.running(state); } +void PointViz::running(bool state) { pimpl->glfw->running(state); } -void PointViz::visible(bool state) { pimpl->glfw.visible(state); } +void PointViz::visible(bool state) { pimpl->glfw->visible(state); } bool PointViz::update() { std::lock_guard guard{pimpl->update_mx}; @@ -240,7 +245,7 @@ void PointViz::draw() { // draw images { std::lock_guard guard{pimpl->update_mx}; - const auto& ctx = pimpl->glfw.window_context; + const auto& ctx = pimpl->glfw->window_context; // calculate camera matrices auto camera_data = @@ -279,7 +284,7 @@ void PointViz::draw() { pimpl->front_changed = false; } - glfwSwapBuffers(pimpl->glfw.window); + glfwSwapBuffers(pimpl->glfw->window); } /* @@ -420,7 +425,8 @@ void Cloud::clear() { } void Cloud::set_range(const uint32_t* x) { - std::copy(x, x + n_, range_data_.begin()); + std::transform(x, x + n_, std::begin(range_data_), + [](uint32_t i) { return static_cast(i); }); range_changed_ = true; } @@ -650,7 +656,7 @@ void add_default_controls(viz::PointViz& viz, std::mutex* mx) { viz.push_scroll_handler([=, &viz](const WindowCtx&, double, double yoff) { auto lock = mx ? std::unique_lock{*mx} : std::unique_lock{}; - viz.camera().dolly(yoff * 5); + viz.camera().dolly(static_cast(yoff * 5)); viz.update(); return true; }); diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index fc8a5f53..d9b63931 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -1,10 +1,13 @@ -cmake_minimum_required(VERSION 3.1.0) +cmake_minimum_required(VERSION 3.10...3.22) set(OUSTER_SDK_PATH "${CMAKE_CURRENT_LIST_DIR}/.." CACHE STRING "SDK source directory") file(TO_CMAKE_PATH "${OUSTER_SDK_PATH}" OUSTER_SDK_PATH) message(STATUS "Ouster SDK location: ${OUSTER_SDK_PATH}") list(APPEND CMAKE_MODULE_PATH ${OUSTER_SDK_PATH}/cmake) +# configure vcpkg from environment variables, if present +include(VcpkgEnv) + project(python-ouster-sdk) # ==== Options ==== @@ -13,10 +16,10 @@ set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) if(MSVC) - add_compile_options(/W3 /wd4996) + add_compile_options(/W2 /wd4996) add_compile_definitions(NOMINMAX _USE_MATH_DEFINES WIN32_LEAN_AND_MEAN) else() - add_compile_options(-Wall -Wextra -Werror -Wno-error=deprecated-declarations) + add_compile_options(-Wall -Wextra -Wno-error=deprecated-declarations) endif() option(BUILD_VIZ "Enabled for Python build" ON) @@ -26,7 +29,7 @@ option(BUILD_PCAP "Enabled for Python build" ON) find_package(pybind11 2.0 REQUIRED) find_package(Eigen3 REQUIRED) find_package(OusterSDK REQUIRED) - + # when building as a top-level project if(CMAKE_PROJECT_NAME STREQUAL PROJECT_NAME) message(STATUS "Ouster SDK client: Using EIGEN_MAX_ALIGN_BYTES = 32") @@ -43,21 +46,20 @@ set(EXT_DIR ${CMAKE_LIBRARY_OUTPUT_DIRECTORY}) # they contain a generator expression, so we use a noop: $<0:> # https://cmake.org/cmake/help/latest/prop_tgt/LIBRARY_OUTPUT_DIRECTORY.html pybind11_add_module(_client src/cpp/_client.cpp) -target_link_libraries(_client PRIVATE ouster_client) +target_link_libraries(_client PRIVATE ouster_client ouster_build) target_include_directories(_client SYSTEM PRIVATE ${EIGEN3_INCLUDE_DIR}) set_target_properties(_client PROPERTIES POSITION_INDEPENDENT_CODE TRUE LIBRARY_OUTPUT_DIRECTORY ${EXT_DIR}/client/$<0:>) pybind11_add_module(_pcap src/cpp/_pcap.cpp) -target_link_libraries(_pcap PRIVATE ouster_pcap) -target_include_directories(_pcap SYSTEM PRIVATE ${EIGEN3_INCLUDE_DIR}) +target_link_libraries(_pcap PRIVATE ouster_pcap ouster_build) set_target_properties(_pcap PROPERTIES POSITION_INDEPENDENT_CODE TRUE LIBRARY_OUTPUT_DIRECTORY ${EXT_DIR}/pcap/$<0:>) pybind11_add_module(_viz src/cpp/_viz.cpp) -target_link_libraries(_viz PRIVATE ouster_client ouster_viz) +target_link_libraries(_viz PRIVATE ouster_client ouster_viz ouster_build) target_include_directories(_viz SYSTEM PRIVATE ${EIGEN3_INCLUDE_DIR}) set_target_properties(_viz PROPERTIES POSITION_INDEPENDENT_CODE TRUE diff --git a/python/Dockerfile b/python/Dockerfile index eccbe5cc..2f719674 100644 --- a/python/Dockerfile +++ b/python/Dockerfile @@ -10,20 +10,22 @@ RUN set -xe \ && apt-get install -y --no-install-recommends \ build-essential \ cmake \ + doxygen \ +# SDK deps \ libeigen3-dev \ libjsoncpp-dev \ - libtins-dev \ libpcap-dev \ + libtins-dev \ + libcurl4-openssl-dev \ + libglfw3-dev \ + libglew-dev \ +# Python deps python3-dev \ python3-pip \ python3-venv \ pybind11-dev \ # Install any additional available cpython versions for testing - 'python3.[6-9]-dev' \ - libglfw3-dev \ - libglew-dev \ - python3-breathe \ - doxygen \ + 'python3.(7|8|9|10)-dev' \ && rm -rf /var/lib/apt/lists # Set up non-root build user and environment @@ -40,14 +42,18 @@ ENV PATH="${PATH}:${BUILD_HOME}/.local/bin" \ WORKDIR ${BUILD_HOME} RUN set -xe \ -&& python3 -m pip install --no-cache-dir --user -U pip tox +# use oldest available, supported python as tox default +&& PYTHON=$(which python3.7 python3.8 python3.9 python3.10 | head -1) \ +&& $PYTHON -m pip install --no-cache-dir --user -U pip tox # Populate source dir COPY . ${OUSTER_SDK_PATH} # Entrypoint for running tox: # -# Usage: docker run --rm -it [-e VAR=VAL ..] ouster-sdk-tox [TOX ARGS ..] +# Usage: from OUSTER_SDK root, run +# docker build . -f python/Dockerfile -t ouster-sdk-tox +# docker run --rm -it [-e VAR=VAL ..] ouster-sdk-tox [TOX ARGS ..] # # Without any arguments: run unit tests with all available Python versions. See # the tox.ini for other commands. @@ -56,11 +62,11 @@ COPY . ${OUSTER_SDK_PATH} # running with additional host bind mounts: # ARTIFACT_DIR: where to put test output. Defaults to ${BUILD_HOME}/artifacts # WHEELS_DIR: where to look for wheels for running tests against wheels -# OUSTER_SDK_PTH: path of SDK source +# OUSTER_SDK_PATH: path of SDK source # ENTRYPOINT ["sh", "-c", "set -e \ && rm -rf ./src && cp -a ${OUSTER_SDK_PATH} ./src \ && export ARTIFACT_DIR=${ARTIFACT_DIR:-$BUILD_HOME/artifacts} \ && . /etc/os-release && export ID VERSION_ID \ -&& exec python3 -m tox -c ./src/python --workdir ${HOME}/tox \"$@\" \ +&& exec tox -c ./src/python --workdir ${HOME}/tox \"$@\" \ ", "tox-entrypoint"] diff --git a/python/MANIFEST.in b/python/MANIFEST.in index 41658664..c2e965b8 100644 --- a/python/MANIFEST.in +++ b/python/MANIFEST.in @@ -1,8 +1,7 @@ include README.rst CMakeLists.txt mypy.ini tox.ini -graft docs graft src graft tests # add the C++ source via the symlink created by setup.py; prune to avoid a cycle graft sdk -prune sdk/python \ No newline at end of file +prune sdk/python diff --git a/python/README.rst b/python/README.rst index be7ce6a3..ac610f8d 100644 --- a/python/README.rst +++ b/python/README.rst @@ -2,6 +2,29 @@ Ouster Python SDK ================= +.. + [sdk-overview-start] + +The Ouster Sensor SDK provides developers interfaces for interacting with sensor hardware and +recorded sensor data suitable for prototyping, evaluation, and other non-safety-critical +applications in Python and C++. Example and reference code is provided for common operations on +sensor data in both languages. The SDK includes APIs for: + +* Querying and setting sensor configuration +* Recording and reading data in pcap format +* Reading and buffering sensor UDP data streams reliably +* Conversion of raw data to range/signal/near_ir/reflectivity images (destaggering) +* Efficient projection of range measurements to Cartesian (x, y, z) corrdinates +* Visualization of multi-beam flash lidar data + +Additionally, in Python, the SDK also provides: + +* Frame-based access to lidar data as numpy datatypes +* A responsive visualizer utility for pcap and sensor + +.. + [sdk-overview-end] + Supported Platforms ------------------- @@ -18,7 +41,7 @@ Pre-built binaries are provided on `PyPI`_ for the following platforms: Building from source is supported on: -- Ubuntu 18.04, 20.04, and Debian 10 (x86-64, aarch64) +- Ubuntu 18.04, 20.04, 22.04, and Debian 10 (x86-64, aarch64) - macOS >= 10.13 (x86-64), >= 11.0 (arm64) - Windows 10 (x86-64) diff --git a/python/setup.cfg b/python/setup.cfg index d741ab87..f805ebe0 100644 --- a/python/setup.cfg +++ b/python/setup.cfg @@ -17,10 +17,10 @@ classifiers = Programming Language :: Python Programming Language :: Python :: 3 Programming Language :: Python :: 3 :: Only - Programming Language :: Python :: 3.6 Programming Language :: Python :: 3.7 Programming Language :: Python :: 3.8 Programming Language :: Python :: 3.9 + Programming Language :: Python :: 3.10 Topic :: Software Development :: Libraries Topic :: System :: Hardware :: Hardware Drivers Topic :: Scientific/Engineering diff --git a/python/setup.py b/python/setup.py index 46760e00..f5d77b15 100644 --- a/python/setup.py +++ b/python/setup.py @@ -1,6 +1,8 @@ import os +import re import sys import platform +import shlex import shutil import subprocess @@ -19,6 +21,12 @@ if not os.path.exists(os.path.join(OUSTER_SDK_PATH, "cmake")): raise RuntimeError("Could not guess OUSTER_SDK_PATH") +# https://packaging.python.org/en/latest/guides/single-sourcing-package-version/ +def parse_version(): + with open(os.path.join(OUSTER_SDK_PATH, 'CMakeLists.txt')) as listfile: + content = listfile.read() + groups = re.search("set\(OusterSDK_VERSION_STRING ([^-\)]+)(-(.*))?\)", content) + return groups.group(1) + (groups.group(3) or "") class CMakeExtension(Extension): def __init__(self, name, sourcedir=''): @@ -51,36 +59,23 @@ def build_extension(self, ext): # Bug in pybind11 cmake strips symbols with RelWithDebInfo # https://github.com/pybind/pybind11/issues/1891 cfg = 'Debug' if self.debug else 'Release' + + cmake_args += ['-DCMAKE_BUILD_TYPE=' + cfg] build_args = ['--config', cfg] if platform.system() == "Windows": - cmake_args += ['-G', 'Visual Studio 15 2017 Win64'] build_args += ['--', '/m'] else: - cmake_args += ['-DCMAKE_BUILD_TYPE=' + cfg] build_args += ['--', '-j2'] - env = os.environ.copy() - env['CXXFLAGS'] = '{} -DVERSION_INFO=\\"{}\\"'.format( - env.get('CXXFLAGS', ''), self.distribution.get_version()) - - # allow specifying toolchain in env - toolchain = env.get('CMAKE_TOOLCHAIN_FILE') - if toolchain: - cmake_args += ['-DCMAKE_TOOLCHAIN_FILE=' + toolchain] - - # specify VCPKG triplet in env - triplet = env.get('VCPKG_TARGET_TRIPLET') - if triplet: - cmake_args += ['-DVCPKG_TARGET_TRIPLET=' + triplet] - # pass OUSTER_SDK_PATH to cmake cmake_args += ['-DOUSTER_SDK_PATH=' + OUSTER_SDK_PATH] # specify additional cmake args - extra_args = env.get('CMAKE_ARGS') + env = os.environ.copy() + extra_args = env.get('OUSTER_SDK_CMAKE_ARGS') if extra_args: - cmake_args += [extra_args] + cmake_args += shlex.split(extra_args) if not os.path.exists(self.build_temp): os.makedirs(self.build_temp) @@ -124,16 +119,17 @@ def run(self): setup( name='ouster-sdk', url='https://github.com/ouster-lidar/ouster_example', - version='0.4.1', + # read from top-level sdk CMakeLists.txt + version=parse_version(), package_dir={'': 'src'}, - packages=find_namespace_packages(where='src'), - namespace_packages=['ouster'], + packages=find_namespace_packages(where='src', include='ouster.*'), package_data={ 'ouster.client': ['py.typed', '_client.pyi'], 'ouster.pcap': ['py.typed', '_pcap.pyi'], 'ouster.sdk': ['py.typed', '_viz.pyi'], }, - author='Ouster SW Developers', + author='Ouster Sensor SDK Developers', + author_email='oss@ouster.io', description='Ouster sensor SDK', license='BSD 3-Clause License', ext_modules=[ @@ -145,9 +141,8 @@ def run(self): 'bdist_wheel': sdk_bdist_wheel, }, zip_safe=False, - python_requires='>=3.6, <4', + python_requires='>=3.7, <4', install_requires=[ - 'dataclasses >=0.7; python_version >="3.6" and python_version <"3.7"', 'more-itertools >=8.6', 'numpy >=1.19, <2, !=1.19.4', 'typing-extensions >=3.7', @@ -162,7 +157,6 @@ def run(self): 'sphinx-copybutton ==0.5.0', 'docutils <0.18', 'sphinx-tabs ==3.3.1', - 'open3d', 'breathe ==4.33.1' ], 'examples': [ diff --git a/python/src/cpp/_client.cpp b/python/src/cpp/_client.cpp index 9ccdaaa8..4b05a21b 100644 --- a/python/src/cpp/_client.cpp +++ b/python/src/cpp/_client.cpp @@ -27,6 +27,7 @@ #include "ouster/buffered_udp_source.h" #include "ouster/client.h" #include "ouster/image_processing.h" +#include "ouster/impl/build.h" #include "ouster/lidar_scan.h" #include "ouster/types.h" @@ -55,14 +56,14 @@ namespace ouster { namespace sensor { namespace impl { -extern const Table lidar_mode_strings; +extern const Table lidar_mode_strings; extern const Table timestamp_mode_strings; extern const Table operating_mode_strings; extern const Table multipurpose_io_mode_strings; extern const Table polarity_strings; extern const Table nmea_baud_rate_strings; -extern Table chanfield_strings; +extern Table chanfield_strings; extern Table udp_profile_lidar_strings; extern Table udp_profile_imu_strings; @@ -397,7 +398,9 @@ PYBIND11_PLUGIN(_client) { Expected baud rate sensor attempts to decode for NMEA UART input $GPRMC messages.)", py::metaclass()); def_enum(NMEABaudRate, sensor::impl::nmea_baud_rate_strings); - auto ChanField = py::enum_(m, "ChanField", "Channel data block fields.", py::metaclass()); + auto ChanField = py::enum_(m, "ChanField", R"( + Channel data block fields + )", py::metaclass()); def_enum(ChanField, sensor::impl::chanfield_strings); auto UDPProfileLidar = py::enum_(m, "UDPProfileLidar", "UDP lidar profile.", py::metaclass()); @@ -575,23 +578,67 @@ PYBIND11_PLUGIN(_client) { )") .def_readonly_static("N_FIELDS", &LidarScan::N_FIELDS, "Deprecated.") // TODO: Python and C++ API differ in h/w order for some reason - .def("__init__", [](LidarScan& self, size_t h, - size_t w) { new (&self) LidarScan(w, h); }) - .def("__init__", - [](LidarScan& self, size_t h, size_t w, - sensor::UDPProfileLidar profile) { - new (&self) LidarScan(w, h, profile); - }) - .def("__init__", - [](LidarScan& self, size_t h, size_t w, - const std::map& field_types) { - std::map ft; - for (const auto& kv : field_types) { - auto dt = py::dtype::from_args(kv.second); - ft[kv.first] = field_type_of_dtype(dt); - } - new (&self) LidarScan(w, h, ft.begin(), ft.end()); - }) + .def( + "__init__", + [](LidarScan& self, size_t h, size_t w) { + new (&self) LidarScan(w, h); + }, + R"( + + Default constructor creates a 0 x 0 scan + + Args: + height: height of scan + width: width of scan + + Returns: + New LidarScan of 0x0 expecting fields of the LEGACY profile + + )") + .def( + "__init__", + [](LidarScan& self, size_t h, size_t w, + sensor::UDPProfileLidar profile) { + new (&self) LidarScan(w, h, profile); + }, + R"( + + Initialize a scan with the default fields for a particular udp profile + + Args: + height: height of LidarScan, i.e., number of channels + width: width of LidarScan + profile: udp profile + + Returns: + New LidarScan of specified dimensions expecting fields of specified profile + + )") + .def( + "__init__", + [](LidarScan& self, size_t h, size_t w, + const std::map& field_types) { + std::map ft; + for (const auto& kv : field_types) { + auto dt = py::dtype::from_args(kv.second); + ft[kv.first] = field_type_of_dtype(dt); + } + new (&self) LidarScan(w, h, ft.begin(), ft.end()); + }, + R"( + Initialize a scan with a custom set of fields + + Args: + height: height of LidarScan, i.e., number of channels + width: width of LidarScan + fields_dict: dict where keys are ChanFields and values are type, e.g., {client.ChanField.SIGNAL: np.uint32} + + Returns: + New LidarScan of specified dimensions expecting fields specified by dict + + + + )") .def_readonly("w", &LidarScan::w, "Width or horizontal resolution of the scan.") .def_readonly("h", &LidarScan::h, @@ -600,29 +647,16 @@ PYBIND11_PLUGIN(_client) { "frame_id", &LidarScan::frame_id, "Corresponds to the frame id header in the packet format.") .def( - "_complete", + "complete", [](const LidarScan& self, - nonstd::optional window) { - if (!window) window = {0, self.w - 1}; - - const auto& status = self.status(); - auto start = window.value().first; - auto end = window.value().second; - - if (start <= end) - return status.segment(start, end - start + 1) - .unaryExpr([](uint32_t s) { return s & 0x01; }) - .isConstant(0x01); - else - return status.segment(0, end) - .unaryExpr([](uint32_t s) { return s & 0x01; }) - .isConstant(0x01) && - status.segment(start, self.w - start) - .unaryExpr([](uint32_t s) { return s & 0x01; }) - .isConstant(0x01); + nonstd::optional window) { + if (!window) { + window = {0, static_cast(self.w) - 1}; + } + return self.complete(window.value()); }, py::arg("window") = - static_cast>( + static_cast>( nonstd::nullopt)) .def( "field", @@ -768,8 +802,6 @@ PYBIND11_PLUGIN(_client) { return cartesian(scan, self); }); - m.attr("__version__") = VERSION_INFO; - // Image processing py::class_(m, "AutoExposure") .def(py::init<>()) @@ -788,5 +820,7 @@ PYBIND11_PLUGIN(_client) { .def("__call__", &image_proc_call, py::arg("image"), py::arg("update_state") = true); + m.attr("__version__") = ouster::SDK_VERSION; + return m.ptr(); } diff --git a/python/src/cpp/_pcap.cpp b/python/src/cpp/_pcap.cpp index 427167ee..74454e4c 100644 --- a/python/src/cpp/_pcap.cpp +++ b/python/src/cpp/_pcap.cpp @@ -11,6 +11,7 @@ #include #include +#include "ouster/impl/build.h" #include "ouster/os_pcap.h" using namespace ouster::sensor_utils; @@ -84,9 +85,18 @@ This module is generated from the C++ code and not meant to be used directly. // pcap writing py::class_>(m, "record_handle"); - m.def("record_initialize", &record_initialize, py::arg("file_name"), - py::arg("src_ip"), py::arg("dst_ip"), py::arg("frag_size"), - py::arg("use_sll_encapsulation") = false); + m.def("record_initialize", + py::overload_cast(&record_initialize), + py::arg("file_name"), py::arg("src_ip"), py::arg("dst_ip"), + py::arg("frag_size"), py::arg("use_sll_encapsulation") = false, + R"( + ``def record_initialize(file_name: str, src_ip: str, dst_ip: str, frag_size: int, + use_sll_encapsulation: bool = ...) -> record_handle:`` + + Initialize record handle for single sensor pcap files + + )"); m.def("record_uninitialize", [](std::shared_ptr& handle) { record_uninitialize(*handle); @@ -105,5 +115,7 @@ This module is generated from the C++ code and not meant to be used directly. llround(timestamp * 1e6)); }); + m.attr("__version__") = ouster::SDK_VERSION; + return m.ptr(); } diff --git a/python/src/cpp/_viz.cpp b/python/src/cpp/_viz.cpp index cc5a9eef..bdc10bbe 100644 --- a/python/src/cpp/_viz.cpp +++ b/python/src/cpp/_viz.cpp @@ -19,6 +19,7 @@ #include #include +#include "ouster/impl/build.h" #include "ouster/lidar_scan.h" #include "ouster/point_viz.h" #include "ouster/types.h" @@ -68,7 +69,7 @@ using pymatrixd = PYBIND11_PLUGIN(_viz) { py::module m("_viz", R"( - PointViz and LidarScabViz bindings generated by pybind11. + PointViz bindings generated by pybind11. This module is generated from the C++ code and not meant to be used directly. )"); @@ -596,7 +597,7 @@ PYBIND11_PLUGIN(_viz) { {static_cast(viz::calref_n), static_cast(3)}, &viz::calref_palette[0][0]}; - m.attr("__version__") = VERSION_INFO; + m.attr("__version__") = ouster::SDK_VERSION; return m.ptr(); } diff --git a/python/src/ouster/client/_client.pyi b/python/src/ouster/client/_client.pyi index ad685995..3d4c8b43 100644 --- a/python/src/ouster/client/_client.pyi +++ b/python/src/ouster/client/_client.pyi @@ -216,11 +216,12 @@ class PacketFormat: class LidarMode: MODE_UNSPEC: ClassVar[LidarMode] + MODE_512x10: ClassVar[LidarMode] + MODE_512x20: ClassVar[LidarMode] MODE_1024x10: ClassVar[LidarMode] MODE_1024x20: ClassVar[LidarMode] MODE_2048x10: ClassVar[LidarMode] - MODE_512x10: ClassVar[LidarMode] - MODE_512x20: ClassVar[LidarMode] + MODE_4096x5: ClassVar[LidarMode] __members__: ClassVar[Dict[str, LidarMode]] values: ClassVar[Iterator[LidarMode]] @@ -398,6 +399,16 @@ class ChanField: FLAGS: ClassVar[ChanField] FLAGS2: ClassVar[ChanField] NEAR_IR: ClassVar[ChanField] + CUSTOM0: ClassVar[ChanField] + CUSTOM1: ClassVar[ChanField] + CUSTOM2: ClassVar[ChanField] + CUSTOM3: ClassVar[ChanField] + CUSTOM4: ClassVar[ChanField] + CUSTOM5: ClassVar[ChanField] + CUSTOM6: ClassVar[ChanField] + CUSTOM7: ClassVar[ChanField] + CUSTOM8: ClassVar[ChanField] + CUSTOM9: ClassVar[ChanField] RAW32_WORD1: ClassVar[ChanField] RAW32_WORD2: ClassVar[ChanField] RAW32_WORD3: ClassVar[ChanField] @@ -586,7 +597,7 @@ class LidarScan: def status(self) -> ndarray: ... - def _complete(self, window: Optional[Tuple[int, int]] = ...) -> bool: + def complete(self, window: Optional[Tuple[int, int]] = ...) -> bool: ... @property diff --git a/python/src/ouster/client/core.py b/python/src/ouster/client/core.py index 2e1a3de9..e44d4348 100644 --- a/python/src/ouster/client/core.py +++ b/python/src/ouster/client/core.py @@ -2,7 +2,7 @@ Copyright (c) 2021, Ouster, Inc. All rights reserved. - + This module contains more idiomatic wrappers around the lower-level module generated using pybind11. """ @@ -383,7 +383,7 @@ def __iter__(self) -> Iterator[LidarScan]: packet = next(it) except StopIteration: if ls_write is not None: - if not self._complete or ls_write._complete(column_window): + if not self._complete or ls_write.complete(column_window): yield ls_write return @@ -396,7 +396,7 @@ def __iter__(self) -> Iterator[LidarScan]: if batch(packet._data, ls_write): # Got a new frame, return it and start another - if not self._complete or ls_write._complete(column_window): + if not self._complete or ls_write.complete(column_window): yield ls_write start_ts = time.monotonic() ls_write = None diff --git a/python/src/ouster/sdk/examples/client.py b/python/src/ouster/sdk/examples/client.py index 52f7396d..53c77c78 100644 --- a/python/src/ouster/sdk/examples/client.py +++ b/python/src/ouster/sdk/examples/client.py @@ -115,7 +115,12 @@ def filter_3d_by_range_and_azimuth(hostname: str, lidar_port: UDP port to listen on for lidar data range_min: range minimum in meters """ - import matplotlib.pyplot as plt # type: ignore + try: + import matplotlib.pyplot as plt # type: ignore + except ModuleNotFoundError: + print("This example requires matplotlib and an appropriate Matplotlib " + "GUI backend such as TkAgg or Qt5Agg.") + exit(1) import math # set up figure @@ -154,8 +159,8 @@ def filter_3d_by_range_and_azimuth(hostname: str, plt.show() -def live_plot_signal(hostname: str, lidar_port: int = 7502) -> None: - """Display signal from live sensor +def live_plot_reflectivity(hostname: str, lidar_port: int = 7502) -> None: + """Display reflectivity from live sensor Args: hostname: hostname of the sensor @@ -166,7 +171,7 @@ def live_plot_signal(hostname: str, lidar_port: int = 7502) -> None: print("press ESC from visualization to exit") - # [doc-stag-live-plot-signal] + # [doc-stag-live-plot-reflectivity] # establish sensor connection with closing(client.Scans.stream(hostname, lidar_port, complete=False)) as stream: @@ -175,12 +180,12 @@ def live_plot_signal(hostname: str, lidar_port: int = 7502) -> None: for scan in stream: # uncomment if you'd like to see frame id printed # print("frame id: {} ".format(scan.frame_id)) - signal = client.destagger(stream.metadata, - scan.field(client.ChanField.SIGNAL)) - signal = (signal / np.max(signal) * 255).astype(np.uint8) - cv2.imshow("scaled signal", signal) + reflectivity = client.destagger(stream.metadata, + scan.field(client.ChanField.REFLECTIVITY)) + reflectivity = (reflectivity / np.max(reflectivity) * 255).astype(np.uint8) + cv2.imshow("scaled reflectivity", reflectivity) key = cv2.waitKey(1) & 0xFF - # [doc-etag-live-plot-signal] + # [doc-etag-live-plot-reflectivity] # 27 is esc if key == 27: show = False @@ -274,7 +279,7 @@ def main() -> None: "configure-sensor": configure_sensor_params, "fetch-metadata": fetch_metadata, "filter-3d-by-range-and-azimuth": filter_3d_by_range_and_azimuth, - "live-plot-signal": live_plot_signal, + "live-plot-reflectivity": live_plot_reflectivity, "plot-xyz-points": plot_xyz_points, "record-pcap": record_pcap, } diff --git a/python/src/ouster/sdk/examples/pcap.py b/python/src/ouster/sdk/examples/pcap.py index cd09a86e..49e06946 100644 --- a/python/src/ouster/sdk/examples/pcap.py +++ b/python/src/ouster/sdk/examples/pcap.py @@ -18,6 +18,7 @@ from ouster import client, pcap from .colormaps import normalize + def pcap_3d_one_scan(source: client.PacketSource, metadata: client.SensorInfo, num: int = 0) -> None: @@ -49,8 +50,10 @@ def pcap_3d_one_scan(source: client.PacketSource, xyz = client.XYZLut(metadata)(scan) # create point cloud and coordinate axes geometries - cloud = o3d.geometry.PointCloud(o3d.utility.Vector3dVector(xyz.reshape((-1, 3)))) # type: ignore - axes = o3d.geometry.TriangleMesh.create_coordinate_frame(1.0) # type: ignore + cloud = o3d.geometry.PointCloud( + o3d.utility.Vector3dVector(xyz.reshape((-1, 3)))) # type: ignore + axes = o3d.geometry.TriangleMesh.create_coordinate_frame( + 1.0) # type: ignore # [doc-etag-open3d-one-scan] @@ -101,9 +104,9 @@ def pcap_display_xyz_points(source: client.PacketSource, # transform data to 3d points and graph xyzlut = client.XYZLut(metadata) - xyz = xyzlut(scan) + xyz = xyzlut(scan.field(client.ChanField.RANGE)) - key = scan.field(client.ChanField.SIGNAL) + key = scan.field(client.ChanField.REFLECTIVITY) [x, y, z] = [c.flatten() for c in np.dsplit(xyz, 3)] ax.scatter(x, y, z, c=normalize(key.flatten()), s=0.2) @@ -139,12 +142,22 @@ def pcap_to_csv(source: client.PacketSource, csv_ext: file extension to use, "csv" by default """ + dual = False + if metadata.format.udp_profile_lidar == client.UDPProfileLidar.PROFILE_LIDAR_RNG19_RFL8_SIG16_NIR16_DUAL: + dual = True + print("Note: You've selected to convert a dual returns pcap to CSV. Each row " + "will represent a single pixel, so that both returns for that pixel will " + "be on a single row. As this is an example we provide for getting " + "started, we realize that you may have conversion needs which are not met " + "by this function. You can find the source code on the Python SDK " + "documentation website to modify it for your own needs.") + # ensure that base csv_dir exists if not os.path.exists(csv_dir): os.makedirs(csv_dir) # construct csv header and data format - def get_fields_info(scan : client.LidarScan) -> Tuple[str, List[str]]: + def get_fields_info(scan: client.LidarScan) -> Tuple[str, List[str]]: field_names = 'TIMESTAMP (ns)' field_fmts = ['%d'] for chan_field in scan.fields: @@ -154,10 +167,13 @@ def get_fields_info(scan : client.LidarScan) -> Tuple[str, List[str]]: field_fmts.append('%d') field_names += ', X (mm), Y (mm), Z (mm)' field_fmts.extend(3 * ['%d']) + if dual: + field_names += ', X2 (mm), Y2 (mm), Z2 (mm)' + field_fmts.extend(3 * ['%d']) return field_names, field_fmts - field_names : str = '' - field_fmts : List[str] = [] + field_names: str = '' + field_fmts: List[str] = [] # [doc-stag-pcap-to-csv] from itertools import islice @@ -182,10 +198,19 @@ def get_fields_info(scan : client.LidarScan) -> Tuple[str, List[str]]: fields_values = [scan.field(ch) for ch in scan.fields] # use integer mm to avoid loss of precision casting timestamps - xyz = (xyzlut(scan) * 1000).astype(np.int64) + xyz = (xyzlut(scan.field(client.ChanField.RANGE)) * 1000).astype( + np.int64) + + if dual: + xyz2 = (xyzlut(scan.field(client.ChanField.RANGE2)) * 1000).astype( + np.int64) - # get all data as one H x W x 8 int64 array for savetxt() - frame = np.dstack((timestamps, *fields_values, xyz)) + # get all data as one H x W x num fields int64 array for savetxt() + frame = np.dstack((timestamps, *fields_values, xyz, xyz2)) + + else: + # get all data as one H x W x num fields int64 array for savetxt() + frame = np.dstack((timestamps, *fields_values, xyz)) # not necessary, but output points in "image" vs. staggered order frame = client.destagger(metadata, frame) @@ -212,6 +237,13 @@ def pcap_to_las(source: client.PacketSource, las_ext: str = "las") -> None: "Write scans from a pcap to las files (one per lidar scan)." + if (metadata.format.udp_profile_lidar == + client.UDPProfileLidar.PROFILE_LIDAR_RNG19_RFL8_SIG16_NIR16_DUAL): + print("Note: You've selected to convert a dual returns pcap to LAS. " + "Second returns are ignored in this conversion by this example " + "for clarity reasons. You can modify the code as needed by " + "accessing it through Github or the SDK documentation.") + from itertools import islice import laspy # type: ignore @@ -225,7 +257,7 @@ def pcap_to_las(source: client.PacketSource, for idx, scan in enumerate(scans): - xyz = xyzlut(scan) + xyz = xyzlut(scan.field(client.ChanField.RANGE)) las = laspy.create() las.x = xyz[:, :, 0].flatten() @@ -246,6 +278,13 @@ def pcap_to_pcd(source: client.PacketSource, pcd_ext: str = "pcd") -> None: "Write scans from a pcap to pcd files (one per lidar scan)." + if (metadata.format.udp_profile_lidar == + client.UDPProfileLidar.PROFILE_LIDAR_RNG19_RFL8_SIG16_NIR16_DUAL): + print("Note: You've selected to convert a dual returns pcap. Second " + "returns are ignored in this conversion by this example " + "for clarity reasons. You can modify the code as needed by " + "accessing it through github or the SDK documentation.") + from itertools import islice try: import open3d as o3d # type: ignore @@ -268,17 +307,19 @@ def pcap_to_pcd(source: client.PacketSource, for idx, scan in enumerate(scans): - xyz = xyzlut(scan) + xyz = xyzlut(scan.field(client.ChanField.RANGE)) pcd = o3d.geometry.PointCloud() # type: ignore - pcd.points = o3d.utility.Vector3dVector(xyz.reshape(-1, 3)) # type: ignore + pcd.points = o3d.utility.Vector3dVector(xyz.reshape(-1, + 3)) # type: ignore pcd_path = os.path.join(pcd_dir, f'{pcd_base}_{idx:06d}.{pcd_ext}') print(f'write frame #{idx} to file: {pcd_path}') o3d.io.write_point_cloud(pcd_path, pcd) # type: ignore + def pcap_to_ply(source: client.PacketSource, metadata: client.SensorInfo, num: int = 0, @@ -287,6 +328,8 @@ def pcap_to_ply(source: client.PacketSource, ply_ext: str = "ply") -> None: "Write scans from a pcap to ply files (one per lidar scan)." + # Don't need to print warning about dual returns since this leverages pcap_to_pcd + # We are reusing the same Open3d File IO function to write the PLY file out pcap_to_pcd(source, metadata, @@ -295,6 +338,7 @@ def pcap_to_ply(source: client.PacketSource, pcd_base=ply_base, pcd_ext=ply_ext) + def pcap_query_scan(source: client.PacketSource, metadata: client.SensorInfo, num: int = 0) -> None: @@ -388,12 +432,6 @@ def main(): with open(args.metadata_path, 'r') as f: metadata = client.SensorInfo(f.read()) - if (metadata.format.udp_profile_lidar != client.UDPProfileLidar.PROFILE_LIDAR_LEGACY - and metadata.format.udp_profile_lidar != - client.UDPProfileLidar.PROFILE_LIDAR_RNG19_RFL8_SIG16_NIR16 and args.example != 'query-scan'): - print(f"This pcap example is only for pcaps of sensors in LEGACY or SINGLE RETURN mode. Exiting...") - exit(1) - print(f'example: {args.example}') source = pcap.Pcap(args.pcap_path, metadata) diff --git a/python/src/ouster/sdk/examples/viz.py b/python/src/ouster/sdk/examples/viz.py index a5f631dd..f70cfe82 100644 --- a/python/src/ouster/sdk/examples/viz.py +++ b/python/src/ouster/sdk/examples/viz.py @@ -9,8 +9,6 @@ """ import argparse -import weakref -from typing import Iterable from ouster import client, pcap from ouster.sdk import viz import os @@ -59,8 +57,9 @@ def main(): meta_path = os.getenv("SAMPLE_DATA_JSON_PATH", args.meta_path) if not pcap_path or not meta_path: - print("ERROR: Please add SAMPLE_DATA_PCAP_PATH and SAMPLE_DATA_JSON_PATH to" + - " environment variables or pass and ") + print( + "ERROR: Please add SAMPLE_DATA_PCAP_PATH and SAMPLE_DATA_JSON_PATH to" + + " environment variables or pass and ") sys.exit() print(f"Using:\n\tjson: {meta_path}\n\tpcap: {pcap_path}") @@ -85,9 +84,9 @@ def main(): point_viz.run() # [doc-etag-empty-pointviz] - # ========================================================================= - print("Ex 1.0:\tImages and Labels: the Image object and 2D Image set_position() - height-normalized screen coordinates") + print("Ex 1.0:\tImages and Labels: the Image object and 2D Image " + "set_position() - height-normalized screen coordinates") label_top = viz.Label("[0, 1]", 0.5, 0.0, align_top=True) label_top.set_scale(2) @@ -109,7 +108,8 @@ def main(): point_viz.run() # ========================================================================= - print("Ex 1.1:\tImages and Labels: Window-aligned images with 2D Image set_hshift() - width-normalized [-1, 1] horizontal shift") + print("Ex 1.1:\tImages and Labels: Window-aligned images with 2D Image " + "set_hshift() - width-normalized [-1, 1] horizontal shift") # [doc-stag-image-pos-left] # move img to the left @@ -141,7 +141,6 @@ def main(): point_viz.update() point_viz.run() - # remove_objs(point_viz, [label_top, label_mid, label_bot, img]) remove_objs(point_viz, [label_top, label_bot, img]) @@ -153,7 +152,7 @@ def main(): img_aspect = (meta.beam_altitude_angles[0] - meta.beam_altitude_angles[-1]) / 360.0 - img_screen_height = 0.4 # [0..2] + img_screen_height = 0.4 # [0..2] img_screen_len = img_screen_height / img_aspect # prepare field data @@ -161,7 +160,7 @@ def main(): ranges = client.destagger(meta, ranges) ranges = np.divide(ranges, np.amax(ranges), dtype=np.float32) - signal = scan.field(client.ChanField.SIGNAL) + signal = scan.field(client.ChanField.REFLECTIVITY) signal = client.destagger(meta, signal) signal = np.divide(signal, np.amax(signal), dtype=np.float32) @@ -176,8 +175,8 @@ def main(): signal_img = viz.Image() signal_img.set_image(signal) img_aspect = (meta.beam_altitude_angles[0] - - meta.beam_altitude_angles[-1]) / 360.0 - img_screen_height = 0.4 # [0..2] + meta.beam_altitude_angles[-1]) / 360.0 + img_screen_height = 0.4 # [0..2] img_screen_len = img_screen_height / img_aspect # bottom center position signal_img.set_position(-img_screen_len / 2, img_screen_len / 2, -1, @@ -192,13 +191,17 @@ def main(): print("Ex 1.3:\tImages and Labels: Adding labels") # [doc-stag-scan-fields-images-labels] - range_label = viz.Label(str(client.ChanField.RANGE), 0.5, 0, align_top=True) + range_label = viz.Label(str(client.ChanField.RANGE), + 0.5, + 0, + align_top=True) range_label.set_scale(1) point_viz.add(range_label) - signal_label = viz.Label(str(client.ChanField.SIGNAL), - 0.5, 1 - img_screen_height / 2, - align_top=True) + signal_label = viz.Label(str(client.ChanField.REFLECTIVITY), + 0.5, + 1 - img_screen_height / 2, + align_top=True) signal_label.set_scale(1) point_viz.add(signal_label) # [doc-etag-scan-fields-images-labels] @@ -259,10 +262,9 @@ def main(): axis_points = np.hstack((x_ @ line, y_ @ line, z_ @ line)).transpose() # colors for basis vectors - axis_color_mask = np.vstack(( - np.full((axis_n, 4), [1, 0.1, 0.1, 1]), - np.full((axis_n, 4), [0.1, 1, 0.1, 1]), - np.full((axis_n, 4), [0.1, 0.1, 1, 1]))) + axis_color_mask = np.vstack((np.full( + (axis_n, 4), [1, 0.1, 0.1, 1]), np.full((axis_n, 4), [0.1, 1, 0.1, 1]), + np.full((axis_n, 4), [0.1, 0.1, 1, 1]))) cloud_axis = viz.Cloud(axis_points.shape[0]) cloud_axis.set_xyz(axis_points) @@ -323,7 +325,6 @@ def main(): point_viz.update() point_viz.run() - # =============================================== print("Ex 4.0:\tOverlay 2D Images and 2D Labels") @@ -363,7 +364,11 @@ def main(): point_viz.add(img2) # Adding Label for image 2: positioned at top left corner - img_label2 = viz.Label("Second", 1.0, 0.25, align_top=True, align_right=True) + img_label2 = viz.Label("Second", + 1.0, + 0.25, + align_top=True, + align_right=True) img_label2.set_rgba((0.0, 1.0, 1.0, 1)) img_label2.set_scale(1) point_viz.add(img_label2) @@ -373,7 +378,6 @@ def main(): point_viz.update() point_viz.run() - # =============================================================== print("Ex 5.0:\tAdding key handlers: 'R' for random camera dolly") diff --git a/python/tests/conftest.py b/python/tests/conftest.py index 6338cd3f..882e137c 100644 --- a/python/tests/conftest.py +++ b/python/tests/conftest.py @@ -51,6 +51,8 @@ def pytest_collection_modifyitems(items, config) -> None: 'legacy-2.0': 'OS-2-32-U0_v2.0.0_1024x10', 'legacy-2.1': 'OS-1-32-G_v2.1.1_1024x10', 'dual-2.2': 'OS-0-32-U1_v2.2.0_1024x10', + 'single-2.3': 'OS-2-128-U1_v2.3.0_1024x10', + 'low-data-rate-2.3': 'OS-0-128-U1_v2.3.0_1024x10', } diff --git a/python/tests/test_batching.py b/python/tests/test_batching.py index d82bb851..e1b843a1 100644 --- a/python/tests/test_batching.py +++ b/python/tests/test_batching.py @@ -120,7 +120,9 @@ def test_batch_custom_fields(lidar_stream: client.PacketSource) -> None: # create LidarScan with only 2 fields fields: Dict[client.ChanField, client.FieldDType] = { client.ChanField.RANGE: np.uint32, - client.ChanField.SIGNAL: np.uint16 + client.ChanField.SIGNAL: np.uint16, + client.ChanField.CUSTOM0: np.uint8, + client.ChanField.CUSTOM8: np.uint16 } ls = client.LidarScan(info.format.pixels_per_column, @@ -130,6 +132,9 @@ def test_batch_custom_fields(lidar_stream: client.PacketSource) -> None: for f in ls.fields: assert np.count_nonzero(ls.field(f)) == 0 + # set non zero data into users' custom field + ls.field(client.ChanField.CUSTOM8)[:] = 8 + # do batching into ls with a fields subset for p in take(packets_per_frame, lidar_stream): batch(p._data, ls) @@ -139,7 +144,12 @@ def test_batch_custom_fields(lidar_stream: client.PacketSource) -> None: # and the content shouldn't be zero after batching for f in ls.fields: - assert np.count_nonzero(ls.field(f)) > 0 + if f in [client.ChanField.RANGE, client.ChanField.SIGNAL]: + assert np.count_nonzero(ls.field(f)) > 0 + + # custom field data should be preserved after batching + assert np.all(ls.field(client.ChanField.CUSTOM0) == 0) + assert np.all(ls.field(client.ChanField.CUSTOM8) == 8) @pytest.mark.parametrize('test_key', ['legacy-2.0']) diff --git a/python/tests/test_core.py b/python/tests/test_core.py index f038df79..1df3d263 100644 --- a/python/tests/test_core.py +++ b/python/tests/test_core.py @@ -122,7 +122,7 @@ def test_scans_meta(packets: client.PacketSource) -> None: assert len(scan.status) == scan.w assert len(scan.header(ColHeader.ENCODER_COUNT)) == scan.w - assert scan._complete() + assert scan.complete() # all timestamps valid assert np.count_nonzero(scan.timestamp) == scan.w @@ -150,17 +150,25 @@ def test_scans_first_packet(packet: client.LidarPacket, h = packet._pf.pixels_per_column w = packet._pf.columns_per_packet - assert np.array_equal(packet.field(ChanField.RANGE), - scan.field(ChanField.RANGE)[:h, :w]) + is_low_data_rate_profile = (packets.metadata.format.udp_profile_lidar == + client.UDPProfileLidar.PROFILE_LIDAR_RNG15_RFL8_NIR8) + + if not is_low_data_rate_profile: # low data rate profile RANGE is scaled up + assert np.array_equal(packet.field(ChanField.RANGE), + scan.field(ChanField.RANGE)[:h, :w]) assert np.array_equal(packet.field(ChanField.REFLECTIVITY), scan.field(ChanField.REFLECTIVITY)[:h, :w]) - assert np.array_equal(packet.field(ChanField.SIGNAL), - scan.field(ChanField.SIGNAL)[:h, :w]) + if is_low_data_rate_profile: # low data rate profile has no SIGNAL + assert ChanField.SIGNAL not in set(scan.fields) + else: + assert np.array_equal(packet.field(ChanField.SIGNAL), + scan.field(ChanField.SIGNAL)[:h, :w]) - assert np.array_equal(packet.field(ChanField.NEAR_IR), - scan.field(ChanField.NEAR_IR)[:h, :w]) + if not is_low_data_rate_profile: # low data rate profile NEAR_IR is scaled up + assert np.array_equal(packet.field(ChanField.NEAR_IR), + scan.field(ChanField.NEAR_IR)[:h, :w]) assert packet.frame_id == scan.frame_id diff --git a/python/tests/test_data.py b/python/tests/test_data.py index ec2b3235..9e603683 100644 --- a/python/tests/test_data.py +++ b/python/tests/test_data.py @@ -59,14 +59,20 @@ def test_lidar_packet(meta: client.SensorInfo) -> None: w = pf.columns_per_packet h = pf.pixels_per_column + scan_has_signal = (meta.format.udp_profile_lidar != + client.UDPProfileLidar.PROFILE_LIDAR_RNG15_RFL8_NIR8) + assert len( - client.ChanField.__members__) == 13, "Don't forget to update tests!" + client.ChanField.__members__) == 23, "Don't forget to update tests!" assert np.array_equal(p.field(client.ChanField.RANGE), np.zeros((h, w))) assert np.array_equal(p.field(client.ChanField.REFLECTIVITY), np.zeros((h, w))) - assert np.array_equal(p.field(client.ChanField.SIGNAL), np.zeros((h, w))) assert np.array_equal(p.field(client.ChanField.NEAR_IR), np.zeros((h, w))) + if scan_has_signal: + assert np.array_equal(p.field(client.ChanField.SIGNAL), np.zeros( + (h, w))) + assert len( client.ColHeader.__members__) == 5, "Don't forget to update tests!" assert np.array_equal(p.header(client.ColHeader.TIMESTAMP), np.zeros(w)) @@ -84,7 +90,7 @@ def test_lidar_packet(meta: client.SensorInfo) -> None: # should not be able to modify packet data with pytest.raises(ValueError): - p.field(client.ChanField.SIGNAL)[0] = 1 + p.field(client.ChanField.REFLECTIVITY)[0] = 1 with pytest.raises(ValueError): p.header(client.ColHeader.MEASUREMENT_ID)[0] = 1 @@ -196,40 +202,40 @@ def test_scan_not_complete() -> None: ls = client.LidarScan(32, 1024) status = ls.status - assert not ls._complete() + assert not ls.complete() status[0] = 0x02 - assert not ls._complete() - assert not ls._complete((0, 0)) + assert not ls.complete() + assert not ls.complete((0, 0)) status[1:] = 0xFFFFFFFF - assert not ls._complete() + assert not ls.complete() status[:] = 0xFFFFFFFF status[-1] = 0x02 - assert not ls._complete() + assert not ls.complete() # windows are inclusive but python slicing is not status[:] = 0x00 status[:10] = 0xFFFFFFFF - assert not ls._complete((0, 10)) + assert not ls.complete((0, 10)) status[:] = 0x00 status[11:21] = 0xFFFFFFFF - assert not ls._complete((10, 20)) + assert not ls.complete((10, 20)) # window [i, i] status[:] = 0x00 status[0] = 0xFFFFFFFF - assert not ls._complete() - assert not ls._complete((0, 1)) - assert ls._complete((0, 0)) + assert not ls.complete() + assert not ls.complete((0, 1)) + assert ls.complete((0, 0)) status[:] = 0x00 status[128] = 0xFFFFFFFF - assert not ls._complete() - assert not ls._complete((127, 128)) - assert ls._complete((128, 128)) + assert not ls.complete() + assert not ls.complete((127, 128)) + assert ls.complete((128, 128)) @pytest.mark.parametrize("w, win_start, win_end", [ @@ -250,7 +256,7 @@ def test_scan_not_complete() -> None: (2048, 511, 511), ]) def test_scan_complete(w, win_start, win_end) -> None: - """Set the status headers to the specified window and check _complete().""" + """Set the status headers to the specified window and check complete().""" ls = client.LidarScan(32, w) status = ls.status @@ -261,7 +267,7 @@ def test_scan_complete(w, win_start, win_end) -> None: status[0:win_end + 1] = 0xFFFFFFFF status[win_start:] = 0xFFFFFFFF - assert ls._complete((win_start, win_end)) + assert ls.complete((win_start, win_end)) def test_scan_fields_ref() -> None: @@ -309,6 +315,31 @@ def test_scan_dual_profile() -> None: } +def test_scan_low_data_rate() -> None: + """Low Data Rate scan has the expected fields.""" + ls = client.LidarScan(32, 1024, + client.UDPProfileLidar.PROFILE_LIDAR_RNG15_RFL8_NIR8) + + assert set(ls.fields) == { + client.ChanField.RANGE, + client.ChanField.REFLECTIVITY, + client.ChanField.NEAR_IR, + } + + +def test_scan_single_return() -> None: + """Single Return scan has the expected fields.""" + ls = client.LidarScan( + 32, 1024, client.UDPProfileLidar.PROFILE_LIDAR_RNG19_RFL8_SIG16_NIR16) + + assert set(ls.fields) == { + client.ChanField.RANGE, + client.ChanField.REFLECTIVITY, + client.ChanField.SIGNAL, + client.ChanField.NEAR_IR, + } + + def test_scan_empty() -> None: """Sanity check scan with no fields.""" ls = client.LidarScan(32, 1024, {}) @@ -322,13 +353,19 @@ def test_scan_empty() -> None: def test_scan_custom() -> None: """Sanity check scan with a custom set of fields.""" - ls = client.LidarScan(32, 1024, { - client.ChanField.SIGNAL: np.uint16, - client.ChanField.FLAGS: np.uint8 - }) + ls = client.LidarScan( + 32, 1024, { + client.ChanField.SIGNAL: np.uint16, + client.ChanField.FLAGS: np.uint8, + client.ChanField.CUSTOM0: np.uint32 + }) - assert set(ls.fields) == {client.ChanField.SIGNAL, client.ChanField.FLAGS} + assert set(ls.fields) == { + client.ChanField.SIGNAL, client.ChanField.FLAGS, + client.ChanField.CUSTOM0 + } assert ls.field(client.ChanField.SIGNAL).dtype == np.uint16 + assert ls.field(client.ChanField.CUSTOM0).dtype == np.uint32 with pytest.raises(ValueError): ls.field(client.ChanField.RANGE) @@ -376,7 +413,7 @@ def test_scan_copy_eq() -> None: ls0 = client.LidarScan(32, 512) ls0.status[:] = 0x1 - ls0.field(client.ChanField.SIGNAL)[:] = 100 + ls0.field(client.ChanField.REFLECTIVITY)[:] = 100 ls1 = deepcopy(ls0) @@ -400,3 +437,26 @@ def test_scan_copy_eq() -> None: ls1.field(client.ChanField.RANGE)[0, 0] = 42 assert ls0 == ls1 + + +def test_scan_eq_with_custom_fields() -> None: + """Test equality with custom fields.""" + + ls0 = client.LidarScan(32, 512, { + client.ChanField.CUSTOM0: np.uint32, + client.ChanField.CUSTOM4: np.uint8 + }) + + ls1 = deepcopy(ls0) + + ls0.field(client.ChanField.CUSTOM0)[:] = 100 + + ls2 = deepcopy(ls0) + + assert np.count_nonzero( + ls2.field(client.ChanField.CUSTOM0) == 100) == ls0.h * ls0.w + assert np.count_nonzero(ls2.field(client.ChanField.CUSTOM4) == 100) == 0 + + assert ls1 is not ls0 + assert ls1 != ls0 + assert ls2 == ls0 diff --git a/python/tests/test_metadata.py b/python/tests/test_metadata.py index d4b14279..8d121c4a 100644 --- a/python/tests/test_metadata.py +++ b/python/tests/test_metadata.py @@ -40,6 +40,7 @@ def test_timestamp_mode_misc() -> None: (client.LidarMode.MODE_1024x10, 1024, 10, "1024x10"), (client.LidarMode.MODE_1024x20, 1024, 20, "1024x20"), (client.LidarMode.MODE_2048x10, 2048, 10, "2048x10"), + (client.LidarMode.MODE_4096x5, 4096, 5, "4096x5"), ]) def test_lidar_mode(mode, cols, frequency, string) -> None: """Check lidar mode (un)parsing and cols/frequency.""" @@ -53,7 +54,7 @@ def test_lidar_mode(mode, cols, frequency, string) -> None: def test_lidar_mode_misc() -> None: """Check some misc properties of lidar mode.""" assert len( - client.LidarMode.__members__) == 6, "Don't forget to update tests!" + client.LidarMode.__members__) == 7, "Don't forget to update tests!" assert client.LidarMode.from_string('foo') == client.LidarMode.MODE_UNSPEC assert client.LidarMode(0) == client.LidarMode.MODE_UNSPEC assert str(client.LidarMode.MODE_UNSPEC) == "UNKNOWN" diff --git a/python/tests/test_pcap.py b/python/tests/test_pcap.py index aea478ec..16c30b4b 100644 --- a/python/tests/test_pcap.py +++ b/python/tests/test_pcap.py @@ -18,7 +18,7 @@ from ouster import client from ouster.client import _client -SLL_PROTO = 42 +SLL_PROTO = 113 ETH_PROTO = 1 UDP_PROTO = 17 @@ -27,7 +27,6 @@ def fake_packets(metadata: client.SensorInfo, n_lidar: int = 0, n_imu: int = 0, timestamped: bool = False) -> Iterator[client.Packet]: - pf = _client.PacketFormat.from_info(metadata) current_ts = time.time() diff --git a/python/tests/test_viz.py b/python/tests/test_viz.py index d108024c..eada810f 100644 --- a/python/tests/test_viz.py +++ b/python/tests/test_viz.py @@ -24,7 +24,7 @@ pytestmark = pytest.mark.interactive -def make_checker_board(square_size: int, reps:Tuple[int, int]) -> np.ndarray: +def make_checker_board(square_size: int, reps: Tuple[int, int]) -> np.ndarray: img_data = np.full((square_size, square_size), 0) img_data = np.hstack([img_data, np.logical_xor(img_data, 1)]) img_data = np.vstack([img_data, np.logical_xor(img_data, 1)]) @@ -75,8 +75,10 @@ def test_point_viz_image_with_labels_aligned(point_viz: viz.PointViz) -> None: white_rgba = (1.0, 1.0, 1.0, 1) gray_rgba = (0.5, 0.5, 0.5, 1) - colors = [red_rgba, blue_rgba, green_rgba, yellow_rgba, cyan_rgba, magenta_rgba, white_rgba, - gray_rgba] + colors = [ + red_rgba, blue_rgba, green_rgba, yellow_rgba, cyan_rgba, magenta_rgba, + white_rgba, gray_rgba + ] ylen = 0.15 xlen_calc = lambda vlen, shape: shape[1] * vlen / shape[0] @@ -84,22 +86,33 @@ def test_point_viz_image_with_labels_aligned(point_viz: viz.PointViz) -> None: label_posy = lambda posy: 1 - (posy + 1) / 2 def gen_rand_img(): - return make_checker_board(10, (random.randrange(2, 5), random.randrange(2, 5))) + return make_checker_board( + 10, (random.randrange(2, 5), random.randrange(2, 5))) def gen_rand_color(): return random.choice(colors) - def add_image(im_data, xpos, ypos, hshift = 0): + def add_image(im_data, xpos, ypos, hshift=0): img = viz.Image() img.set_position(*xpos, *ypos) img.set_image(im_data) img.set_hshift(hshift) point_viz.add(img) - def add_label(text, xpos, ypos, align_right=False, align_top=False, rgba=None, scale=2): + def add_label(text, + xpos, + ypos, + align_right=False, + align_top=False, + rgba=None, + scale=2): xpos = label_posx(xpos) ypos = label_posy(ypos) - label = viz.Label(text, xpos, ypos, align_right=align_right, align_top=align_top) + label = viz.Label(text, + xpos, + ypos, + align_right=align_right, + align_top=align_top) label.set_rgba(rgba if rgba else gen_rand_color()) label.set_scale(scale) point_viz.add(label) @@ -107,15 +120,15 @@ def add_label(text, xpos, ypos, align_right=False, align_top=False, rgba=None, s # center img_data = gen_rand_img() xlen = xlen_calc(ylen, img_data.shape) - ypos = [- ylen / 2, ylen / 2] # center - xpos = [- xlen / 2, xlen / 2] + ypos = [-ylen / 2, ylen / 2] # center + xpos = [-xlen / 2, xlen / 2] add_image(img_data, xpos, ypos, 0) add_label("Center", 0, ypos[1]) # top left img_data = gen_rand_img() xlen = xlen_calc(ylen, img_data.shape) - ypos = [1 - ylen, 1] # top + ypos = [1 - ylen, 1] # top xpos = [0, xlen] add_image(img_data, xpos, ypos, -1.0) add_label("Top Left - top", -1.0, ypos[1], align_top=True) @@ -124,29 +137,53 @@ def add_label(text, xpos, ypos, align_right=False, align_top=False, rgba=None, s # top right img_data = gen_rand_img() xlen = xlen_calc(ylen, img_data.shape) - ypos = [1 - ylen, 1] # top - xpos = [- xlen, 0] + ypos = [1 - ylen, 1] # top + xpos = [-xlen, 0] add_image(img_data, xpos, ypos, 1.0) - add_label("Top Right - top", 1.0, ypos[1], align_right=True, align_top=True) - add_label("Top Right - bottom", 1.0, ypos[0], align_right=True, align_top=False) + add_label("Top Right - top", + 1.0, + ypos[1], + align_right=True, + align_top=True) + add_label("Top Right - bottom", + 1.0, + ypos[0], + align_right=True, + align_top=False) # bottom left img_data = gen_rand_img() xlen = xlen_calc(ylen, img_data.shape) - ypos = [-1, -1 + ylen] # bottom + ypos = [-1, -1 + ylen] # bottom xpos = [0, xlen] add_image(img_data, xpos, ypos, -1.0) - add_label("Bottom Left - top", -1.0, ypos[1], align_right=False, align_top=True) - add_label("Bottom Left - bottom", -1.0, ypos[0], align_right=False, align_top=False) + add_label("Bottom Left - top", + -1.0, + ypos[1], + align_right=False, + align_top=True) + add_label("Bottom Left - bottom", + -1.0, + ypos[0], + align_right=False, + align_top=False) # bottom right img_data = gen_rand_img() xlen = xlen_calc(ylen, img_data.shape) - ypos = [-1, -1 + ylen] # bottom + ypos = [-1, -1 + ylen] # bottom xpos = [-xlen, 0] add_image(img_data, xpos, ypos, 1.0) - add_label("Bottom Right - top", 1.0, ypos[1], align_right=True, align_top=True) - add_label("Bottom Right - bottom", 1.0, ypos[0], align_right=True, align_top=False) + add_label("Bottom Right - top", + 1.0, + ypos[1], + align_right=True, + align_top=True) + add_label("Bottom Right - bottom", + 1.0, + ypos[0], + align_right=True, + align_top=False) point_viz.update() point_viz.run() diff --git a/python/tox.ini b/python/tox.ini index ec1aabaf..a32c85c5 100644 --- a/python/tox.ini +++ b/python/tox.ini @@ -1,5 +1,5 @@ [tox] -envlist = py{36,37,38,39,310} +envlist = py{37,38,39,310} isolated_build = true skip_missing_interpreters = true @@ -16,7 +16,7 @@ commands = --junit-prefix="{env:ID}__{env:VERSION_ID}__{envname}" \ --junitxml="{env:ARTIFACT_DIR}/tox-tests/ouster-sdk-{env:ID}-{env:VERSION_ID}-{envname}.xml" -[testenv:py{36,37,38,39,310}-use_wheels] +[testenv:py{37,38,39,310}-use_wheels] description = installs ouster-sdk-python from wheels and runs tests passenv = WHEELS_DIR skipsdist = true @@ -35,25 +35,29 @@ commands = [testenv:flake] description = checking style with flake8 +skip_install = true deps = flake8 - flake8-docstrings - flake8-html + flake8_formatter_junit_xml +commands = + mkdir -p {env:ARTIFACT_DIR}/lint + flake8 --format=junit-xml --output-file={env:ARTIFACT_DIR}/lint/flake.xml ./src ./tests + +[testenv:mypy] +description = check types with mypy +deps = mypy commands = - mkdir -p {env:ARTIFACT_DIR} - flake8 --exit-zero --format=html --htmldir={env:ARTIFACT_DIR}/flake-report src tests + mypy --junit-xml {env:ARTIFACT_DIR}/lint/mypy.xml ./src ./tests [flake8] -statistics = true -tee = true max-line-length = 120 per-file-ignores = tests/*: D docs/*: D ignore = - # E125 continuation line indentation, yapf doesn't fix this + # E125, E126, E128 continuation line indentation, yapf doesn't fix this E125, - # E128 more continuation line indentation + E126, E128, # E251 newlines around equals in keywords, yapf again E251, diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 64915779..0bb921c8 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -1,5 +1,3 @@ -cmake_minimum_required(VERSION 3.1.0) - #Several deprecations since gtest 1.8 if(MSVC) add_compile_options(/wd4996 /wd4189) diff --git a/tests/lidar_scan_test.cpp b/tests/lidar_scan_test.cpp index de9e8edf..e61ef8bc 100644 --- a/tests/lidar_scan_test.cpp +++ b/tests/lidar_scan_test.cpp @@ -84,7 +84,7 @@ TEST(LidarScan, EmptyConstructorInit) { EXPECT_EQ(scan.frame_id, -1); - EXPECT_EQ(scan.headers.size(), 0); + EXPECT_EQ(scan.headers.size(), 0u); EXPECT_EQ(scan.end() - scan.begin(), 0); @@ -99,8 +99,8 @@ TEST(LidarScan, LegacyConstructorInit) { EXPECT_EQ(scan.h, h); EXPECT_EQ(scan.frame_id, -1); - int count = 0; - int hit_count = 0; + size_t count = 0; + size_t hit_count = 0; std::vector field_copy; for (auto item : legacy_field_slots) { field_copy.push_back(std::get<0>(item)); @@ -114,7 +114,7 @@ TEST(LidarScan, LegacyConstructorInit) { } count++; } - EXPECT_EQ(field_copy.size(), 0); + EXPECT_EQ(field_copy.size(), 0u); EXPECT_EQ(hit_count, count); EXPECT_EQ(legacy_field_slots.size(), count); @@ -134,8 +134,8 @@ TEST(LidarScan, DualReturnConstructorInit) { EXPECT_EQ(scan.h, h); EXPECT_EQ(scan.frame_id, -1); - int count = 0; - int hit_count = 0; + size_t count = 0; + size_t hit_count = 0; std::vector field_copy; for (auto item : dual_field_slots) { field_copy.push_back(std::get<0>(item)); @@ -149,7 +149,7 @@ TEST(LidarScan, DualReturnConstructorInit) { } count++; } - EXPECT_EQ(field_copy.size(), 0); + EXPECT_EQ(field_copy.size(), 0u); EXPECT_EQ(hit_count, count); EXPECT_EQ(dual_field_slots.size(), count); @@ -169,8 +169,8 @@ TEST(LidarScan, CustomFieldConstructorInit) { EXPECT_EQ(scan.h, h); EXPECT_EQ(scan.frame_id, -1); - int count = 0; - int hit_count = 0; + size_t count = 0; + size_t hit_count = 0; std::vector field_copy; for (auto item : contrived_slots) { field_copy.push_back(std::get<0>(item)); @@ -185,7 +185,7 @@ TEST(LidarScan, CustomFieldConstructorInit) { } count++; } - EXPECT_EQ(field_copy.size(), 0); + EXPECT_EQ(field_copy.size(), 0u); EXPECT_EQ(hit_count, count); EXPECT_EQ(contrived_slots.size(), count); @@ -325,3 +325,19 @@ TEST(LidarScan, DataCheck) { EXPECT_TRUE(scan1 != scan3); } } + +TEST(LidarScan, CustomUserFields) { + using LidarScanFieldTypes = std::vector< + std::pair>; + + LidarScanFieldTypes user_fields{ + {ChanField::CUSTOM0, ChanFieldType::UINT8}, + {ChanField::CUSTOM3, ChanFieldType::UINT64}, + {ChanField::CUSTOM9, ChanFieldType::UINT16}}; + + ouster::LidarScan user_scan(10, 10, user_fields.begin(), user_fields.end()); + + EXPECT_EQ(3, std::distance(user_scan.begin(), user_scan.end())); + + zero_check_fields(user_scan); +} diff --git a/tests/pcaps/OS-0-128-U1_v2.3.0_1024x10.json b/tests/pcaps/OS-0-128-U1_v2.3.0_1024x10.json new file mode 100644 index 00000000..47bea351 --- /dev/null +++ b/tests/pcaps/OS-0-128-U1_v2.3.0_1024x10.json @@ -0,0 +1,464 @@ +{ + "base_pn": "", + "base_sn": "", + "beam_altitude_angles": + [ + 45.75, + 44.7, + 43.93, + 43.47, + 42.72, + 41.68, + 40.94, + 40.46, + 39.7, + 38.69, + 37.95, + 37.47, + 36.71, + 35.72, + 35, + 34.51, + 33.74, + 32.78, + 32.06, + 31.56, + 30.8, + 29.85, + 29.13, + 28.62, + 27.86, + 26.93, + 26.23, + 25.7, + 24.93, + 24.03, + 23.33, + 22.79, + 22.03, + 21.16, + 20.46, + 19.91, + 19.13, + 18.3, + 17.6, + 17.04, + 16.27, + 15.45, + 14.76, + 14.19, + 13.41, + 12.62, + 11.94, + 11.34, + 10.55, + 9.81, + 9.13, + 8.51, + 7.71, + 6.99, + 6.31, + 5.68, + 4.87, + 4.17, + 3.5, + 2.86, + 2.06, + 1.37, + 0.71, + 0.04, + -0.77, + -1.43, + -2.09, + -2.77, + -3.6, + -4.23, + -4.9, + -5.6, + -6.43, + -7.05, + -7.71, + -8.43, + -9.24, + -9.85, + -10.51, + -11.28, + -12.07, + -12.65, + -13.34, + -14.1, + -14.91, + -15.48, + -16.16, + -16.95, + -17.77, + -18.32, + -18.99, + -19.8, + -20.62, + -21.17, + -21.84, + -22.68, + -23.5, + -24.02, + -24.7, + -25.56, + -26.39, + -26.9, + -27.57, + -28.46, + -29.29, + -29.78, + -30.47, + -31.36, + -32.21, + -32.68, + -33.37, + -34.29, + -35.14, + -35.6, + -36.29, + -37.24, + -38.11, + -38.55, + -39.24, + -40.21, + -41.08, + -41.51, + -42.23, + -43.22, + -44.09, + -44.52, + -45.24, + -46.26 + ], + "beam_azimuth_angles": + [ + 11.24, + 3.93, + -3.29, + -10.34, + 10.81, + 3.75, + -3.18, + -10, + 10.41, + 3.61, + -3.1, + -9.7, + 10.09, + 3.49, + -3.02, + -9.45, + 9.81, + 3.39, + -2.96, + -9.23, + 9.56, + 3.3, + -2.91, + -9.05, + 9.35, + 3.21, + -2.87, + -8.88, + 9.15, + 3.14, + -2.83, + -8.76, + 8.99, + 3.06, + -2.8, + -8.63, + 8.85, + 3.02, + -2.79, + -8.55, + 8.73, + 2.96, + -2.77, + -8.49, + 8.64, + 2.93, + -2.77, + -8.41, + 8.55, + 2.89, + -2.76, + -8.38, + 8.5, + 2.86, + -2.77, + -8.37, + 8.45, + 2.83, + -2.77, + -8.35, + 8.41, + 2.82, + -2.77, + -8.35, + 8.39, + 2.8, + -2.8, + -8.39, + 8.39, + 2.79, + -2.82, + -8.43, + 8.4, + 2.8, + -2.83, + -8.47, + 8.42, + 2.79, + -2.87, + -8.55, + 8.46, + 2.79, + -2.92, + -8.63, + 8.51, + 2.78, + -2.96, + -8.73, + 8.58, + 2.8, + -3.01, + -8.86, + 8.66, + 2.83, + -3.07, + -9.01, + 8.78, + 2.85, + -3.14, + -9.17, + 8.9, + 2.88, + -3.22, + -9.35, + 9.05, + 2.91, + -3.31, + -9.57, + 9.23, + 2.95, + -3.41, + -9.83, + 9.44, + 3.01, + -3.51, + -10.12, + 9.7, + 3.08, + -3.64, + -10.47, + 9.99, + 3.15, + -3.8, + -10.88, + 10.34, + 3.24, + -3.98, + -11.35 + ], + "build_date": "2022-04-14T21:11:47Z", + "build_rev": "v2.3.0", + "client_version": "ouster_client 0.3.0", + "data_format": + { + "column_window": + [ + 0, + 1023 + ], + "columns_per_frame": 1024, + "columns_per_packet": 16, + "pixel_shift_by_row": + [ + 64, + 43, + 23, + 3, + 63, + 43, + 23, + 4, + 62, + 42, + 23, + 4, + 61, + 42, + 23, + 5, + 60, + 42, + 24, + 6, + 59, + 41, + 24, + 6, + 59, + 41, + 24, + 7, + 58, + 41, + 24, + 7, + 58, + 41, + 24, + 7, + 57, + 41, + 24, + 8, + 57, + 40, + 24, + 8, + 57, + 40, + 24, + 8, + 56, + 40, + 24, + 8, + 56, + 40, + 24, + 8, + 56, + 40, + 24, + 8, + 56, + 40, + 24, + 8, + 56, + 40, + 24, + 8, + 56, + 40, + 24, + 8, + 56, + 40, + 24, + 8, + 56, + 40, + 24, + 8, + 56, + 40, + 24, + 7, + 56, + 40, + 24, + 7, + 56, + 40, + 23, + 7, + 57, + 40, + 23, + 6, + 57, + 40, + 23, + 6, + 57, + 40, + 23, + 5, + 58, + 40, + 23, + 5, + 58, + 40, + 22, + 4, + 59, + 41, + 22, + 3, + 60, + 41, + 22, + 2, + 60, + 41, + 21, + 1, + 61, + 41, + 21, + 0 + ], + "pixels_per_column": 128, + "udp_profile_imu": "LEGACY", + "udp_profile_lidar": "RNG15_RFL8_NIR8" + }, + "hostname": "", + "image_rev": "ousteros-image-prod-aries-v2.3.0+20220415163956", + "imu_to_sensor_transform": + [ + 1, + 0, + 0, + 6.253, + 0, + 1, + 0, + -11.775, + 0, + 0, + 1, + 7.645, + 0, + 0, + 0, + 1 + ], + "initialization_id": 5431292, + "json_calibration_version": 4, + "lidar_mode": "1024x10", + "lidar_origin_to_beam_origin_mm": 27.67, + "lidar_to_sensor_transform": + [ + -1, + 0, + 0, + 0, + 0, + -1, + 0, + 0, + 0, + 0, + 1, + 36.18, + 0, + 0, + 0, + 1 + ], + "prod_line": "OS-0-128", + "prod_pn": "840-103574-06", + "prod_sn": "122150000150", + "proto_rev": "", + "status": "RUNNING", + "udp_port_imu": 7503, + "udp_port_lidar": 7502 +} \ No newline at end of file diff --git a/tests/pcaps/OS-0-128-U1_v2.3.0_1024x10.pcap b/tests/pcaps/OS-0-128-U1_v2.3.0_1024x10.pcap new file mode 100644 index 00000000..18ca30b5 Binary files /dev/null and b/tests/pcaps/OS-0-128-U1_v2.3.0_1024x10.pcap differ diff --git a/tests/pcaps/OS-0-128-U1_v2.3.0_1024x10_digest.json b/tests/pcaps/OS-0-128-U1_v2.3.0_1024x10_digest.json new file mode 100644 index 00000000..eba7e3f0 --- /dev/null +++ b/tests/pcaps/OS-0-128-U1_v2.3.0_1024x10_digest.json @@ -0,0 +1,26 @@ +{ + "packet_hash": { + "TIMESTAMP": "7180800dc6e5641c5d01686a3cef19b2", + "ENCODER_COUNT": "620f0b67a91f7f74151bc5be745b7110", + "MEASUREMENT_ID": "ab4e111268cce8326714d9308aafd5fe", + "STATUS": "24736efb524c4c76e8d5aca0c04ff392", + "FRAME_ID": "c99a74c555371a433d121f551d6c6398", + "RANGE": "d3534495ce22ab669ed773c8c7b5fe10", + "REFLECTIVITY": "4f306b1768c4bb14b18af92f59e0e1fd", + "NEAR_IR": "bcc5eca360e337b68ae3f6edce36ed0c", + "FLAGS": "6524418ef0b26ac92d75f0e2c060a235", + "RAW32_WORD1": "6312573eb9dfea925785d75cf92e57d5" + }, + "scans": [ + { + "FRAME_ID": "1491", + "TIMESTAMP": "7180800dc6e5641c5d01686a3cef19b2", + "STATUS": "e497f5aaabed8bfebd1b713872dd3ab9", + "MEASUREMENT_ID": "ab4e111268cce8326714d9308aafd5fe", + "ENCODER_COUNT": "0829f71740aab1ab98b33eae21dee122", + "RANGE": "04aba38789e3e0ea52c08938b49c0cb1", + "REFLECTIVITY": "33dd535b7db8850b53594391898fb45b", + "NEAR_IR": "3d272728a86fe98101c976e1fbb30d3b" + } + ] +} diff --git a/tests/pcaps/OS-0-32-U1_v2.2.0_1024x10_digest.json b/tests/pcaps/OS-0-32-U1_v2.2.0_1024x10_digest.json index a4337d13..536a260d 100644 --- a/tests/pcaps/OS-0-32-U1_v2.2.0_1024x10_digest.json +++ b/tests/pcaps/OS-0-32-U1_v2.2.0_1024x10_digest.json @@ -3,7 +3,7 @@ "TIMESTAMP": "060e7ccbcf80ef9632dec5688e47a6d1", "ENCODER_COUNT": "620f0b67a91f7f74151bc5be745b7110", "MEASUREMENT_ID": "ab4e111268cce8326714d9308aafd5fe", - "STATUS": "ebf96a2536e5a247f7ef6edb3f66292a", + "STATUS": "24736efb524c4c76e8d5aca0c04ff392", "FRAME_ID": "c99a74c555371a433d121f551d6c6398", "RANGE": "03ec29cd989be70a882090996e8969a1", "RANGE2": "211a3b157ce6c31e36082d1b6b7be681", @@ -13,13 +13,17 @@ "REFLECTIVITY2": "34a0f11475e553768c4aeb42989a0666", "NEAR_IR": "50d1fe8a5c054a1ce84133f18d01b824", "FLAGS": "0bb2f219090b02f48d8c1998009e1e4d", - "FLAGS2": "0b53385e7cb208e773b89756fc97714d" + "FLAGS2": "0b53385e7cb208e773b89756fc97714d", + "RAW32_WORD1": "fab0923fc675a584b1c6735d8b21f711", + "RAW32_WORD2": "b546da92280bf5a8fb73009077302ff9", + "RAW32_WORD3": "72846d8908567605ad7a77184ebc96bd", + "RAW32_WORD4": "7ca8e6a1d79d9adcb2629e9129baae62" }, "scans": [ { "FRAME_ID": "1453", "TIMESTAMP": "060e7ccbcf80ef9632dec5688e47a6d1", - "STATUS": "4c81b69242689aca8a87c05726f5322a", + "STATUS": "e497f5aaabed8bfebd1b713872dd3ab9", "MEASUREMENT_ID": "ab4e111268cce8326714d9308aafd5fe", "ENCODER_COUNT": "0829f71740aab1ab98b33eae21dee122", "RANGE": "b7e7a2a5aa86516f86fd2e8f56b8f6b0", diff --git a/tests/pcaps/OS-2-128-U1_v2.3.0_1024x10.json b/tests/pcaps/OS-2-128-U1_v2.3.0_1024x10.json new file mode 100644 index 00000000..d919a625 --- /dev/null +++ b/tests/pcaps/OS-2-128-U1_v2.3.0_1024x10.json @@ -0,0 +1,464 @@ +{ + "base_pn": "", + "base_sn": "", + "beam_altitude_angles": + [ + 10.76, + 10.58, + 10.42, + 10.24, + 10.1, + 9.93, + 9.76, + 9.57, + 9.41, + 9.24, + 9.09, + 8.91, + 8.75, + 8.57, + 8.41, + 8.23, + 8.08, + 7.9, + 7.74, + 7.55, + 7.39, + 7.21, + 7.03, + 6.86, + 6.7, + 6.53, + 6.35, + 6.18, + 6.02, + 5.83, + 5.67, + 5.5, + 5.32, + 5.14, + 4.97, + 4.81, + 4.63, + 4.46, + 4.28, + 4.11, + 3.95, + 3.76, + 3.58, + 3.43, + 3.24, + 3.07, + 2.9, + 2.72, + 2.55, + 2.37, + 2.21, + 2.03, + 1.87, + 1.69, + 1.51, + 1.32, + 1.16, + 0.98, + 0.81, + 0.63, + 0.46, + 0.29, + 0.11, + -0.05, + -0.25, + -0.42, + -0.58, + -0.76, + -0.93, + -1.11, + -1.28, + -1.45, + -1.63, + -1.81, + -1.97, + -2.16, + -2.32, + -2.5, + -2.68, + -2.85, + -3.02, + -3.19, + -3.37, + -3.55, + -3.73, + -3.9, + -4.07, + -4.25, + -4.4, + -4.58, + -4.75, + -4.94, + -5.11, + -5.28, + -5.46, + -5.64, + -5.8, + -5.96, + -6.15, + -6.32, + -6.49, + -6.66, + -6.84, + -7, + -7.17, + -7.35, + -7.51, + -7.69, + -7.86, + -8.04, + -8.22, + -8.38, + -8.55, + -8.72, + -8.89, + -9.06, + -9.23, + -9.4, + -9.57, + -9.74, + -9.91, + -10.07, + -10.24, + -10.41, + -10.58, + -10.75, + -10.92, + -11.09 + ], + "beam_azimuth_angles": + [ + 2.07, + 0.66, + -0.71, + -2.08, + 2.07, + 0.69, + -0.7, + -2.09, + 2.06, + 0.67, + -0.71, + -2.06, + 2.06, + 0.69, + -0.71, + -2.07, + 2.07, + 0.69, + -0.71, + -2.07, + 2.07, + 0.69, + -0.7, + -2.08, + 2.05, + 0.68, + -0.69, + -2.06, + 2.06, + 0.69, + -0.69, + -2.07, + 2.08, + 0.69, + -0.69, + -2.06, + 2.08, + 0.69, + -0.69, + -2.07, + 2.08, + 0.69, + -0.69, + -2.05, + 2.08, + 0.7, + -0.68, + -2.06, + 2.09, + 0.7, + -0.67, + -2.05, + 2.08, + 0.7, + -0.68, + -2.05, + 2.08, + 0.71, + -0.67, + -2.06, + 2.08, + 0.71, + -0.68, + -2.05, + 2.07, + 0.71, + -0.67, + -2.05, + 2.08, + 0.7, + -0.68, + -2.05, + 2.09, + 0.71, + -0.66, + -2.05, + 2.09, + 0.71, + -0.67, + -2.05, + 2.09, + 0.71, + -0.66, + -2.05, + 2.1, + 0.71, + -0.66, + -2.05, + 2.09, + 0.73, + -0.67, + -2.05, + 2.1, + 0.72, + -0.66, + -2.05, + 2.09, + 0.72, + -0.66, + -2.05, + 2.11, + 0.73, + -0.69, + -2.03, + 2.1, + 0.72, + -0.65, + -2.04, + 2.11, + 0.72, + -0.67, + -2.05, + 2.1, + 0.72, + -0.66, + -2.04, + 2.11, + 0.72, + -0.66, + -2.04, + 2.1, + 0.72, + -0.65, + -2.03, + 2.11, + 0.73, + -0.66, + -2.04 + ], + "build_date": "2022-04-14T21:11:47Z", + "build_rev": "v2.3.0", + "client_version": "ouster_client 0.3.0", + "data_format": + { + "column_window": + [ + 0, + 1023 + ], + "columns_per_frame": 1024, + "columns_per_packet": 16, + "pixel_shift_by_row": + [ + 12, + 8, + 4, + 0, + 12, + 8, + 4, + 0, + 12, + 8, + 4, + 0, + 12, + 8, + 4, + 0, + 12, + 8, + 4, + 0, + 12, + 8, + 4, + 0, + 12, + 8, + 4, + 0, + 12, + 8, + 4, + 0, + 12, + 8, + 4, + 0, + 12, + 8, + 4, + 0, + 12, + 8, + 4, + 0, + 12, + 8, + 4, + 0, + 12, + 8, + 4, + 0, + 12, + 8, + 4, + 0, + 12, + 8, + 4, + 0, + 12, + 8, + 4, + 0, + 12, + 8, + 4, + 0, + 12, + 8, + 4, + 0, + 12, + 8, + 4, + 0, + 12, + 8, + 4, + 0, + 12, + 8, + 4, + 0, + 12, + 8, + 4, + 0, + 12, + 8, + 4, + 0, + 12, + 8, + 4, + 0, + 12, + 8, + 4, + 0, + 12, + 8, + 4, + 0, + 12, + 8, + 4, + 0, + 12, + 8, + 4, + 0, + 12, + 8, + 4, + 0, + 12, + 8, + 4, + 0, + 12, + 8, + 4, + 0, + 12, + 8, + 4, + 0 + ], + "pixels_per_column": 128, + "udp_profile_imu": "LEGACY", + "udp_profile_lidar": "RNG19_RFL8_SIG16_NIR16" + }, + "hostname": "", + "image_rev": "ousteros-image-prod-aries-v2.3.0+20220415163956", + "imu_to_sensor_transform": + [ + 1, + 0, + 0, + 6.253, + 0, + 1, + 0, + -11.775, + 0, + 0, + 1, + 11.645, + 0, + 0, + 0, + 1 + ], + "initialization_id": 5431293, + "json_calibration_version": 4, + "lidar_mode": "1024x10", + "lidar_origin_to_beam_origin_mm": 13.762, + "lidar_to_sensor_transform": + [ + -1, + 0, + 0, + 0, + 0, + -1, + 0, + 0, + 0, + 0, + 1, + 78.296, + 0, + 0, + 0, + 1 + ], + "prod_line": "OS-2-128", + "prod_pn": "840-103576-06", + "prod_sn": "992210000957", + "proto_rev": "", + "status": "RUNNING", + "udp_port_imu": 7503, + "udp_port_lidar": 7502 +} \ No newline at end of file diff --git a/tests/pcaps/OS-2-128-U1_v2.3.0_1024x10.pcap b/tests/pcaps/OS-2-128-U1_v2.3.0_1024x10.pcap new file mode 100644 index 00000000..502ed93d Binary files /dev/null and b/tests/pcaps/OS-2-128-U1_v2.3.0_1024x10.pcap differ diff --git a/tests/pcaps/OS-2-128-U1_v2.3.0_1024x10_digest.json b/tests/pcaps/OS-2-128-U1_v2.3.0_1024x10_digest.json new file mode 100644 index 00000000..181fec4a --- /dev/null +++ b/tests/pcaps/OS-2-128-U1_v2.3.0_1024x10_digest.json @@ -0,0 +1,30 @@ +{ + "packet_hash": { + "TIMESTAMP": "1f59c7327c5d01386c3055a08324a003", + "ENCODER_COUNT": "620f0b67a91f7f74151bc5be745b7110", + "MEASUREMENT_ID": "ab4e111268cce8326714d9308aafd5fe", + "STATUS": "24736efb524c4c76e8d5aca0c04ff392", + "FRAME_ID": "c99a74c555371a433d121f551d6c6398", + "RANGE": "1d0dc85d743a1d7846ff1e398a304fd0", + "SIGNAL": "cc687e735b3746a30c675816a3e75d67", + "REFLECTIVITY": "015c9db0f368c7c3d05d882eedb74c5a", + "NEAR_IR": "f34755723d2375c47b75025015cdd531", + "FLAGS": "0dfbe8aa4c20b52e1b8bf3cb6cbdf193", + "RAW32_WORD1": "f391606c041dc8322ec731f75739fd07", + "RAW32_WORD2": "144ae7ef89691b79c3d8182b17fc0fc3", + "RAW32_WORD3": "54d67e6f4548b6da42d7af7b7865bd0f" + }, + "scans": [ + { + "FRAME_ID": "1259", + "TIMESTAMP": "1f59c7327c5d01386c3055a08324a003", + "STATUS": "e497f5aaabed8bfebd1b713872dd3ab9", + "MEASUREMENT_ID": "ab4e111268cce8326714d9308aafd5fe", + "ENCODER_COUNT": "0829f71740aab1ab98b33eae21dee122", + "RANGE": "33c7da9fdc46569efda75264e95e1693", + "SIGNAL": "ec22cca5f78098b1d8ec35df8ced5cda", + "REFLECTIVITY": "d1c4e56e3ebf42a149fd6aa4966c95d5", + "NEAR_IR": "5d3f606712e020ab731410f908616063" + } + ] +}