diff --git a/.gitignore b/.gitignore deleted file mode 100644 index 54f35a4b..00000000 --- a/.gitignore +++ /dev/null @@ -1,7 +0,0 @@ -/CMakeLists.txt -.catkin_tools/ -build/ -devel/ -logs/ -install/ -doc/ diff --git a/CHANGELOG.md b/CHANGELOG.md deleted file mode 100644 index b2325311..00000000 --- a/CHANGELOG.md +++ /dev/null @@ -1,148 +0,0 @@ -# Changelog - -## [1.14.0-beta.14] - 2020-09-02 -### Added -- support for ROS noetic in `ouster_ros`. Note: this may break building on very old platforms - without a C++14-capable compiler -- an extra extrinsics field in `sensor_info` for conveniently passing around an extra user-supplied - transform -- a utility function to convert `lidar_scan` data between the "staggered" representation where each - column has the same timestamp and "de-staggered" representation where each column has the same - azimuth angle -- mask support in the visualizer library in `ouster_viz` - -### Changed -- `ouster_ros` now requires C++14 to support building against noetic libraries -- replaced `batch_to_iter` with `batch_to_scan`, a simplified function that writes directly to - a `lidar_scan` instead of arbitrary iterator - -### Fixed -- ipv6 support using dual-stack sockets on all supported platforms. This was broken since the - beta.10 release -- projection to Cartesian coordinates now takes into account the vertical offset the sensor and - lidar frames -- the reference frame of point cloud topics in `ouster_ros` is now correctly reported as the "sensor - frame" defined in the user guide - -## [1.14.0-beta.12] - 2020-07-10 -*no changes* - -## [1.14.0-beta.11] - 2020-06-17 -*no changes* - -## [1.14.0-beta.10] - 2020-05-21 -### Added -- preliminary support for Windows and Mac 10.15 for `ouster_viz` and - `ouster_client` - -### Changed -- replaced VTK visualizer library with one based on GLFW -- renamed all instances of "OS1" including namespaces, headers, node and topic - names, to reflect support for other product lines -- updated all xyz point cloud calculations to take into account new - `lidar_origin_to_beam_origin` parameter reported by sensors -- client and `os_node` and `simple_viz` now avoid setting the lidar and timestamp - modes when connecting to a client unless values are explicitly specicified - -### Fixed -- increase the UDP receive buffer size in the client to reduce chances of - dropping packets on platforms with low defaults -- `os_cloud_node` output now uses the updated point cloud calculation, taking - into account the lidar origin offset -- minor regression with destaggering in img_node output in previous beta - -## [1.14.0-beta.4] - 2020-03-17 -### Added -- support for gen2 hardware in client, visualizer, and ROS sample code -- support for updated "packed" lidar UDP data format for 16 and - 32-beam devices with firmware 1.14 -- range markers in `simple_viz` and `viz_node`. Toggle display using - `g` key. Distances can be configured from `os1.launch`. -- post-processing to improve ambient image uniformity in visualizer - -### Changed -- use random ports for lidar and imu data by default when unspecified - -## [1.13.0] - 2020-03-16 -### Added -- post-processing to improve ambient image uniformity in visualizer -- make timestamp mode configurable via the client (PR #97) - -### Changed -- turn on position-independent code by default to make using code in libraries - easier (PR #65) -- use random ports for lidar and imu data by default when unspecified - -### Fixed -- prevent legacy tf prefix from making invalid frame names (PR #56) -- use `iterator_traits` to make `batch_to_iter` work with more types (PR #70) -- use correct name for json dependency in `package.xml` (PR #116) -- handle udp socket creation error gracefully in client - -## [1.12.0] - 2019-05-02 -### Added -- install directives for `ouster_ros` build (addresses #50) - -### Changed -- flip the sign on IMU acceleration output to follow usual conventions -- increase the update rate in the visualizer to ~60hz - -### Fixed -- visualizer issue where the point cloud would occasionally occasionally not be - displayed using newer versions of Eigen - -## [1.11.0] - 2019-03-26 -### Added -- allow renaming tf ids using the `tf_prefix` parameter - -### Changed -- use frame id to batch packets so client code deals with reordered lidar - packets without splitting frames -- use a uint32_t for PointOS1 timestamps to avoid unnecessary loss of precision - -### Fixed -- bug causing ring and reflectivity to be corrupted in os1_cloud_node output -- misplaced sine in azimuth angle calculation (addresses #42) -- populate timestamps on image node output (addresses #39) - -## [1.10.0] - 2019-01-27 -### Added -- `os1_node` now queries and uses calibrated beam angles from the sensor -- `os1_node` now queries and uses imu / lidar frames from the sensor -- `os1_node` reads and writes metadata to `${ROS_HOME}` to support replaying - data with calibration -- ROS example code now publishes tf2 transforms for imu / lidar frames - (addresses #12) -- added `metadata` parameter to `os1.launch` to override location of metadata -- added `viz` parameter to `os1.launch` to run the example visualizer with ROS -- added `image` parameter to `os1.launch` to publish image topics to rviz - (addresses #21) -- added range field to `PointOS1` - -### Changed -- split point-cloud publishing out of `os1_node` into `os1_cloud_node` -- example visualizer controls: - + press `m` to cycle through color modes instead of `i`, `z`, `Z`, `r` - + `r` now resets the camera position - + range/intensity images automatically resized to fit window height -- updated OS-1 client to use newer TCP configuration commands -- updated OS-1 client to set the requested lidar mode, reinitialize on connection -- changed point cloud batching to be based on angle rather than scan duration -- `ouster_client` now depends on the `jsoncpp` library -- switched order of fields in `PointOS1` to be compatible with `PointXYZI` - (addresses #16) -- moved example visualizer VTK rendering into the main thread (merged #23) -- the timestamp field of PointOS1 now represents time since the start of the - scan (the timestamp of the PointCloud2 message) in nanoseconds - -### Removed -- removed keyboard camera controls in example visualizer -- removed panning and rotating of the image panel in example visualizer - -### Fixed -- no longer dropping UDP packets in 2048 mode on tested hardware -- example visualizer: - + point cloud display focus no longer snaps back on rotation - + fixed clipping issues with parallel projection - + fixed point coloring issues in z-color mode - + improved performance diff --git a/CHANGELOG.rst b/CHANGELOG.rst new file mode 100644 index 00000000..ef1dd25d --- /dev/null +++ b/CHANGELOG.rst @@ -0,0 +1,229 @@ +========= +Changelog +========= + +[20201202] +========== + +Changed +------- + +* switched to date-based version scheme. No longer tracking firmware versions +* added a top-level ``CMakeLists.txt``. Client and visualizer should no longer be built + separately. See the README for updated build instructions +* cmake cleanup, including using custom "find modules" to provide better compatibility between + different versions of cmake +* respect standard cmake ``BUILD_SHARED_LIBS`` and ``CMAKE_POSITION_INDEPENDENT_CODE`` flags +* make ``ouster_ros`` easier to use as a dependency by bundling the client and viz libraries + together into a single library that can be used through catkin +* updated client example code. Now uses more of the client APIs to capture data and write to a + CSV. See ``ouster_client/src/example.cpp`` +* replace callback-based ``batch_to_scan`` function with ``ScanBatcher``. See ``lidar_scan.h`` for + API docs and the new client example code +* update ``LidarScan`` API. Now includes accessors for measurement blocks as well as channel data + fields. See ``lidar_scan.h`` for API docs +* add client version field to metadata json, logs, and help text +* client API renaming to better reflect the Sensor Software Manual + +[1.14.0-beta.14] - 2020-08-27 +============================= + +Added +----- + +* support for ROS noetic in ``ouster_ros``. Note: this may break building on very old platforms + without a C++14-capable compiler +* an extra extrinsics field in ``sensor_info`` for conveniently passing around an extra user-supplied + transform +* a utility function to convert ``lidar_scan`` data between the "staggered" representation where each + column has the same timestamp and "de-staggered" representation where each column has the same + azimuth angle +* mask support in the visualizer library in ``ouster_viz`` + +Changed +------- + +* ``ouster_ros`` now requires C++14 to support building against noetic libraries +* replaced ``batch_to_iter`` with ``batch_to_scan``, a simplified function that writes directly to a + ``lidar_scan`` instead of arbitrary iterator + +Fixed +----- + +* ipv6 support using dual-stack sockets on all supported platforms. This was broken since the + beta.10 release +* projection to Cartesian coordinates now takes into account the vertical offset the sensor and + lidar frames +* the reference frame of point cloud topics in ``ouster_ros`` is now correctly reported as the "sensor + frame" defined in the user guide + +[1.14.0-beta.12] - 2020-07-10 +============================= + +*no changes* + +[1.14.0-beta.11] - 2020-06-17 +============================= + +*no changes* + +[1.14.0-beta.10] - 2020-05-21 +============================= + +Added +----- + +* preliminary support for Windows and macOS for ``ouster_viz`` and ``ouster_client`` + +Changed +------- + +* replaced VTK visualizer library with one based on GLFW +* renamed all instances of "OS1" including namespaces, headers, node and topic names, to reflect + support for other product lines +* updated all xyz point cloud calculations to take into account new ``lidar_origin_to_beam_origin`` + parameter reported by sensors +* client and ``os_node`` and ``simple_viz`` now avoid setting the lidar and timestamp modes when + connecting to a client unless values are explicitly specicified + +Fixed +----- + +* increase the UDP receive buffer size in the client to reduce chances of dropping packets on + platforms with low defaults +* ``os_cloud_node`` output now uses the updated point cloud calculation, taking into account the lidar + origin offset +* minor regression with destaggering in img_node output in previous beta + +[1.14.0-beta.4] - 2020-03-17 +============================ + +Added +----- + +* support for gen2 hardware in client, visualizer, and ROS sample code +* support for updated "packed" lidar UDP data format for 16 and 32-beam devices with firmware 1.14 +* range markers in ``simple_viz`` and ``viz_node``. Toggle display using ``g`` key. Distances can be + configured from ``os1.launch``. +* post-processing to improve ambient image uniformity in visualizer + +Changed +------- + +* use random ports for lidar and imu data by default when unspecified + +[1.13.0] - 2020-03-16 +===================== + +Added +----- + +* post-processing to improve ambient image uniformity in visualizer +* make timestamp mode configurable via the client (PR #97) + +Changed +------- + +* turn on position-independent code by default to make using code in libraries easier (PR #65) +* use random ports for lidar and imu data by default when unspecified + +Fixed +----- + +* prevent legacy tf prefix from making invalid frame names (PR #56) +* use ``iterator_traits`` to make ``batch_to_iter`` work with more types (PR #70) +* use correct name for json dependency in ``package.xml`` (PR #116) +* handle udp socket creation error gracefully in client + +[1.12.0] - 2019-05-02 +===================== + +Added +----- + +* install directives for ``ouster_ros`` build (addresses #50) + +Changed +------- + +* flip the sign on IMU acceleration output to follow usual conventions +* increase the update rate in the visualizer to ~60hz + +Fixed +----- + +* visualizer issue where the point cloud would occasionally occasionally not be displayed using + newer versions of Eigen + +[1.11.0] - 2019-03-26 +===================== + +Added +----- + +* allow renaming tf ids using the ``tf_prefix`` parameter + +Changed +------- + +* use frame id to batch packets so client code deals with reordered lidar packets without splitting + frames +* use a uint32_t for PointOS1 timestamps to avoid unnecessary loss of precision + +Fixed +----- + +* bug causing ring and reflectivity to be corrupted in os1_cloud_node output +* misplaced sine in azimuth angle calculation (addresses #42) +* populate timestamps on image node output (addresses #39) + +[1.10.0] - 2019-01-27 +===================== + +Added +----- + +* ``os1_node`` now queries and uses calibrated beam angles from the sensor +* ``os1_node`` now queries and uses imu / lidar frames from the sensor +* ``os1_node`` reads and writes metadata to ``${ROS_HOME}`` to support replaying data with calibration +* ROS example code now publishes tf2 transforms for imu / lidar frames (addresses #12) +* added ``metadata`` parameter to ``os1.launch`` to override location of metadata +* added ``viz`` parameter to ``os1.launch`` to run the example visualizer with ROS +* added ``image`` parameter to ``os1.launch`` to publish image topics to rviz (addresses #21) +* added range field to ``PointOS1`` + +Changed +------- + +* split point-cloud publishing out of ``os1_node`` into ``os1_cloud_node`` +* example visualizer controls: + + - press ``m`` to cycle through color modes instead of ``i``, ``z``, ``Z``, ``r`` + - ``r`` now resets the camera position + - range/signal images automatically resized to fit window height + +* updated OS-1 client to use newer TCP configuration commands +* updated OS-1 client to set the requested lidar mode, reinitialize on connection +* changed point cloud batching to be based on angle rather than scan duration +* ``ouster_client`` now depends on the ``jsoncpp`` library +* switched order of fields in ``PointOS1`` to be compatible with ``PointXYZI`` (addresses #16) +* moved example visualizer VTK rendering into the main thread (merged #23) +* the timestamp field of PointOS1 now represents time since the start of the scan (the timestamp of + the PointCloud2 message) in nanoseconds + +Removed +------- + +* removed keyboard camera controls in example visualizer +* removed panning and rotating of the image panel in example visualizer + +Fixed +----- + +* no longer dropping UDP packets in 2048 mode on tested hardware +* example visualizer: + + - point cloud display focus no longer snaps back on rotation + - fixed clipping issues with parallel projection + - fixed point coloring issues in z-color mode + - improved visualizer performance diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 00000000..bc9d499f --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,27 @@ +cmake_minimum_required(VERSION 3.1.0) + +list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake) +include(DefaultBuildType) + +# ==== Project Name ==== +project(ouster_example) + +# ==== Options ==== +option(CMAKE_POSITION_INDEPENDENT_CODE "Build position independent code." ON) +option(BUILD_SHARED_LIBS "Build shared libraries." OFF) +option(BUILD_VIZ "Build Ouster visualizer." ON) + +set(CMAKE_CXX_STANDARD 11) +if(MSVC) + add_compile_options(/W3) + add_compile_definitions(NOMINMAX _USE_MATH_DEFINES WIN32_LEAN_AND_MEAN) +else() + add_compile_options(-Wall -Wextra -Werror) +endif() + +# === Subdirectories === +add_subdirectory(ouster_client) + +if(BUILD_VIZ) + add_subdirectory(ouster_viz) +endif() diff --git a/ouster_client/CMakeSettings.json b/CMakeSettings.json old mode 100755 new mode 100644 similarity index 100% rename from ouster_client/CMakeSettings.json rename to CMakeSettings.json diff --git a/README-WINDOWS.md b/README-WINDOWS.md deleted file mode 100644 index 0b8310e5..00000000 --- a/README-WINDOWS.md +++ /dev/null @@ -1,36 +0,0 @@ -# Requirements -- Visual Studio Installed [Download Here](https://visualstudio.microsoft.com/downloads/) -- Visual Studio CMake Support [Doc Here](https://docs.microsoft.com/en-us/cpp/build/cmake-projects-in-visual-studio?view=vs-2019) -- Visual Studio CPP Support [Doc Here](https://docs.microsoft.com/en-us/cpp/build/vscpp-step-0-installation?view=vs-2019) - -# Dependency Install -1. Download vcpkg [Download Here](https://github.com/microsoft/vcpkg/archive/master.zip) - - vcpkg is a dependency manager developed by microsoft, [more info located here](https://github.com/microsoft/vcpkg) -2. Extract the vcpkg zip file to somewhere on your filesystem -3. Open 'Developer Command Prompt for VS' from start menu -4. Run the following steps in the command window (Make sure visual studio is not open during this) [Doc Here](https://docs.microsoft.com/en-us/cpp/build/vcpkg?view=vs-2019) - 1. `cd ` - - Replace with the path of the extracted vcpkg folder - 2. `bootstrap-vcpkg.bat` - - This will start building the vcpkg framework - 3. `.\vcpkg integrate install` - - This will integrate vcpkg with your Visual Studio IDE - 4. `.\vcpkg install --triplet x64-windows glfw3 glew tclap jsoncpp eigen3` - - This will build and install the dependencies - - **NOTE: This will take a while** - -# Opening The Project -1. Start Visual Studio -2. When the prompt opens asking you what type of project to open click 'Open a local folder' -3. Open one of the following - 1. ouster_client - - This is just the generic example program - 2. ouster_viz - - This is the example visualizer -4. Make sure Visual Studio is building in release mode [Doc Here](https://docs.microsoft.com/en-us/visualstudio/debugger/how-to-set-debug-and-release-configurations?view=vs-2019) -5. Go to Build -> Build All - - **NOTE: If 'Build All' is not under the build menu there may have been a cmake issue** - - **look at the error output to see if there is an error** - - **you may need to regenerate the cmake cache by going to the following** - - Project -> Generate cache for .... -6. To run make sure you allow the client/visualizer through the windows firewall diff --git a/README.md b/README.md deleted file mode 100644 index 04c14d64..00000000 --- a/README.md +++ /dev/null @@ -1,14 +0,0 @@ -# Ouster Example Code -Sample code for connecting to and configuring ouster sensors, reading and -visualizing data, and interfacing with ROS. - -See the `README.md` in each subdirectory for details. - -## Contents -* [ouster_client/](ouster_client/README.md) contains an example C++ client for ouster sensors -* [ouster_viz/](ouster_viz/README.md) contains a basic visualizer ouster sensors -* [ouster_ros/](ouster_ros/README.md) contains example ROS nodes for publishing point cloud messages -* **NOTE:** Please ignore the CMakeSettings.json files as they are specific to windows -## Sample Data -* Sample sensor output usable with the provided ROS code is available - [here](https://data.ouster.io/sample-data-1.13) diff --git a/README.rst b/README.rst new file mode 100644 index 00000000..bc8ff40e --- /dev/null +++ b/README.rst @@ -0,0 +1,280 @@ +=================== +Ouster Example Code +=================== + +:Description: Sample code provided for working with Ouster sensors + +.. contents:: Contents: + :local: + + +Summary +======= + +Sample code for connecting to and configuring ouster sensors, reading and visualizing data, and +interfacing with ROS. + +* `ouster_client `_ contains an example C++ client for ouster sensors +* `ouster_viz `_ contains a basic point cloud visualizer +* `ouster_ros `_ contains example ROS nodes for publishing point cloud messages + +To get started building the client and visualizer libraries, see the ``Building`` section below. For +instructions on ROS, start with the ``ROS`` section. + + +Sample Client and Visualizer +============================ + +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 +requires the GLFW3 and GLEW libraries. + +Building on Linux / macOS +------------------------- + +To install build dependencies on Ubuntu, run:: + + sudo apt install build-essential cmake libglfw3-dev libglew-dev libeigen3-dev \ + libjsoncpp-dev libtclap-dev + +On macOS, install XCode and `homebrew `_ and run:: + + brew install cmake pkg-config glfw glew eigen jsoncpp tclap + +To build run the following commands:: + + mkdir build + cd build + cmake -DCMAKE_BUILD_TYPE=Release + make + +where ```` is the location of the ``ouster_example`` source directory. The +CMake build script supports several optional flags:: + + -DBUILD_VIZ=OFF Do not build the sample visualizer + -DBUILD_SHARED_LIBS Build shared libraries (.dylib or .so) + -DCMAKE_POSITION_INDEPENDENT_CODE Standard flag for position independent code + +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: + +* `Visual Studio `_ +* `Visual Studio CMake Support + `_ +* `Visual Studio CPP Support + `_ +* `Vcpkg, at tag "2020.07" installed and integrated with Visual Studio + `_ + +.. note:: + + You'll need to run ``git checkout 2020.07`` in the vcpkg directory before bootstrapping 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:: + + .\vcpkg.exe integrate install + +You should be able to install dependencies with:: + + .\vcpkg.exe install --triplet x64-windows glfw3 glew tclap jsoncpp eigen3 + +After these steps are complete, you should be able to open, build and run the ``ouster_example`` +project using Visual Studio: + +1. Start Visual Studio. +2. When the prompt opens asking you what type of project to open click **Open a local folder** and + navigate to the ``ouster_example`` source directory. +3. After opening the project for the first time, wait for CMake configuration to complete. +4. Make sure Visual Studio is `building in release mode`_. You may experience performance issues and + 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:: + + .\out\build\x64-Release\ouster_client\ouster_client_example.exe -h + +.. _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:: + + ./ouster_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. + +On Windows, you may need to allow the client/visualizer through the Windows firewall to receive +sensor data. + +Running the Sample Visualizer +----------------------------- + +Navigate to ``ouster_viz`` under the build directory, which should contain an executable named +``simple_viz`` . Run:: + + ./simple_viz + +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. + +For usage and other options, run ``./simple_viz -h`` Mouse controls: + +* Click and drag rotates the view +* Middle click and drag moves the view +* Scroll adjusts how far away the camera is from the vehicle + +Keyboard controls: + + ============= ============================================ + key what it does + ============= ============================================ + ``p`` Increase point size + ``o`` Decrease point size + ``m`` Cycle point cloud coloring mode + ``v`` Toggle color cycling in range image + ``n`` Toggle display near-IR image from the sensor + ``r`` Toggle auto-rotating + ``shift + r`` Reset camera + ``e`` Change range and signal image size + ``;`` Increase spacing in range markers + ``'`` Decrease spacing in range markers + ``r`` Toggle auto rotate + ``w`` Camera pitch up + ``s`` Camera pitch down + ``a`` Camera yaw left + ``d`` Camera yaw right + ``1`` Toggle point cloud visibility + ``0`` Toggle orthographic camera + ``=`` Zoom in + ``-`` Zoom out + ``shift`` Camera Translation with mouse drag + ============= ============================================ + + +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 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. + +Building +-------- + +The build dependencies include those of the sample code:: + + sudo apt install build-essential cmake libglfw3-dev libglew-dev libeigen3-dev \ + libjsoncpp-dev libtclap-dev + +and, additionally:: + + sudo apt install ros--ros-core ros--pcl-ros \ + ros--tf2-geometry-msgs ros--rviz + +where ```` is ``kinetic``, ``melodic``, or ``noetic``. To build:: + + source /opt/ros//setup.bash + mkdir -p ./myworkspace/src + cd myworkspace + ln -s ./src/ + 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. + +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 + +Running ROS Nodes with a Live Sensor +------------------------------------ + +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. + +To publish ROS topics from a running sensor, run:: + + roslaunch ouster_ros ouster.launch sensor_hostname:= \ + udp_dest:= \ + metadata:= \ + lidar_mode:= viz:= + +where: + +* ```` can be the hostname (os-99xxxxxxxxxx) or IP of the sensor +* ```` is the hostname or IP to which the sensor should send data +* ```` is an optional path to json file to save calibration metadata +* ```` is one of ``512x10``, ``512x20``, ``1024x10``, ``1024x20``, or ``2048x10``, and +* ```` is either ``true`` or ``false``: if true, a window should open and start displaying data + after a few seconds. + +Note that if the ``metadata`` parameter is not specified, this command will write metadata to +``${ROS_HOME}``. By default, the name of this file is based on the hostname of the sensor, +e.g. ``os-99xxxxxxxxxx.json``. + +Recording Data +-------------- + +To record raw sensor output use `rosbag record`_. After starting the ``roslaunch`` command above, in +another terminal, run:: + + rosbag record /os_node/imu_packets /os_node/lidar_packets` + +This will save a bag file of recorded data in the current working directory. + +It's recommended to +copy and save the metadata file at ``$(ROS_HOME)/.json`` alongside the bag. + +.. _rosbag record: https://wiki.ros.org/rosbag/Commandline#rosbag_record + +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:= + +And in a second terminal run `rosbag play`_:: + + rosbag play --clock + +If a metadata file is not available, the visualizer will default to ``1024x10``. This can be +overridden with the ``lidar_mode`` parameter. Visualizer output will only be correct if the same +``lidar_mode`` parameter is used for both recording and replay. + +.. _rosbag play: https://wiki.ros.org/rosbag/Commandline#rosbag_play + +Visualizing Data in Rviz +------------------------ + +To display sensor output using built-in ROS tools (rviz), follow the instructions above for running +the example ROS code with a sensor or recorded data. Then, run:: + + rviz -d ouster_example/ouster_ros/viz.rviz + +in another terminal with the ROS environment set up. To view lidar intensity, near-IR, and range +images, add ``image:=true`` to the ``roslaunch`` command above. + + +Additional Information +====================== + +* Sample sensor output usable with the provided ROS code `is available here + `_. +* For network configuration, refer to "Connecting to the Sensor" in the `Software User Manual`_. diff --git a/cmake/DefaultBuildType.cmake b/cmake/DefaultBuildType.cmake new file mode 100644 index 00000000..0e290439 --- /dev/null +++ b/cmake/DefaultBuildType.cmake @@ -0,0 +1,6 @@ +# default to "Release" if build type is not set +if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES) + message(STATUS "Build type not specified, using: Release") + set(CMAKE_BUILD_TYPE "Release" CACHE STRING "Choose the type of build." FORCE) + set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS "Debug" "MinSizeRel" "Release" "RelWithDebInfo") +endif() diff --git a/cmake/FindEigen3.cmake b/cmake/FindEigen3.cmake new file mode 100644 index 00000000..5b41116f --- /dev/null +++ b/cmake/FindEigen3.cmake @@ -0,0 +1,18 @@ +# Define forwards-compatible imported target for old platforms (Ubuntu 16.04) + +include(FindPackageHandleStandardArgs) + +find_package(Eigen3 CONFIG QUIET) + +find_package_handle_standard_args(Eigen3 + REQUIRED_VARS EIGEN3_INCLUDE_DIR + VERSION_VAR EIGEN3_VERSION +) + +if(NOT TARGET Eigen3::Eigen) + add_library(Eigen3::Eigen INTERFACE IMPORTED) + + set_target_properties(Eigen3::Eigen PROPERTIES + INTERFACE_INCLUDE_DIRECTORIES ${EIGEN3_INCLUDE_DIR}) +endif() + diff --git a/cmake/FindOpenGL.cmake b/cmake/FindOpenGL.cmake new file mode 100644 index 00000000..954e7340 --- /dev/null +++ b/cmake/FindOpenGL.cmake @@ -0,0 +1,23 @@ +# 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 new file mode 100644 index 00000000..37cb3cd2 --- /dev/null +++ b/cmake/Findglfw3.cmake @@ -0,0 +1,20 @@ +# For ROS/catkin compatibility only. Set variables expected by catkin to allow exporting GLFW3 in +# the DEPENDS clause of the catkin_package() macro. + +include(FindPackageHandleStandardArgs) + +find_package(glfw3 CONFIG REQUIRED) + +# Package on >=18.04 sets a target, 16.04 uses default vars +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 +) diff --git a/cmake/Findjsoncpp.cmake b/cmake/Findjsoncpp.cmake new file mode 100644 index 00000000..7ea278c7 --- /dev/null +++ b/cmake/Findjsoncpp.cmake @@ -0,0 +1,43 @@ +# Try to find a cmake config compatible with vcpkg and fall back to defining +# targets using pkgconfig, which seems more consistent for jsoncpp across +# different platforms. + +include(FindPackageHandleStandardArgs) + +# recent jsoncpp config shipped with vcpkg defines this target +find_package(jsoncpp CONFIG QUIET) +if(WIN32 OR (jsoncpp_FOUND AND TARGET jsoncpp_lib)) + find_package_handle_standard_args(jsoncpp DEFAULT_MSG jsoncpp_CONFIG) + return() +endif() + +# fall back to pkg-config +find_package(PkgConfig QUIET REQUIRED) +pkg_check_modules(PC_JSONCPP QUIET jsoncpp) + +set(jsoncpp_VERSION ${PC_JSONCPP_VERSION}) + +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_package_handle_standard_args(jsoncpp + REQUIRED_VARS jsoncpp_LIBRARY jsoncpp_INCLUDE_DIR + VERSION_VAR json_VERSION +) + +add_library(jsoncpp_lib_static STATIC IMPORTED) +set_target_properties(jsoncpp_lib_static PROPERTIES + INTERFACE_INCLUDE_DIRECTORIES ${jsoncpp_INCLUDE_DIR} + IMPORTED_LOCATION ${jsoncpp_STATIC_LIBRARY}) + +add_library(jsoncpp_lib SHARED IMPORTED) +set_target_properties(jsoncpp_lib PROPERTIES + INTERFACE_INCLUDE_DIRECTORIES ${jsoncpp_INCLUDE_DIR} + IMPORTED_LOCATION ${jsoncpp_LIBRARY}) diff --git a/cmake/VersionGen.cmake b/cmake/VersionGen.cmake new file mode 100644 index 00000000..e20d9dcd --- /dev/null +++ b/cmake/VersionGen.cmake @@ -0,0 +1,22 @@ +cmake_minimum_required(VERSION 3.1.0) + +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 + WORKING_DIRECTORY ${VERSION_GEN_SOURCE_DIR}) + + if(${GIT_RESULT} EQUAL 0) + set(GIT_HASH "${GIT_OUTPUT}") + endif() +endif() + +string(STRIP "${GIT_HASH}" GIT_HASH) +string(TOLOWER "${BUILD_TYPE}" BUILD_TYPE) + +configure_file(${VERSION_GEN_SOURCE_DIR}/cmake/build.h.in ${VERSION_GEN_OUT_DIR}/build.h @ONLY) diff --git a/cmake/build.h.in b/cmake/build.h.in new file mode 100644 index 00000000..a587b689 --- /dev/null +++ b/cmake/build.h.in @@ -0,0 +1,17 @@ +#pragma once + +namespace ouster { + +const char* const BUILD_HASH = "@GIT_HASH@"; + +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 CLIENT_VERSION_FULL = + "@ouster_client_VERSION@@ouster_client_VERSION_SUFFIX@+@GIT_HASH@-@BUILD_TYPE@"; + +} diff --git a/ouster_client/.gitignore b/ouster_client/.gitignore deleted file mode 100644 index 567609b1..00000000 --- a/ouster_client/.gitignore +++ /dev/null @@ -1 +0,0 @@ -build/ diff --git a/ouster_client/CMakeLists.txt b/ouster_client/CMakeLists.txt index 7869c6dd..7c80ae96 100644 --- a/ouster_client/CMakeLists.txt +++ b/ouster_client/CMakeLists.txt @@ -1,61 +1,36 @@ cmake_minimum_required(VERSION 3.1.0) # ==== Project Name ==== -project(ouster_client) +project(ouster_client VERSION 0.1.0) +set(ouster_client_VERSION_SUFFIX "") # ==== Requirements ==== -set(OUSTER_CLIENT_ADDITIONAL_LIBS "") -set(OUSTER_CLIENT_ADDITIONAL_LIB_DIRS "") - -if(WIN32) - find_package(jsoncpp CONFIG REQUIRED) - set(OUSTER_CLIENT_ADDITIONAL_LIBS jsoncpp ws2_32) - - get_target_property(jsoncpp_INCLUDE_DIRS jsoncpp_lib - INTERFACE_INCLUDE_DIRECTORIES) - set(OUSTER_CLIENT_ADDITIONAL_LIB_DIRS - ${_VCPKG_INSTALLED_DIR}/x64-windows/lib) - add_definitions(-D_USE_MATH_DEFINES) - add_compile_options(/W4) -elseif(APPLE) - find_package(PkgConfig REQUIRED) - pkg_check_modules(JSONCPP REQUIRED jsoncpp) - set(OUSTER_CLIENT_ADDITIONAL_INCLUDES ${JSONCPP_INCLUDEDIR}) - set(OUSTER_CLIENT_ADDITIONAL_LIBS ${JSONCPP_LIBRARIES}) - set(OUSTER_CLIENT_ADDITIONAL_LIB_DIRS ${JSONCPP_LIBDIR}) - add_compile_options(-Wextra -Werror) -else() - find_package(PkgConfig REQUIRED) - pkg_check_modules(jsoncpp REQUIRED jsoncpp) - set(OUSTER_CLIENT_ADDITIONAL_LIBS jsoncpp) - add_compile_options(-Wextra -Werror) -endif() - find_package(Eigen3 REQUIRED) +find_package(jsoncpp REQUIRED) -# ==== Options ==== -set(CMAKE_CXX_STANDARD 11) -if(NOT WIN32) - add_compile_options(-Wall) -endif() +# ==== 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 ${CMAKE_CURRENT_BINARY_DIR}/include) +add_dependencies(ouster_build generate_build_header) # ==== Libraries ==== -add_library(ouster_client STATIC src/client.cpp src/types.cpp src/compat.cpp - src/lidar_scan.cpp) -set_target_properties(ouster_client PROPERTIES POSITION_INDEPENDENT_CODE ON) -target_link_libraries(ouster_client PUBLIC ${OUSTER_CLIENT_ADDITIONAL_LIBS}) -target_include_directories(ouster_client - PUBLIC include ${OUSTER_CLIENT_ADDITIONAL_INCLUDES}) -target_include_directories( - ouster_client SYSTEM - PUBLIC ${jsoncpp_INCLUDE_DIRS} SYSTEM - PRIVATE ${EIGEN3_INCLUDE_DIR}) -if(WIN32 OR APPLE) - target_link_directories(ouster_client PUBLIC - ${OUSTER_CLIENT_ADDITIONAL_LIB_DIRS}) +add_library(ouster_client src/client.cpp src/types.cpp src/netcompat.cpp src/lidar_scan.cpp) +target_link_libraries(ouster_client PUBLIC jsoncpp_lib Eigen3::Eigen ouster_build) +if(WIN32) + target_link_libraries(ouster_client PUBLIC ws2_32) endif() +target_include_directories(ouster_client PUBLIC include) + # ==== Executables ==== -add_executable(ouster_client_example src/main.cpp) -target_link_libraries(ouster_client_example ouster_client) -target_include_directories(ouster_client_example PRIVATE include - ${EIGEN3_INCLUDE_DIR}) +add_executable(ouster_client_example src/example.cpp) +target_link_libraries(ouster_client_example ouster_client ouster_build) diff --git a/ouster_client/README.md b/ouster_client/README.md deleted file mode 100644 index 66e89f61..00000000 --- a/ouster_client/README.md +++ /dev/null @@ -1,23 +0,0 @@ -# Example Sensor Client - -## Contents -* `ouster_client/` contains a simple C++ client application that prints lidar - data to the terminal -* can be built both with and without ROS. See the instructions in - [ouster_ros/](../ouster_ros/README.md) for building in a ROS environment - -## Building the Sample Client -* The sample client requires a compiler supporting C++11 or newer and CMake -* Build with `cd /path/to/ouster_example/ouster_client && mkdir build && cd - build && cmake .. && make` where `/path/to/ouster_example` is where you've - cloned the repository - -## Running the Sample Client -* Make sure the sensor is connected to the network and has obtained a DHCP - lease. See section 3.1 in the accompanying - [software user guide](https://www.ouster.io/downloads) for more details -* An executable called `ouster_client_example` is generated in the build - directory on success -* Run `./ouster_client_example ` where - `` is the hostname or IP address of the sensor, and - `` is the IP to which the sensor should send lidar data diff --git a/ouster_client/cmake/ouster_clientConfig.cmake b/ouster_client/cmake/ouster_clientConfig.cmake deleted file mode 100644 index f64018c6..00000000 --- a/ouster_client/cmake/ouster_clientConfig.cmake +++ /dev/null @@ -1,4 +0,0 @@ -if(NOT TARGET OUSTER_CLIENT_INCLUDED) - add_custom_target(OUSTER_CLIENT_INCLUDED) - add_subdirectory(${CMAKE_CURRENT_LIST_DIR}/../ ${CMAKE_CURRENT_BINARY_DIR}/ouster_client) -endif() diff --git a/ouster_client/include/ouster/client.h b/ouster_client/include/ouster/client.h index 76c7e267..2f163b91 100644 --- a/ouster_client/include/ouster/client.h +++ b/ouster_client/include/ouster/client.h @@ -25,13 +25,12 @@ enum client_state { EXIT = 8 }; -/** - * Minimum supported version - */ +/** Minimum supported version. */ const util::version min_version = {1, 12, 0}; /** - * Listen for sensor data on the specified ports; do not configure the sensor + * Listen for sensor data on the specified ports; do not configure the sensor. + * * @param lidar_port port on which the sensor will send lidar data * @param imu_port port on which the sensor will send imu data * @return pointer owning the resources associated with the connection @@ -40,7 +39,8 @@ std::shared_ptr init_client(const std::string& hostname = "", int lidar_port = 7502, int imu_port = 7503); /** - * Connect to and configure the sensor and start listening for data + * Connect to and configure the sensor and start listening for data. + * * @param hostname hostname or ip of the sensor * @param udp_dest_host hostname or ip where the sensor should send data * @param lidar_port port on which the sensor will send lidar data @@ -56,7 +56,8 @@ std::shared_ptr init_client(const std::string& hostname, int timeout_sec = 30); /** - * Block for up to timeout_sec until either data is ready or an error occurs + * Block for up to timeout_sec until either data is ready or an error occurs. + * * NOTE: will return immediately if LIDAR_DATA or IMU_DATA are set and not * cleared by read_lidar_data() and read_imu_data() before the next call * @param cli client returned by init_client associated with the connection @@ -68,7 +69,8 @@ std::shared_ptr init_client(const std::string& hostname, client_state poll_client(const client& cli, int timeout_sec = 1); /** - * Read lidar data from the sensor. Will not block + * Read lidar data from the sensor. Will not block. + * * @param cli client returned by init_client associated with the connection * @param buf buffer to which to write lidar data. Must be at least * lidar_packet_bytes + 1 bytes @@ -78,7 +80,8 @@ bool read_lidar_packet(const client& cli, uint8_t* buf, const packet_format& pf); /** - * Read imu data from the sensor. Will not block + * Read imu data from the sensor. Will not block. + * * @param cli client returned by init_client associated with the connection * @param buf buffer to which to write imu data. Must be at least * imu_packet_bytes + 1 bytes @@ -87,8 +90,10 @@ bool read_lidar_packet(const client& cli, uint8_t* buf, bool read_imu_packet(const client& cli, uint8_t* buf, const packet_format& pf); /** - * Get metadata text blob from the sensor. Attempt to fetch from the network if - * not already populated + * Get metadata text blob from the sensor. + * + * Will attempt to fetch from the network if not already populated. + * * @param cli client returned by init_client associated with the connection * @param timeout_sec how long to wait for the sensor to initialize * @return a text blob of metadata parseable into a sensor_info struct diff --git a/ouster_client/include/ouster/compat.h b/ouster_client/include/ouster/compat.h deleted file mode 100644 index 5b6c4182..00000000 --- a/ouster_client/include/ouster/compat.h +++ /dev/null @@ -1,135 +0,0 @@ -/** - * @file - * @brief Compatibility with windows (unsupported) - */ - -#pragma once - -#include - -#if defined _WIN32 -#define WPCAP 1 -// Try and limit the stuff windows brings in -#define WIN32_LEAN_AND_MEAN -#define NOMINMAX 1 -#define HAVE_U_INT8_T 1 -#define HAVE_U_INT16_T 1 - -// Fix for windows compiler issue -#define VTK_FALLTHROUGH [[fallthrough]] - -// Windows headers -#include -#include -#include -#include -#include -#include -#include - -// ssize_t is available in vs -#ifdef _MSC_VER -#include -#define ssize_t SSIZE_T -#endif - -#pragma pack(push, 1) -struct ether_header { - uint8_t padding1[12]; - uint16_t ether_type; -}; - -struct iphdr { - uint8_t ihl : 4; - uint8_t version : 4; - uint8_t padding1; - uint16_t tot_len; - uint16_t id; - uint16_t frag_off; - uint8_t ttl; - uint8_t protocol; - uint8_t padding2[6]; - uint32_t daddr; -}; - -struct udphdr { - uint16_t padding1; - uint16_t dest; - uint16_t len; - uint16_t padding2; -}; -#pragma pack(pop) - -#define ETHERTYPE_IP 0x0800 - -#define SET_SIN_ADDR(data, address) data.addr.sin_addr.S_un.S_addr = (address) -#else // --------- Compiling on Linux --------- -// Linux headers -#include -#include -#include -#include -#include -#include -#include -#include -#include - -// Define windows types for linux -typedef int SOCKET; -typedef fd_set FDSET; -#define SOCKET_ERROR -1 -#define SET_SIN_ADDR(data, address) data.addr.sin_addr = {address} -#endif // --------- End Platform Differentiation Block --------- - -/** - * Initialize the socket stack (Noop on linux) - * @return success on windows - */ -int socket_init(void); - -/** - * Shutdown the socket stack (Noop on linux) - * @return success on windows - */ -int socket_quit(void); - -/** - * Close a specified socket - * @param sock The socket file descriptor to close - * @return success - */ -int socket_close(SOCKET sock); - -/** - * Get the error message for socket errors - * @return The socket error message - */ -std::string socket_get_error(); - -/** - * Check if a socket file descriptor is valid - * @param sock The socket file descriptor to check - * @return The validity of the socket file descriptor - */ -bool socket_valid(SOCKET value); - -/** - * Check if the last error was a socket exit event - * @return If the socket has exited - */ -bool socket_exit(); - -/** - * Set a specified socket to non-blocking - * @param sock The socket file descriptor to set non-blocking - * @return success - */ -int socket_set_non_blocking(SOCKET value); - -/** - * Set a specified socket to reuse - * @param sock The socket file descriptor to set reuse - * @return success - */ -int socket_set_reuse(SOCKET value); diff --git a/ouster_client/include/ouster/impl/client_impl.h b/ouster_client/include/ouster/impl/client_impl.h deleted file mode 100644 index ec4d99bf..00000000 --- a/ouster_client/include/ouster/impl/client_impl.h +++ /dev/null @@ -1,21 +0,0 @@ -#pragma once - -#include - -#include "ouster/compat.h" -#include "stdio.h" - -namespace ouster { -namespace sensor { -struct client { - SOCKET lidar_fd; - SOCKET imu_fd; - std::string hostname; - Json::Value meta; - ~client() { - socket_close(lidar_fd); - socket_close(imu_fd); - } -}; -} // namespace sensor -} // namespace ouster diff --git a/ouster_client/include/ouster/impl/netcompat.h b/ouster_client/include/ouster/impl/netcompat.h new file mode 100644 index 00000000..ff414027 --- /dev/null +++ b/ouster_client/include/ouster/impl/netcompat.h @@ -0,0 +1,82 @@ +/** + * @file + * @brief Compatibility with windows (unsupported) + */ + +#pragma once + +#include + +#if defined _WIN32 // --------- On Windows --------- + +// Try and limit the stuff windows brings in +#include +#include + +// ssize_t is available in vs +#ifdef _MSC_VER +#include +#define ssize_t SSIZE_T +#endif + +#else // --------- Compiling on *nix --------- + +#include +#include +#include +#include +#include +#include + +// Define windows types +typedef int SOCKET; +typedef fd_set FDSET; +#define SOCKET_ERROR -1 + +#endif // --------- End Platform Differentiation Block --------- + +namespace ouster { +namespace impl { + +/** + * Close a specified socket + * @param sock The socket file descriptor to close + * @return success + */ +int socket_close(SOCKET sock); + +/** + * Get the error message for socket errors + * @return The socket error message + */ +std::string socket_get_error(); + +/** + * Check if a socket file descriptor is valid + * @param sock The socket file descriptor to check + * @return The validity of the socket file descriptor + */ +bool socket_valid(SOCKET value); + +/** + * Check if the last error was a socket exit event + * @return If the socket has exited + */ +bool socket_exit(); + +/** + * Set a specified socket to non-blocking + * @param sock The socket file descriptor to set non-blocking + * @return success + */ +int socket_set_non_blocking(SOCKET value); + +/** + * Set a specified socket to reuse + * @param sock The socket file descriptor to set reuse + * @return success + */ +int socket_set_reuse(SOCKET value); + +} // namespace impl +} // namespace ouster diff --git a/ouster_client/include/ouster/impl/packet_impl.h b/ouster_client/include/ouster/impl/parsing.h similarity index 84% rename from ouster_client/include/ouster/impl/packet_impl.h rename to ouster_client/include/ouster/impl/parsing.h index b79a3920..dc84f2c3 100644 --- a/ouster_client/include/ouster/impl/packet_impl.h +++ b/ouster_client/include/ouster/impl/parsing.h @@ -33,9 +33,9 @@ const uint8_t* nth_col(int n, const uint8_t* lidar_buf) { } template -inline uint32_t col_valid(const uint8_t* col_buf) { +inline uint32_t col_status(const uint8_t* col_buf) { uint32_t res; - memcpy(&res, col_buf + column_bytes(N_PIXELS) - 4, sizeof(uint32_t)); + std::memcpy(&res, col_buf + column_bytes(N_PIXELS) - 4, sizeof(uint32_t)); return res; } @@ -45,7 +45,7 @@ inline uint64_t col_timestamp(const uint8_t* col_buf) { return res; // nanoseconds } -inline uint32_t col_h_angle(const uint8_t* col_buf) { +inline uint32_t col_encoder(const uint8_t* col_buf) { uint32_t res; std::memcpy(&res, col_buf + 12, sizeof(uint32_t)); return res; @@ -53,13 +53,13 @@ inline uint32_t col_h_angle(const uint8_t* col_buf) { inline uint16_t col_measurement_id(const uint8_t* col_buf) { uint16_t res; - memcpy(&res, col_buf + 8, sizeof(uint16_t)); + std::memcpy(&res, col_buf + 8, sizeof(uint16_t)); return res; } inline uint16_t col_frame_id(const uint8_t* col_buf) { uint16_t res; - memcpy(&res, col_buf + 10, sizeof(uint16_t)); + std::memcpy(&res, col_buf + 10, sizeof(uint16_t)); return res; } @@ -80,13 +80,13 @@ inline uint16_t px_reflectivity(const uint8_t* px_buf) { return res; } -inline uint16_t px_signal_photons(const uint8_t* px_buf) { +inline uint16_t px_signal(const uint8_t* px_buf) { uint16_t res; std::memcpy(&res, px_buf + 6, sizeof(uint16_t)); return res; } -inline uint16_t px_noise_photons(const uint8_t* px_buf) { +inline uint16_t px_ambient(const uint8_t* px_buf) { uint16_t res; std::memcpy(&res, px_buf + 8, sizeof(uint16_t)); return res; @@ -94,13 +94,13 @@ inline uint16_t px_noise_photons(const uint8_t* px_buf) { inline uint64_t imu_sys_ts(const uint8_t* imu_buf) { uint64_t res; - memcpy(&res, imu_buf, sizeof(uint64_t)); + std::memcpy(&res, imu_buf, sizeof(uint64_t)); return res; } inline uint64_t imu_accel_ts(const uint8_t* imu_buf) { uint64_t res; - memcpy(&res, imu_buf + 8, sizeof(uint64_t)); + std::memcpy(&res, imu_buf + 8, sizeof(uint64_t)); return res; } @@ -147,7 +147,7 @@ inline float imu_av_z(const uint8_t* imu_buf) { } template -constexpr packet_format packet__1_14_0() { +constexpr packet_format packet_2_0() { return { impl::packet_bytes(N_PIXELS), impl::imu_packet_size, @@ -157,16 +157,16 @@ constexpr packet_format packet__1_14_0() { impl::nth_col, impl::col_timestamp, - impl::col_h_angle, + impl::col_encoder, impl::col_measurement_id, impl::col_frame_id, - impl::col_valid, + impl::col_status, impl::nth_px, impl::px_range, impl::px_reflectivity, - impl::px_signal_photons, - impl::px_noise_photons, + impl::px_signal, + impl::px_ambient, impl::imu_sys_ts, impl::imu_accel_ts, diff --git a/ouster_client/include/ouster/lidar_scan.h b/ouster_client/include/ouster/lidar_scan.h index 85dab4a9..8c5903b6 100644 --- a/ouster_client/include/ouster/lidar_scan.h +++ b/ouster_client/include/ouster/lidar_scan.h @@ -5,121 +5,174 @@ #pragma once -#if defined _WIN32 -#pragma warning(push, 2) -#endif - #include - -#if defined _WIN32 -#pragma warning(pop) -#endif - #include -#include -#include +#include +#include #include #include "ouster/types.h" namespace ouster { -struct LidarScan { - enum LidarScanIndex { RANGE, INTENSITY, NOISE, REFLECTIVITY }; - using ts_t = std::chrono::nanoseconds; +/** + * Datastructure for efficient operations on aggregated lidar data. + * + * Stores each field (range, intensity, etc.) contiguously as a H x W block of + * 4-byte unsigned integers, where H is the number of beams and W is the + * horizontal resolution (e.g. 512, 1024, 2048). + * + * Note: this is the "staggered" representation where each column corresponds + * to a single measurement in time. Use the destagger() function to create an + * image where columns correspond to a single azimuth angle. + */ +class LidarScan { + public: + static constexpr int N_FIELDS = 4; + using raw_t = uint32_t; - using data_t = Eigen::Array; - using index_t = std::ptrdiff_t; + using ts_t = std::chrono::nanoseconds; + using data_t = Eigen::Array; - template - using field_t = Eigen::Array; + using DynStride = Eigen::Stride; + /** XYZ coordinates with dimensions arranged contiguously in columns */ using Points = Eigen::Array; - index_t w; - index_t h; + /** Data fields reported per channel */ + enum Field { RANGE, INTENSITY, AMBIENT, REFLECTIVITY }; - data_t data; - std::vector ts; + /** Measurement block information, other than the channel data */ + struct BlockHeader { + ts_t timestamp; + uint32_t encoder; + uint32_t status; + }; - uint32_t range(size_t u, size_t v) const { - assert(u < (size_t)h && v < (size_t)w); - return this->range()[this->ind(u, v)]; - } - uint32_t intensity(size_t u, size_t v) const { - assert(u < (size_t)h && v < (size_t)w); - return this->intensity()[this->ind(u, v)]; - } + /* Members variables: use with caution, some of these will become private */ + std::ptrdiff_t w{0}; + std::ptrdiff_t h{0}; + data_t data{}; + std::vector headers{}; + int32_t frame_id{-1}; - LidarScan() : w(0), h(0), data{0, 4}, ts(0){}; - LidarScan(size_t w, size_t h) : w(w), h(h), data{w * h, 4}, ts(w){}; + /** The default constructor creates an invalid 0 x 0 scan */ + LidarScan() = default; - // get ts in us. We may use us everywhere in our code. In that case, we will - // remove this function. (Hao) - std::chrono::microseconds ts_us(size_t v) const { - assert(v < (size_t)w); - return std::chrono::duration_cast(ts[v]); - } + /** + * Initialize an empty scan with the given horizontal / vertical resolution. + * + * @param w horizontal resoulution, i.e. the number of measurements per scan + * @param h vertical resolution, i.e. the number of channels + */ + LidarScan(size_t w, size_t h) + : w{static_cast(w)}, + h{static_cast(h)}, + data{w * h, N_FIELDS}, + headers{w, BlockHeader{ts_t{0}, 0, 0}} {}; - inline index_t ind(const index_t u, const index_t v) const { - return u * w + v; + /** + * Access timestamps as a vector. + * + * @returns copy of the measurement timestamps as a vector + */ + std::vector timestamps() const { + std::vector res; + res.reserve(headers.size()); + for (const auto& h : headers) res.push_back(h.timestamp); + return res; } - data_t::ColXpr range() { return data.col(LidarScanIndex::RANGE); } - data_t::ColXpr intensity() { return data.col(LidarScanIndex::INTENSITY); } - data_t::ColXpr noise() { return data.col(LidarScanIndex::NOISE); } - data_t::ColXpr reflectivity() { - return data.col(LidarScanIndex::REFLECTIVITY); - } + /** + * Access measurement block header fields. + * + * @return the header values for the specified measurement id + */ + BlockHeader& header(size_t m_id) { return headers.at(m_id); } + + /** @copydoc header(size_t m_id) */ + const BlockHeader& header(size_t m_id) const { return headers.at(m_id); } - data_t::ConstColXpr range() const { - return data.col(LidarScanIndex::RANGE); + /** + * Access measurement block data. + * + * @param m_id the measurement id of the desired block + * @return a view of the measurement block data + */ + Eigen::Map block(size_t m_id) { + return Eigen::Map( + data.row(m_id).data(), h, N_FIELDS, {w * h, w}); } - data_t::ConstColXpr intensity() const { - return data.col(LidarScanIndex::INTENSITY); + + /** @copydoc block(size_t m_id) */ + Eigen::Map block( + size_t m_id) const { + return Eigen::Map( + data.row(m_id).data(), h, N_FIELDS, {w * h, w}); } - data_t::ConstColXpr noise() const { - return data.col(LidarScanIndex::NOISE); + + /** + * Access a lidar data field. + * + * @param f the field to view + * @return a view of the field data + */ + Eigen::Map> field(Field f) { + return Eigen::Map>(data.col(f).data(), h, w); } - data_t::ConstColXpr reflectivity() const { - return data.col(LidarScanIndex::REFLECTIVITY); + + /** @copydoc field(Field f) */ + Eigen::Map> field(Field f) const { + return Eigen::Map>(data.col(f).data(), h, w); } }; -/** - * Lookup table of beam directions and offsets - */ +/** Equality for column headers. */ +inline bool operator==(const LidarScan::BlockHeader& a, + const LidarScan::BlockHeader& b) { + return a.timestamp == b.timestamp && a.encoder == b.encoder && + a.status == b.status; +} + +/** Equality for scans. */ +inline bool operator==(const LidarScan& a, const LidarScan& b) { + return a.w == b.w && a.h == b.h && (a.data == b.data).all() && + a.headers == b.headers && a.frame_id && b.frame_id; +} + +/** Lookup table of beam directions and offsets. */ struct XYZLut { LidarScan::Points direction; LidarScan::Points offset; }; /** - * Generate a matrix of unit vectors pointing radially outwards, useful for - * efficiently computing cartesian coordinates from ranges. The result is a - * n x 3 array of doubles stored in column-major order where each row is the - * unit vector corresponding to the nth point in a lidar scan, with 0 <= n < - * h * w. - * The ordering of the rows is consistent with LidarScan::ind(u, v) for the - * 3D point corresponding to the pixel at row u, column v in the LidarScan. - * @param w number of columns in the lidar scan. e.g. 512, 1024, or 2048. + * Generate a matrix of unit vectors pointing radially outwards. + * + * Useful for efficiently computing cartesian coordinates from + * ranges. The result is a n x 3 array of doubles stored in + * column-major order where each row is the unit vector corresponding + * to the nth point in a lidar scan, with 0 <= n < h * w. + * + * @param w number of columns in the lidar scan. e.g. 512, 1024, or 2048 * @param h number of rows in the lidar scan * @param range_unit the unit, in meters, of the range, e.g. sensor::range_unit * @param lidar_origin_to_beam_origin_mm the radius to the beam origin point of - * the unit, in millimeters + * the unit, in millimeters * @param transform additional transformation to apply to resulting points * @param azimuth_angles_deg azimuth offsets in degrees for each of h beams - * @param altitude_angles_Deg altitude in degrees for each of h beams + * @param altitude_angles_deg altitude in degrees for each of h beams * @return xyz direction unit vectors for each point in the lidar scan */ -XYZLut make_xyz_lut(LidarScan::index_t w, LidarScan::index_t h, - double range_unit, double lidar_origin_to_beam_origin_mm, - const sensor::mat4d& transform, +XYZLut make_xyz_lut(size_t w, size_t h, double range_unit, + double lidar_origin_to_beam_origin_mm, + const mat4d& transform, const std::vector& azimuth_angles_deg, const std::vector& altitude_angles_deg); /** - * Convenient overload that uses parameters from the supplied sensor_info + * Convenient overload that uses parameters from the supplied sensor_info. + * * @param sensor metadata returned from the client * @return xyz direction unit vectors for each point in the lidar scan */ @@ -132,67 +185,98 @@ inline XYZLut make_xyz_lut(const sensor::sensor_info& sensor) { } /** - * Generate a destaggered version of a field from LidarScan, - * used for visualizing lidar data as an image view, or for algorithms - * that exploit the structure of the lidar data, such as - * beam_uniformity in ouster_viz, or computer vision algorithms. - * You can re-stagger a destaggered image by setting template parameter - * direction to be -1. + * Convert LidarScan to cartesian points. + * + * @param scan a LidarScan + * @param xyz_lut a lookup table of unit vectors generated by make_xyz_lut + * @return cartesian points where ith row is a 3D point which corresponds + * to ith pixel in LidarScan where i = row * w + col + */ +LidarScan::Points cartesian(const LidarScan& scan, const XYZLut& lut); + +/** + * Generate a destaggered version of a channel field. + * + * Used for visualizing lidar data as an image or for algorithms that + * exploit the structure of the lidar data, such as beam_uniformity in + * ouster_viz, or computer vision algorithms. * * For example: - * destagger(lidarscan.intensity()) - * destagger(lidarscan.intensity().cast()) + * destagger(lidarscan.field(Field::INTENSITY)) + * destagger(lidarscan.field(Field::INTENSITY).cast()) * - * @param field - * @param pixel_shift_by_row - * @return destaggered version of field + * @param img the channel field + * @param pixel_shift_by_row offsets, usually queried from the sensor + * @param inverse perform the inverse operation + * @return destaggered version of the image */ -template -inline LidarScan::field_t destagger( - const Eigen::Ref> field, - const std::vector& pixel_shift_by_row) { - static_assert(direction == 1 || direction == -1, - "Destagger direction should be only +1 or -1"); - const LidarScan::index_t h = pixel_shift_by_row.size(); - const LidarScan::index_t w = field.size() / h; - - Eigen::Array destaggered(h * w); - for (LidarScan::index_t u = 0; u < h; u++) { - const LidarScan::index_t offset = - (direction * pixel_shift_by_row[u] + w) % w; - - destaggered.segment(u * w + offset, w - offset) = - field.segment(u * w, w - offset); - destaggered.segment(u * w, offset) = - field.segment(u * w + w - offset, offset); +template +inline img_t destagger(const Eigen::Ref>& img, + const std::vector& pixel_shift_by_row, + bool inverse = false) { + const size_t h = img.rows(); + const size_t w = img.cols(); + + if (pixel_shift_by_row.size() != h) + throw std::invalid_argument{"image height does not match shifts size"}; + + img_t destaggered{h, w}; + for (size_t u = 0; u < h; u++) { + const std::ptrdiff_t offset = + ((inverse ? -1 : 1) * pixel_shift_by_row[u] + w) % w; + + destaggered.row(u).segment(offset, w - offset) = + img.row(u).segment(0, w - offset); + destaggered.row(u).segment(0, offset) = + img.row(u).segment(w - offset, offset); } return destaggered; } /** - * Convenient overload that uses parameters from the supplied sensor_info - * @param field - * @param sensor - * @return destaggered version of field + * Generate a staggered version of a channel field. + * + * @param img + * @param pixel_shift_by_row + * @return staggered version of the image */ -template -inline LidarScan::field_t destagger( - const Eigen::Ref> field, - const sensor::sensor_info& sensor) { - return destagger(field, sensor.format.pixel_shift_by_row); +template +inline img_t stagger(const Eigen::Ref>& img, + const std::vector& pixel_shift_by_row) { + return destagger(img, pixel_shift_by_row, true); } /** - * Convert LidarScan to cartesian points. - * @param scan a LidarScan - * @param xyz_lut a lookup table of unit vectors generated by make_xyz_lut - * @return cartesian points where ith row is a 3D point which corresponds - * to ith pixel in LidarScan where i = scan.ind(u, v) for a pixel - * at row u, column v. + * Parse lidar packets into a LidarScan. + * + * Make a function that batches a single scan (revolution) of data to a + * LidarScan. */ -inline LidarScan::Points cartesian(const LidarScan& scan, const XYZLut& lut) { - auto raw = lut.direction.colwise() * scan.range().cast(); - return (raw.array() == 0.0).select(raw, raw + lut.offset); -} +class ScanBatcher { + std::ptrdiff_t w; + std::ptrdiff_t h; + uint16_t next_m_id; + LidarScan ls_write; + + public: + sensor::packet_format pf; + + /** + * Create a batcher given information about the scan and packet format. + * + * @param w number of columns in the lidar scan. One of 512, 1024, or 2048 + * @param pf expected format of the incoming packets used for parsing + */ + ScanBatcher(size_t w, const sensor::packet_format& pf); + + /** + * Add a packet to the scan. + * + * @param packet_buf the lidar packet + * @param lidar scan to populate + * @return true when the provided lidar scan is ready to use + */ + bool operator()(const uint8_t* packet_buf, LidarScan& ls); +}; } // namespace ouster diff --git a/ouster_client/include/ouster/packet.h b/ouster_client/include/ouster/packet.h deleted file mode 100644 index 9e1b19cb..00000000 --- a/ouster_client/include/ouster/packet.h +++ /dev/null @@ -1,113 +0,0 @@ -/** - * @file - * @brief Utilities to parse lidar and imu packets - */ - -#pragma once - -#include -#include -#include -#include -#include - -#include "ouster/impl/packet_impl.h" -#include "ouster/lidar_scan.h" -#include "ouster/types.h" - -namespace ouster { -namespace sensor { - -constexpr packet_format packet__1_13_0 = impl::packet__1_14_0<64>(); - -constexpr packet_format packet__1_14_0__16 = impl::packet__1_14_0<16>(); - -constexpr packet_format packet__1_14_0__32 = impl::packet__1_14_0<32>(); - -constexpr packet_format packet__1_14_0__64 = impl::packet__1_14_0<64>(); - -constexpr packet_format packet__1_14_0__128 = impl::packet__1_14_0<128>(); - -/** - * Make a function that batches a single scan (revolution) of data to a - * LidarScan. The callback f() is invoked with the timestamp of the first column - * in the scan before adding data from a new scan. - * - * @param w number of columns in the lidar scan. One of 512, 1024, or 2048 - * @param f callback when batching a scan is done, with one input argument - * chrono::nanoseconds scan_ts for timestamp of the start of the scan - * @return a function taking a lidar packet buffer and LidarScan to which data - * is added for every point in the scan. - */ -template -std::function batch_to_scan( - int w, const packet_format& pf, F&& f) { - int h = pf.pixels_per_column; - int next_m_id{w}; - int32_t cur_f_id{-1}; - - constexpr std::chrono::nanoseconds invalid_ts(-1LL); - std::chrono::nanoseconds scan_ts(invalid_ts); - - // for operations on each row of LidarScan.data - using row_view_t = - Eigen::Map>; - - return [=](const uint8_t* packet_buf, LidarScan& ls) mutable { - for (int icol = 0; icol < pf.columns_per_packet; icol++) { - const uint8_t* col_buf = pf.nth_col(icol, packet_buf); - const uint16_t m_id = pf.col_measurement_id(col_buf); - const uint16_t f_id = pf.col_frame_id(col_buf); - const std::chrono::nanoseconds ts(pf.col_timestamp(col_buf)); - const bool valid = pf.col_valid(col_buf) == 0xffffffff; - - // drop invalid / out-of-bounds data in case of misconfiguration - if (!valid || m_id >= w || f_id + 1 == cur_f_id) continue; - - if (f_id != cur_f_id) { - // if not initializing with first packet - if (scan_ts != invalid_ts) { - // zero out remaining missing columns - auto rows = ls.h * 4; - row_view_t{ls.data.data(), rows, ls.w} - .block(0, next_m_id, rows, w - next_m_id) - .setZero(); - f(scan_ts); - } - - // start new frame - scan_ts = ts; - next_m_id = 0; - cur_f_id = f_id; - } - - // zero out missing columns if we jumped forward - if (m_id >= next_m_id) { - auto rows = ls.h * 4; - row_view_t{ls.data.data(), rows, ls.w} - .block(0, next_m_id, rows, m_id - next_m_id) - .setZero(); - next_m_id = m_id + 1; - } - - ls.ts[m_id] = ts; - for (uint8_t ipx = 0; ipx < h; ipx++) { - // index of the first point in current packet - const std::ptrdiff_t idx = ls.ind(ipx, m_id); - - const uint8_t* px_buf = pf.nth_px(ipx, col_buf); - - // i, ts, reflectivity, ring, noise, range (mm) - ls.data.row(idx) - << static_cast(pf.px_range(px_buf)), - static_cast(pf.px_signal_photons(px_buf)), - static_cast(pf.px_noise_photons(px_buf)), - static_cast(pf.px_reflectivity(px_buf)); - } - } - }; -} - -} // namespace sensor -} // namespace ouster diff --git a/ouster_client/include/ouster/types.h b/ouster_client/include/ouster/types.h index 3f163017..3ed6b4ab 100644 --- a/ouster_client/include/ouster/types.h +++ b/ouster_client/include/ouster/types.h @@ -5,40 +5,29 @@ #pragma once -#if defined _WIN32 -#pragma warning(push, 2) -#endif - #include - -#if defined _WIN32 -#pragma warning(pop) -#endif - #include #include namespace ouster { -namespace sensor { +/** For image operations. */ +template +using img_t = Eigen::Array; + +/** Used for transformations */ using mat4d = Eigen::Matrix; -/** - * Unit of range from sensor packet, in meters - */ +namespace sensor { + +/** Unit of range from sensor packet, in meters. */ constexpr double range_unit = 0.001; -/** - * Design values for altitude and azimuth offset angles for gen1 sensors. Can be - * used if calibrated values are not available. - */ +/** Design values for altitude and azimuth offset angles for gen1 sensors. */ extern const std::vector gen1_altitude_angles; extern const std::vector gen1_azimuth_angles; -/** - * Design values for imu and lidar to sensor-frame transforms. See the manual - * for details. - */ +/** Design values for imu and lidar to sensor-frame transforms. */ extern const mat4d default_imu_to_sensor_transform; extern const mat4d default_lidar_to_sensor_transform; @@ -81,58 +70,67 @@ struct sensor_info { }; /** - * Get a default sensor_info for the given lidar mode + * Get a default sensor_info for the given lidar mode. + * * @param lidar_mode * @return default sensor_info for the OS1-64 */ sensor_info default_sensor_info(lidar_mode mode); /** - * Get string representation of a lidar mode + * Get string representation of a lidar mode. + * * @param lidar_mode * @return string representation of the lidar mode, or "UNKNOWN" */ std::string to_string(lidar_mode mode); /** - * Get lidar mode from string + * Get lidar mode from string. + * * @param string * @return lidar mode corresponding to the string, or 0 on error */ lidar_mode lidar_mode_of_string(const std::string& s); /** - * Get number of columns in a scan for a lidar mode + * Get number of columns in a scan for a lidar mode. + * * @param lidar_mode * @return number of columns per rotation for the mode */ uint32_t n_cols_of_lidar_mode(lidar_mode mode); /** - * Get the lidar rotation frequency from lidar mode + * Get the lidar rotation frequency from lidar mode. + * * @param lidar_mode * @return lidar rotation frequency in Hz */ int frequency_of_lidar_mode(lidar_mode mode); /** - * Get string representation of a timestamp mode + * Get string representation of a timestamp mode. + * * @param timestamp_mode * @return string representation of the timestamp mode, or "UNKNOWN" */ std::string to_string(timestamp_mode mode); /** - * Get timestamp mode from string + * Get timestamp mode from string. + * * @param string * @return timestamp mode corresponding to the string, or 0 on error */ timestamp_mode timestamp_mode_of_string(const std::string& s); /** - * Parse metadata text blob from the sensor into a sensor_info struct. String - * and vector fields will have size 0 if the parameter cannot be found or - * parsed, while lidar_mode will be set to 0 (invalid) + * Parse metadata text blob from the sensor into a sensor_info struct. + * + * String and vector fields will have size 0 if the parameter cannot + * be found or parsed, while lidar_mode will be set to 0 (invalid). + * * @throw runtime_error if the text is not valid json * @param metadata a text blob returned by get_metadata from client.h * @return a sensor_info struct populated with a subset of the metadata @@ -140,7 +138,8 @@ timestamp_mode timestamp_mode_of_string(const std::string& s); sensor_info parse_metadata(const std::string& metadata); /** - * Parse metadata given path to a json file + * Parse metadata given path to a json file. + * * @throw runtime_error if json file does not exist or is malformed * @param json_file path to a json file containing sensor metadata * @return a sensor_info struct populated with a subset of the metadata @@ -148,15 +147,25 @@ sensor_info parse_metadata(const std::string& metadata); sensor_info metadata_from_json(const std::string& json_file); /** - * Get string representation of metadata + * Get string representation of metadata. + * * @param metadata a struct of sensor metadata * @return a json metadata string */ std::string to_string(const sensor_info& metadata); /** - * A packet_format is a table of accessors and indices for extracting data - * from imu and lidar packets. + * Table of accessors for extracting data from imu and lidar packets. + * + * In the user guide, refer to section 9 for the lidar packet format and section + * 10 for imu packets. + * + * For 0 <= n < columns_per_packet, nth_col(n, packet_buf) returns a pointer to + * the nth measurement block. For 0 <= m < pixels_per_column, nth_px(m, col_buf) + * returns the mth channel data block. + * + * Use imu_la_{x,y,z} to access the acceleration in the corresponding + * direction. Use imu_av_{x,y,z} to read the angular velocity. */ struct packet_format { const size_t lidar_packet_size; @@ -165,19 +174,22 @@ struct packet_format { const int pixels_per_column; const int encoder_ticks_per_rev; + // Measurement block accessors const uint8_t* (*const nth_col)(int n, const uint8_t* lidar_buf); uint64_t (*const col_timestamp)(const uint8_t* col_buf); - uint32_t (*const col_h_angle)(const uint8_t* col_buf); + uint32_t (*const col_encoder)(const uint8_t* col_buf); uint16_t (*const col_measurement_id)(const uint8_t* col_buf); uint16_t (*const col_frame_id)(const uint8_t* col_buf); - uint32_t (*const col_valid)(const uint8_t* col_buf); + uint32_t (*const col_status)(const uint8_t* col_buf); + // Channel data block accessors const uint8_t* (*const nth_px)(int n, const uint8_t* col_buf); uint32_t (*const px_range)(const uint8_t* px_buf); uint16_t (*const px_reflectivity)(const uint8_t* px_buf); - uint16_t (*const px_signal_photons)(const uint8_t* px_buf); - uint16_t (*const px_noise_photons)(const uint8_t* px_buf); + uint16_t (*const px_signal)(const uint8_t* px_buf); + uint16_t (*const px_ambient)(const uint8_t* px_buf); + // IMU packet accessors uint64_t (*const imu_sys_ts)(const uint8_t* imu_buf); uint64_t (*const imu_accel_ts)(const uint8_t* imu_buf); uint64_t (*const imu_gyro_ts)(const uint8_t* imu_buf); @@ -190,7 +202,8 @@ struct packet_format { }; /** - * Get a packet parser for a particular data format + * Get a packet parser for a particular data format. + * * @param data_format parameters provided by the sensor * @returns a packet_format suitable for parsing UDP packets sent by the sensor */ diff --git a/ouster_client/include/ouster/version.h b/ouster_client/include/ouster/version.h index 4ae6a483..167bc7b4 100644 --- a/ouster_client/include/ouster/version.h +++ b/ouster_client/include/ouster/version.h @@ -3,6 +3,9 @@ * @brief Simple version struct */ +#include +#include + #pragma once namespace ouster { @@ -36,14 +39,16 @@ inline bool operator>=(const version& u, const version& v) { return !(u < v); } inline bool operator>(const version& u, const version& v) { return !(u <= v); } /** - * Get string representation of a version + * Get string representation of a version. + * * @param version * @return string representation of the version */ std::string to_string(const version& v); /** - * Get lidar mode from string + * Get lidar mode from string. + * * @param string * @return lidar mode corresponding to the string, or invalid_version on error */ diff --git a/ouster_client/src/client.cpp b/ouster_client/src/client.cpp index 92a661a5..8bba2b2e 100644 --- a/ouster_client/src/client.cpp +++ b/ouster_client/src/client.cpp @@ -17,8 +17,8 @@ #include #include -#include "ouster/compat.h" -#include "ouster/impl/client_impl.h" +#include "ouster/build.h" +#include "ouster/impl/netcompat.h" #include "ouster/types.h" namespace ouster { @@ -26,17 +26,30 @@ namespace sensor { namespace chrono = std::chrono; +struct client { + SOCKET lidar_fd; + SOCKET imu_fd; + std::string hostname; + Json::Value meta; + ~client() { + impl::socket_close(lidar_fd); + impl::socket_close(imu_fd); + } +}; + namespace { // default udp receive buffer size on windows is very low -- use 256K const int RCVBUF_SIZE = 256 * 1024; -int32_t get_sock_port(int sock_fd) { +int32_t get_sock_port(SOCKET sock_fd) { struct sockaddr_storage ss; socklen_t addrlen = sizeof ss; - if (!socket_valid(getsockname(sock_fd, (struct sockaddr*)&ss, &addrlen))) { - std::cerr << "udp getsockname(): " << socket_get_error() << std::endl; + if (!impl::socket_valid( + getsockname(sock_fd, (struct sockaddr*)&ss, &addrlen))) { + std::cerr << "udp getsockname(): " << impl::socket_get_error() + << std::endl; return SOCKET_ERROR; } @@ -48,7 +61,7 @@ int32_t get_sock_port(int sock_fd) { return SOCKET_ERROR; } -int udp_data_socket(int port) { +SOCKET udp_data_socket(int port) { struct addrinfo hints, *info_start, *ai; memset(&hints, 0, sizeof hints); @@ -68,25 +81,28 @@ int udp_data_socket(int port) { return SOCKET_ERROR; } - int sock_fd; + 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 (!socket_valid(sock_fd)) { - std::cerr << "udp socket(): " << socket_get_error() << std::endl; + if (!impl::socket_valid(sock_fd)) { + std::cerr << "udp socket(): " << impl::socket_get_error() + << std::endl; continue; } int off = 0; - if (!socket_valid(setsockopt(sock_fd, IPPROTO_IPV6, IPV6_V6ONLY, - (char*)&off, sizeof(off)))) { - std::cerr << "udp setsockopt(): " << socket_get_error() << std::endl; - socket_close(sock_fd); + if (setsockopt(sock_fd, IPPROTO_IPV6, IPV6_V6ONLY, (char*)&off, + sizeof(off))) { + std::cerr << "udp setsockopt(): " << impl::socket_get_error() + << std::endl; + impl::socket_close(sock_fd); return SOCKET_ERROR; } - if (!socket_valid(bind(sock_fd, ai->ai_addr, ai->ai_addrlen))) { - socket_close(sock_fd); - std::cerr << "udp bind(): " << socket_get_error() << std::endl; + if (bind(sock_fd, ai->ai_addr, (socklen_t)ai->ai_addrlen)) { + impl::socket_close(sock_fd); + std::cerr << "udp bind(): " << impl::socket_get_error() + << std::endl; continue; } @@ -95,27 +111,29 @@ int udp_data_socket(int port) { freeaddrinfo(info_start); if (ai == NULL) { - socket_close(sock_fd); + impl::socket_close(sock_fd); return SOCKET_ERROR; } - if (!socket_valid(socket_set_non_blocking(sock_fd))) { - std::cerr << "udp fcntl(): " << socket_get_error() << std::endl; - socket_close(sock_fd); + if (!impl::socket_valid(impl::socket_set_non_blocking(sock_fd))) { + std::cerr << "udp fcntl(): " << impl::socket_get_error() << std::endl; + impl::socket_close(sock_fd); return SOCKET_ERROR; } - if (!socket_valid(setsockopt(sock_fd, SOL_SOCKET, SO_RCVBUF, - (char*)&RCVBUF_SIZE, sizeof(RCVBUF_SIZE)))) { - std::cerr << "udp setsockopt(): " << socket_get_error() << std::endl; - socket_close(sock_fd); + if (!impl::socket_valid(setsockopt(sock_fd, SOL_SOCKET, SO_RCVBUF, + (char*)&RCVBUF_SIZE, + sizeof(RCVBUF_SIZE)))) { + std::cerr << "udp setsockopt(): " << impl::socket_get_error() + << std::endl; + impl::socket_close(sock_fd); return SOCKET_ERROR; } return sock_fd; } -int cfg_socket(const char* addr) { +SOCKET cfg_socket(const char* addr) { struct addrinfo hints, *info_start, *ai; memset(&hints, 0, sizeof hints); @@ -132,16 +150,16 @@ int cfg_socket(const char* addr) { return SOCKET_ERROR; } - int sock_fd; + 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 (!socket_valid(sock_fd)) { - std::cerr << "socket: " << socket_get_error() << std::endl; + if (!impl::socket_valid(sock_fd)) { + std::cerr << "socket: " << impl::socket_get_error() << std::endl; continue; } - if (connect(sock_fd, ai->ai_addr, ai->ai_addrlen) == -1) { - socket_close(sock_fd); + if (connect(sock_fd, ai->ai_addr, (socklen_t)ai->ai_addrlen) < 0) { + impl::socket_close(sock_fd); continue; } @@ -156,7 +174,7 @@ int cfg_socket(const char* addr) { return sock_fd; } -bool do_tcp_cmd(int sock_fd, const std::vector& cmd_tokens, +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]}; @@ -195,7 +213,7 @@ void update_json_obj(Json::Value& dst, const Json::Value& src) { } } -bool collect_metadata(client& cli, const int sock_fd, chrono::seconds timeout) { +bool collect_metadata(client& cli, SOCKET sock_fd, chrono::seconds timeout) { Json::CharReaderBuilder builder{}; auto reader = std::unique_ptr{builder.newCharReader()}; Json::Value root{}; @@ -248,6 +266,7 @@ bool collect_metadata(client& cli, const int sock_fd, chrono::seconds timeout) { // merge extra info into metadata cli.meta["hostname"] = cli.hostname; cli.meta["lidar_mode"] = root["lidar_mode"]; + cli.meta["client_version"] = ouster::CLIENT_VERSION; return success; } @@ -255,13 +274,13 @@ bool collect_metadata(client& cli, const int sock_fd, chrono::seconds timeout) { std::string get_metadata(client& cli, int timeout_sec) { if (!cli.meta) { - int sock_fd = cfg_socket(cli.hostname.c_str()); + 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}); - socket_close(sock_fd); + impl::socket_close(sock_fd); if (!success) return ""; } @@ -281,7 +300,7 @@ std::shared_ptr init_client(const std::string& hostname, int lidar_port, cli->lidar_fd = udp_data_socket(lidar_port); cli->imu_fd = udp_data_socket(imu_port); - if (!socket_valid(cli->lidar_fd) || !socket_valid(cli->imu_fd)) + if (!impl::socket_valid(cli->lidar_fd) || !impl::socket_valid(cli->imu_fd)) return std::shared_ptr(); return cli; @@ -298,11 +317,11 @@ std::shared_ptr init_client(const std::string& hostname, // update requested ports to actual bound ports lidar_port = get_sock_port(cli->lidar_fd); imu_port = get_sock_port(cli->imu_fd); - if (!socket_valid(lidar_port) || !socket_valid(imu_port)) + if (!impl::socket_valid(lidar_port) || !impl::socket_valid(imu_port)) return std::shared_ptr(); - int sock_fd = cfg_socket(hostname.c_str()); - if (!socket_valid(sock_fd)) 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; @@ -341,7 +360,7 @@ std::shared_ptr init_client(const std::string& hostname, success &= collect_metadata(*cli, sock_fd, chrono::seconds{timeout_sec}); - socket_close(sock_fd); + impl::socket_close(sock_fd); return success ? cli : std::shared_ptr(); } @@ -356,16 +375,16 @@ client_state poll_client(const client& c, const int timeout_sec) { tv.tv_sec = timeout_sec; tv.tv_usec = 0; - int max_fd = std::max(c.lidar_fd, c.imu_fd); + SOCKET max_fd = std::max(c.lidar_fd, c.imu_fd); - int retval = select(max_fd + 1, &rfds, NULL, NULL, &tv); + SOCKET retval = select((int)max_fd + 1, &rfds, NULL, NULL, &tv); client_state res = client_state(0); - if (!socket_valid(retval) && socket_exit()) { + if (!impl::socket_valid(retval) && impl::socket_exit()) { res = EXIT; - } else if (!socket_valid(retval)) { - std::cerr << "select: " << socket_get_error() << std::endl; + } else if (!impl::socket_valid(retval)) { + std::cerr << "select: " << impl::socket_get_error() << std::endl; res = client_state(res | CLIENT_ERROR); } else if (retval) { if (FD_ISSET(c.lidar_fd, &rfds)) res = client_state(res | LIDAR_DATA); @@ -375,12 +394,12 @@ client_state poll_client(const client& c, const int timeout_sec) { return res; } -static bool recv_fixed(int fd, void* buf, int64_t len) { +static bool recv_fixed(SOCKET fd, void* buf, int64_t len) { int64_t n = recv(fd, (char*)buf, len + 1, 0); if (n == len) { return true; - } else if (n == static_cast(-1)) { - std::cerr << "recvfrom: " << socket_get_error() << std::endl; + } else if (n == -1) { + std::cerr << "recvfrom: " << impl::socket_get_error() << std::endl; } else { std::cerr << "Unexpected udp packet length: " << n << std::endl; } diff --git a/ouster_client/src/example.cpp b/ouster_client/src/example.cpp new file mode 100644 index 00000000..70769994 --- /dev/null +++ b/ouster_client/src/example.cpp @@ -0,0 +1,174 @@ +#include +#include +#include +#include + +#include "ouster/build.h" +#include "ouster/client.h" +#include "ouster/lidar_scan.h" +#include "ouster/types.h" + +using namespace ouster; + +const int N_SCANS = 5; +const size_t UDP_BUF_SIZE = 65536; + +void FATAL(const char* msg) { + std::cerr << msg << std::endl; + std::exit(EXIT_FAILURE); +} + +int main(int argc, char* argv[]) { + if (argc != 3) { + std::cerr << "Version: " << ouster::CLIENT_VERSION_FULL << " (" + << ouster::BUILD_SYSTEM << ")" + << "\n\nUsage: ouster_client_example " + "" + << std::endl; + + return EXIT_FAILURE; + } + std::cerr << "Ouster client example " << ouster::CLIENT_VERSION + << std::endl; + /* + * The sensor client consists of the network client and a library for + * reading and working with data. + * + * The network client supports reading and writing a limited number of + * configuration parameters and receiving data without working directly with + * the socket APIs. See the `client.h` for more details. The minimum + * required parameters are the sensor hostname/ip and the data destination + * hostname/ip. + */ + const std::string sensor_hostname = argv[1]; + const std::string data_destination = argv[2]; + + std::cerr << "Connecting to \"" << sensor_hostname << "\"... "; + + auto handle = sensor::init_client(sensor_hostname, data_destination); + if (!handle) FATAL("Failed to connect"); + std::cerr << "ok" << std::endl; + + /* + * Configuration and calibration parameters can be queried directly from the + * sensor. These are required for parsing the packet stream and calculating + * accurate point clouds. + */ + std::cerr << "Gathering metadata..." << std::endl; + auto metadata = sensor::get_metadata(*handle); + + // Raw metadata can be parsed into a `sensor_info` struct + sensor::sensor_info info = sensor::parse_metadata(metadata); + + size_t w = info.format.columns_per_frame; + size_t h = info.format.pixels_per_column; + + std::cerr << " Firmware version: " << info.fw_rev + << "\n Serial number: " << info.sn + << "\n Product line: " << info.prod_line + << "\n Scan dimensions: " << w << " x " << h << std::endl; + + // A LidarScan holds lidar data for an entire rotation of the device + std::vector scans{N_SCANS, LidarScan{w, h}}; + + // A ScanBatcher can be used to batch packets into scans + sensor::packet_format pf = sensor::get_format(info); + ScanBatcher batch_to_scan(info.format.columns_per_frame, pf); + + /* + * The network client provides some convenience wrappers around socket APIs + * to facilitate reading lidar and IMU data from the network. It is also + * possible to configure the sensor offline and read data directly from a + * UDP socket. + */ + std::cerr << "Capturing points... "; + + // buffer to store raw packet data + std::unique_ptr packet_buf(new uint8_t[UDP_BUF_SIZE]); + + for (int i = 0; i < N_SCANS;) { + // wait until sensor data is available + sensor::client_state st = sensor::poll_client(*handle); + + // check for error status + if (st & sensor::CLIENT_ERROR) + FATAL("Sensor client returned error state!"); + + // check for lidar data, read a packet and add it to the current batch + if (st & sensor::LIDAR_DATA) { + if (!sensor::read_lidar_packet(*handle, 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 != 0xffffffff; + }); + // retry until we receive a full scan + if (n_invalid == 0) i++; + } + } + + // check if IMU data is available (but don't do anything with it) + if (st & sensor::IMU_DATA) { + sensor::read_imu_packet(*handle, packet_buf.get(), pf); + } + } + std::cerr << "ok" << std::endl; + + /* + * 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. + * + * [0] http://eigen.tuxfamily.org + */ + std::cerr << "Computing point clouds... " << std::endl; + + // pre-compute a table for efficiently calculating point clouds from range + XYZLut lut = ouster::make_xyz_lut(info); + std::vector clouds; + + for (const LidarScan& scan : scans) { + // compute a point cloud using the lookup table + clouds.push_back(ouster::cartesian(scan, lut)); + + // channel fields can be queried as well + auto n_returns = (scan.field(LidarScan::Field::RANGE) != 0).count(); + + std::cerr << " Frame no. " << scan.frame_id << " with " << n_returns + << " returns" << std::endl; + } + + /* + * Write output to CSV files. The output can be viewed in a point cloud + * viewer like CloudCompare: + * + * [0] https://github.com/cloudcompare/cloudcompare + */ + std::cerr << "Writing files... " << std::endl; + + int file_ind = 0; + std::string file_base{"cloud_"}; + for (const LidarScan::Points& cloud : clouds) { + std::string filename = file_base + std::to_string(file_ind++) + ".csv"; + std::ofstream out; + out.open(filename); + out << std::fixed << std::setprecision(4); + + // write each point, filtering out points without returns + for (int i = 0; i < cloud.rows(); i++) { + auto xyz = cloud.row(i); + if (!xyz.isApproxToConstant(0.0)) + out << xyz(0) << ", " << xyz(1) << ", " << xyz(2) << std::endl; + } + + out.close(); + std::cerr << " Wrote " << filename << std::endl; + } + + return EXIT_SUCCESS; +} diff --git a/ouster_client/src/lidar_scan.cpp b/ouster_client/src/lidar_scan.cpp index b156afc1..ff0d7dab 100644 --- a/ouster_client/src/lidar_scan.cpp +++ b/ouster_client/src/lidar_scan.cpp @@ -1,12 +1,16 @@ #include "ouster/lidar_scan.h" +#include +#include #include namespace ouster { -XYZLut make_xyz_lut(LidarScan::index_t w, LidarScan::index_t h, - double range_unit, double lidar_origin_to_beam_origin_mm, - const sensor::mat4d& transform, +constexpr int LidarScan::N_FIELDS; + +XYZLut make_xyz_lut(size_t w, size_t h, double range_unit, + double lidar_origin_to_beam_origin_mm, + const mat4d& transform, const std::vector& azimuth_angles_deg, const std::vector& altitude_angles_deg) { Eigen::ArrayXd encoder(w * h); // theta_e @@ -16,9 +20,9 @@ XYZLut make_xyz_lut(LidarScan::index_t w, LidarScan::index_t h, const double azimuth_radians = M_PI * 2.0 / w; // populate angles for each pixel - for (LidarScan::index_t v = 0; v < w; v++) { - for (LidarScan::index_t u = 0; u < h; u++) { - LidarScan::index_t i = u * w + v; + for (size_t v = 0; v < w; v++) { + for (size_t u = 0; u < h; u++) { + size_t i = u * w + v; encoder(i) = 2 * M_PI - (v * azimuth_radians); azimuth(i) = -azimuth_angles_deg[u] * M_PI / 180.0; altitude(i) = altitude_angles_deg[u] * M_PI / 180.0; @@ -53,4 +57,78 @@ XYZLut make_xyz_lut(LidarScan::index_t w, LidarScan::index_t h, return lut; } +LidarScan::Points cartesian(const LidarScan& scan, const XYZLut& lut) { + auto reshaped = Eigen::Map>( + scan.field(LidarScan::RANGE).data(), scan.h * scan.w); + auto nooffset = lut.direction.colwise() * reshaped.cast(); + return (nooffset.array() == 0.0).select(nooffset, nooffset + lut.offset); +} + +ScanBatcher::ScanBatcher(size_t w, const sensor::packet_format& pf) + : w(w), h(pf.pixels_per_column), next_m_id(0), ls_write(w, h), pf(pf) {} + +bool ScanBatcher::operator()(const uint8_t* packet_buf, LidarScan& ls) { + using row_view_t = + Eigen::Map>; + + if (ls.w != w || ls.h != h) + throw std::invalid_argument("unexpected scan dimensions"); + + bool swapped = false; + + for (int icol = 0; icol < pf.columns_per_packet; icol++) { + const uint8_t* col_buf = pf.nth_col(icol, packet_buf); + const uint16_t m_id = pf.col_measurement_id(col_buf); + const uint16_t f_id = pf.col_frame_id(col_buf); + const std::chrono::nanoseconds ts(pf.col_timestamp(col_buf)); + const uint32_t encoder = pf.col_encoder(col_buf); + const uint32_t status = pf.col_status(col_buf); + const bool valid = (status == 0xffffffff); + + // drop invalid / out-of-bounds data in case of misconfiguration + if (!valid || m_id >= w || f_id + 1 == ls_write.frame_id) continue; + + if (ls_write.frame_id != f_id) { + // if not initializing with first packet + if (ls_write.frame_id != -1) { + // zero out remaining missing columns + auto rows = h * LidarScan::N_FIELDS; + row_view_t{ls_write.data.data(), rows, w} + .block(0, next_m_id, rows, w - next_m_id) + .setZero(); + + // finish the scan and notify callback + std::swap(ls, ls_write); + swapped = true; + } + + // start new frame + next_m_id = 0; + ls_write.frame_id = f_id; + } + + // zero out missing columns if we jumped forward + if (m_id >= next_m_id) { + auto rows = h * LidarScan::N_FIELDS; + row_view_t{ls_write.data.data(), rows, w} + .block(0, next_m_id, rows, m_id - next_m_id) + .setZero(); + next_m_id = m_id + 1; + } + + ls_write.header(m_id) = {ts, encoder, status}; + for (uint8_t ipx = 0; ipx < h; ipx++) { + const uint8_t* px_buf = pf.nth_px(ipx, col_buf); + + ls_write.block(m_id).row(ipx) + << static_cast(pf.px_range(px_buf)), + static_cast(pf.px_signal(px_buf)), + static_cast(pf.px_ambient(px_buf)), + static_cast(pf.px_reflectivity(px_buf)); + } + } + return swapped; +} + } // namespace ouster diff --git a/ouster_client/src/main.cpp b/ouster_client/src/main.cpp deleted file mode 100644 index 6644ca71..00000000 --- a/ouster_client/src/main.cpp +++ /dev/null @@ -1,88 +0,0 @@ -#include -#include -#include -#include - -#include "ouster/client.h" -#include "ouster/compat.h" - -namespace sensor = ouster::sensor; - -uint64_t n_lidar_packets = 0; -uint64_t n_imu_packets = 0; - -uint64_t lidar_col_0_ts = 0; -uint64_t imu_ts = 0; - -float lidar_col_0_h_angle = 0.0; -float imu_av_z = 0.0; -float imu_la_y = 0.0; - -void handle_lidar(uint8_t* buf, const sensor::packet_format& pf) { - n_lidar_packets++; - lidar_col_0_ts = pf.col_timestamp(pf.nth_col(0, buf)); - lidar_col_0_h_angle = pf.col_h_angle(pf.nth_col(0, buf)); -} - -void handle_imu(uint8_t* buf, const sensor::packet_format& pf) { - n_imu_packets++; - imu_ts = pf.imu_gyro_ts(buf); - imu_av_z = pf.imu_av_z(buf); - imu_la_y = pf.imu_la_y(buf); -} - -void print_headers() { - std::cout << std::setw(15) << "n_lidar_packets" << std::setw(15) - << "col_0_azimuth" << std::setw(20) << "col_0_ts" << std::setw(15) - << "n_imu_packets" << std::setw(15) << "im_av_z" << std::setw(15) - << "im_la_y" << std::setw(20) << "imu_ts" << std::endl; -} - -void print_stats() { - std::cout << "\r" << std::setw(15) << n_lidar_packets << std::setw(15) - << lidar_col_0_h_angle << std::setw(20) << lidar_col_0_ts - << std::setw(15) << n_imu_packets << std::setw(15) << imu_av_z - << std::setw(15) << imu_la_y << std::setw(20) << imu_ts; - std::flush(std::cout); -} - -int main(int argc, char** argv) { - if (argc != 3) { - std::cerr << "Usage: ouster_client_example " - "" - << std::endl; - return 1; - } - socket_init(); - auto cli = sensor::init_client(argv[1], argv[2]); - if (!cli) { - std::cerr << "Failed to connect to client at: " << argv[1] << std::endl; - return 1; - } - - auto metadata = sensor::get_metadata(*cli); - auto info = sensor::parse_metadata(metadata); - auto pf = sensor::get_format(info); - - std::unique_ptr lidar_buf(new uint8_t[pf.lidar_packet_size + 1]); - std::unique_ptr imu_buf(new uint8_t[pf.imu_packet_size + 1]); - - print_headers(); - - while (true) { - sensor::client_state st = sensor::poll_client(*cli); - if (st & sensor::CLIENT_ERROR) { - return 1; - } else if (st & sensor::LIDAR_DATA) { - if (sensor::read_lidar_packet(*cli, lidar_buf.get(), pf)) - handle_lidar(lidar_buf.get(), pf); - } else if (st & sensor::IMU_DATA) { - if (sensor::read_imu_packet(*cli, imu_buf.get(), pf)) - handle_imu(imu_buf.get(), pf); - } - - if (n_imu_packets % 50 == 0) print_stats(); - } - socket_quit(); - return 0; -} diff --git a/ouster_client/src/compat.cpp b/ouster_client/src/netcompat.cpp similarity index 63% rename from ouster_client/src/compat.cpp rename to ouster_client/src/netcompat.cpp index d4c5fe8b..9f8571d8 100644 --- a/ouster_client/src/compat.cpp +++ b/ouster_client/src/netcompat.cpp @@ -1,26 +1,36 @@ -#include "ouster/compat.h" +#include "ouster/impl/netcompat.h" -#include -#include -#include #include -int socket_init(void) { -#ifdef _WIN32 - WSADATA wsa_data; - return WSAStartup(MAKEWORD(1, 1), &wsa_data); +#if defined _WIN32 + +#include + #else - return 0; + +#include +#include +#include + +#include +#include + #endif -} -int socket_quit(void) { +namespace ouster { +namespace impl { + #ifdef _WIN32 - return WSACleanup(); -#else - return 0; +struct StaticWrapper { + WSADATA wsa_data; + + StaticWrapper() { WSAStartup(MAKEWORD(1, 1), &wsa_data); } + + ~StaticWrapper() { WSACleanup(); } +}; + +static StaticWrapper resources = {}; #endif -} int socket_close(SOCKET sock) { int status = 0; @@ -42,20 +52,14 @@ int socket_close(SOCKET sock) { std::string socket_get_error() { #ifdef _WIN32 - wchar_t* char_buffer = NULL; - std::string result; - - FormatMessageW(FORMAT_MESSAGE_ALLOCATE_BUFFER | FORMAT_MESSAGE_FROM_SYSTEM | - FORMAT_MESSAGE_IGNORE_INSERTS, - NULL, WSAGetLastError(), - MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT), - (LPWSTR)&char_buffer, 0, NULL); - - std::wstring wide_string(char_buffer); - result = std::string(wide_string.begin(), wide_string.end()); - LocalFree(char_buffer); - - return result; + int errnum = WSAGetLastError(); + char buf[256] = {0}; + if (FormatMessage(FORMAT_MESSAGE_FROM_SYSTEM, NULL, errnum, 0, buf, + sizeof(buf), NULL) != 0) { + return std::string(buf); + } else { + return std::string{"Unknown WSA error "} + std::to_string(errnum); + } #else return std::strerror(errno); #endif @@ -96,4 +100,7 @@ int socket_set_reuse(SOCKET value) { int option = 1; return setsockopt(value, SOL_SOCKET, SO_REUSEADDR, &option, sizeof(option)); #endif -} \ No newline at end of file +} + +} // namespace impl +} // namespace ouster diff --git a/ouster_client/src/types.cpp b/ouster_client/src/types.cpp index be8e7ced..c4fd7208 100644 --- a/ouster_client/src/types.cpp +++ b/ouster_client/src/types.cpp @@ -2,16 +2,7 @@ #include -#if defined _WIN32 -#pragma warning(push, 2) -#endif - #include - -#if defined _WIN32 -#pragma warning(pop) -#endif - #include #include #include @@ -21,7 +12,8 @@ #include #include -#include "ouster/packet.h" +#include "ouster/build.h" +#include "ouster/impl/parsing.h" #include "ouster/version.h" namespace ouster { @@ -110,18 +102,25 @@ sensor_info default_sensor_info(lidar_mode mode) { mat4d::Identity()}; } + +constexpr packet_format packet_1_13 = impl::packet_2_0<64>(); +constexpr packet_format packet_2_0_16 = impl::packet_2_0<16>(); +constexpr packet_format packet_2_0_32 = impl::packet_2_0<32>(); +constexpr packet_format packet_2_0_64 = impl::packet_2_0<64>(); +constexpr packet_format packet_2_0_128 = impl::packet_2_0<128>(); + const packet_format& get_format(const sensor_info& info) { switch (info.format.pixels_per_column) { case 16: - return packet__1_14_0__16; + return packet_2_0_16; case 32: - return packet__1_14_0__32; + return packet_2_0_32; case 64: - return packet__1_14_0__64; + return packet_2_0_64; case 128: - return packet__1_14_0__128; + return packet_2_0_128; default: - return packet__1_13_0; + return packet_1_13; } } @@ -215,8 +214,7 @@ sensor_info parse_metadata(const std::string& meta) { info.mode = lidar_mode_of_string(root["lidar_mode"].asString()); info.prod_line = root["prod_line"].asString(); - // "data_format" introduced in fw 1.14. Fall back to common 1.13 parameters - // otherwise + // "data_format" introduced in fw 2.0. Fall back to 1.13 if (root.isMember("data_format")) { info.format.pixels_per_column = root["data_format"]["pixels_per_column"].asInt(); @@ -237,8 +235,7 @@ sensor_info parse_metadata(const std::string& meta) { info.format = default_data_format(info.mode); } - // "lidar_origin_to_beam_origin_mm" introduced in fw 1.14. Fall back to - // common 1.13 parameters otherwise + // "lidar_origin_to_beam_origin_mm" introduced in fw 2.0. Fall back to 1.13 if (root.isMember("lidar_origin_to_beam_origin_mm")) { info.lidar_origin_to_beam_origin_mm = root["lidar_origin_to_beam_origin_mm"].asDouble(); @@ -263,24 +260,36 @@ sensor_info parse_metadata(const std::string& meta) { for (const auto& v : root["beam_azimuth_angles"]) info.beam_azimuth_angles.push_back(v.asDouble()); - if (root["imu_to_sensor_transform"].size() != 16) { - throw std::runtime_error{ - "imu_to_sensor_transform must have 16 elements"}; - } - - if (root["lidar_to_sensor_transform"].size() != 16) { - throw std::runtime_error{ - "lidar_to_sensor_transform must have 16 elements"}; + // "imu_to_sensor_transform" may be absent in sensor config + // produced by Ouster Studio, so we backfill it with default value + if (root["imu_to_sensor_transform"].size() == 16) { + for (int i = 0; i < 4; i++) { + for (int j = 0; j < 4; j++) { + const Json::Value::ArrayIndex ind = i * 4 + j; + info.imu_to_sensor_transform(i, j) = + root["imu_to_sensor_transform"][ind].asDouble(); + } + } + } else { + std::cerr << "WARNING: No valid imu_to_sensor_transform found." + << std::endl; + info.imu_to_sensor_transform = default_imu_to_sensor_transform; } - for (size_t i = 0; i < 4; i++) { - for (size_t j = 0; j < 4; j++) { - const Json::Value::ArrayIndex ind = i * 4 + j; - info.imu_to_sensor_transform(i, j) = - root["imu_to_sensor_transform"][ind].asDouble(); - info.lidar_to_sensor_transform(i, j) = - root["lidar_to_sensor_transform"][ind].asDouble(); + // "lidar_to_sensor_transform" may be absent in sensor config + // produced by Ouster Studio, so we backfill it with default value + if (root["lidar_to_sensor_transform"].size() == 16) { + for (int i = 0; i < 4; i++) { + for (int j = 0; j < 4; j++) { + const Json::Value::ArrayIndex ind = i * 4 + j; + info.lidar_to_sensor_transform(i, j) = + root["lidar_to_sensor_transform"][ind].asDouble(); + } } + } else { + std::cerr << "WARNING: No valid lidar_to_sensor_transform found." + << std::endl; + info.lidar_to_sensor_transform = default_lidar_to_sensor_transform; } info.extrinsic = mat4d::Identity(); @@ -306,6 +315,7 @@ sensor_info metadata_from_json(const std::string& json_file) { std::string to_string(const sensor_info& info) { Json::Value root{}; + root["client_version"] = ouster::CLIENT_VERSION; root["hostname"] = info.name; root["prod_sn"] = info.sn; root["build_rev"] = info.fw_rev; diff --git a/ouster_ros/.gitignore b/ouster_ros/.gitignore deleted file mode 100644 index 567609b1..00000000 --- a/ouster_ros/.gitignore +++ /dev/null @@ -1 +0,0 @@ -build/ diff --git a/ouster_ros/CMakeLists.txt b/ouster_ros/CMakeLists.txt index 8b9e6038..52b3ce93 100644 --- a/ouster_ros/CMakeLists.txt +++ b/ouster_ros/CMakeLists.txt @@ -1,11 +1,15 @@ cmake_minimum_required(VERSION 3.1.0) +list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/../cmake) +include(DefaultBuildType) + # ==== Project Name ==== project(ouster_ros) # ==== Requirements ==== -find_package(PkgConfig REQUIRED) -pkg_check_modules(jsoncpp REQUIRED jsoncpp) +find_package(Eigen3 REQUIRED) +find_package(GLEW REQUIRED) +find_package(glfw3 REQUIRED) find_package( catkin REQUIRED @@ -20,76 +24,75 @@ find_package( tf2_ros tf2_geometry_msgs) -find_package(Threads REQUIRED) -find_package(ouster_client REQUIRED HINTS ../) -find_package(ouster_viz REQUIRED HINTS ../) - # ==== Options ==== set(CMAKE_CXX_STANDARD 14) add_compile_options(-Wall -Wextra -Werror) +option(CMAKE_POSITION_INDEPENDENT_CODE "Build position independent code." ON) # ==== Catkin ==== add_message_files(FILES PacketMsg.msg) add_service_files(FILES OSConfigSrv.srv) generate_messages(DEPENDENCIES std_msgs sensor_msgs geometry_msgs) +set(_ouster_ros_INCLUDE_DIRS "include;../ouster_client/include;../ouster_viz/include") + catkin_package( INCLUDE_DIRS - include + ${_ouster_ros_INCLUDE_DIRS} LIBRARIES - ouster_ros + ouster_ros CATKIN_DEPENDS - roscpp - message_runtime - pcl_ros - std_msgs - sensor_msgs - geometry_msgs) + roscpp + message_runtime + pcl_ros + std_msgs + sensor_msgs + geometry_msgs + DEPENDS + EIGEN3 GLFW3 GLEW) # ==== Libraries ==== -add_library(ouster_ros STATIC src/ros.cpp) -target_link_libraries( - ouster_ros - PUBLIC ${catkin_LIBRARIES} - PRIVATE ouster_client) -set_target_properties(ouster_ros PROPERTIES POSITION_INDEPENDENT_CODE ON) -target_include_directories(ouster_ros PUBLIC include ${catkin_INCLUDE_DIRS}) -target_compile_options(ouster_ros PRIVATE "$<$:${FLAGS_DEBUG}>") -target_compile_options(ouster_ros - PRIVATE "$<$:${FLAGS_RELEASE}>") -target_compile_options(ouster_ros PRIVATE "${FLAGS}") +# 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) +add_subdirectory(../ouster_viz ouster_viz EXCLUDE_FROM_ALL) +set(BUILD_SHARED_LIBS ${_SAVE_BUILD_SHARED_LIBS}) + +# catkin adds all include dirs to a single variable, don't try to use targets +include_directories(${_ouster_ros_INCLUDE_DIRS} SYSTEM ${catkin_INCLUDE_DIRS}) + +add_library(ouster_ros src/ros.cpp) +target_link_libraries(ouster_ros PUBLIC ${catkin_LIBRARIES} ouster_build PRIVATE + -Wl,--whole-archive ouster_client ouster_viz -Wl,--no-whole-archive) 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} ouster_viz - ouster_client) -target_include_directories(os_node PRIVATE include ${catkin_INCLUDE_DIRS}) +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} ouster_viz - ouster_client ${VTK_LIBRARIES}) -target_include_directories(os_cloud_node PRIVATE include - ${catkin_INCLUDE_DIRS}) +target_link_libraries(os_cloud_node ouster_ros ${catkin_LIBRARIES}) add_dependencies(os_cloud_node ${PROJECT_NAME}_gencpp) add_executable(viz_node src/viz_node.cpp) -target_link_libraries(viz_node ${catkin_LIBRARIES} ouster_viz ouster_client - glfw GLEW) -target_include_directories(viz_node PRIVATE include ${catkin_INCLUDE_DIRS}) +target_link_libraries(viz_node ouster_ros ${catkin_LIBRARIES} glfw GLEW) add_dependencies(viz_node ${PROJECT_NAME}_gencpp) add_executable(img_node src/img_node.cpp) -target_link_libraries(img_node ouster_ros ${catkin_LIBRARIES} ouster_viz - ouster_client ${VTK_LIBRARIES}) -target_include_directories(img_node PRIVATE include ${catkin_INCLUDE_DIRS}) +target_link_libraries(img_node ouster_ros ${catkin_LIBRARIES}) add_dependencies(img_node ${PROJECT_NAME}_gencpp) # ==== Install ==== install( - TARGETS os_node os_cloud_node viz_node img_node + TARGETS ouster_ros os_node os_cloud_node viz_node img_node 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 ouster.launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) diff --git a/ouster_ros/README.md b/ouster_ros/README.md deleted file mode 100644 index d55beb42..00000000 --- a/ouster_ros/README.md +++ /dev/null @@ -1,67 +0,0 @@ -# Example ROS Node - -## Contents -* `ouster_ros/` contains sample code for publishing sensor data as standard ROS topics - -## Operating System Support -* Tested with [ROS Kinetic](http://wiki.ros.org/kinetic/Installation/Ubuntu) on Ubuntu 16.04, [ROS - Melodic](http://wiki.ros.org/melodic/Installation/Ubuntu) on Ubuntu 18.04, and [ROS - Noetic](http://wiki.ros.org/noetic/Installation/Ubuntu) on Ubuntu 20.04 - -## Building the Sample ROS Nodes -* In the following instruction steps, `/path/to/ouster_example` is where you've cloned the repository -* Requires the `ros-*-ros-core`, `ros-*-pcl-ros`, `ros-*-tf2-geometry-msgs` packages and, - optionally, `ros-*-rviz` for visualization using ROS, where `*` is `kinetic`, `melodic`, or - `noetic` -* Install additional build dependencies with: `sudo apt-get install libglfw3-dev libglew-dev - libtclap-dev` -* Then run the following command `export CMAKE_PREFIX_PATH=/path/to/ouster_example` -* Be sure to source the ROS setup script before building: `source /opt/ros/*/setup.bash` -* Build with `mkdir -p myworkspace/src && cd myworkspace && ln -s - /path/to/ouster_example ./src/ && catkin_make -DCMAKE_BUILD_TYPE=Release` - -## Running the Sample ROS Nodes -* Make sure the sensor is connected to the network and has obtained a DHCP - lease. See section 3.1 in the accompanying [software user guide](https://www.ouster.com/resources) - for more details -* In each new terminal for each command below: - - Make sure to source ROS environment with `source /path/to/myworkspace/devel/setup.bash` where - `/path/to/myworkspace` is where the path to the workspace directory that was created when - building the ROS nodes -* To publish ROS topics from a running sensor from within the `ouster_ros` directory: - - Run `roslaunch ouster.launch sensor_hostname:= - udp_dest:= lidar_mode:= viz:=`where: - - `` can be the hostname (os-991xxxxxxxxx) or IP of the sensor - - `` is the IP to which the sensor should send data - - `` is one of `512x10`, `512x20`, `1024x10`, `1024x20`, or `2048x10` - - `` is either `true` or `false`. If true, a window should open and start - displaying data after a few seconds - - See [ouster_viz](../ouster_viz/README.md) for documentation on using the visualizer -* **To display sensor output using the Ouster Visualizer in ROS:** - - set `` to `true` in the roslaunch command. - - Example command: - - `roslaunch ouster.launch sensor_hostname:=os-991234567890.local - udp_dest:=192.0.2.1 lidar_mode:=2048x10 viz:=true` -* To record raw sensor output - - In another terminal instance, run `rosbag record /os_node/imu_packets - /os_node/lidar_packets` - - This will save a .bag file of recorded data in that directory -* To publish ROS topics from recorded data from withint the `ouster_ros` directory: - - Run `roslaunch ouster.launch replay:=true - sensor_hostname:=` - - In a second terminal run `rosbag play --clock ` - - Note: `os_node` reads and writes metadata to `${ROS_HOME}` to enable - accurately replaying raw data. By default, the name of this file is based - on the hostname of the sensor. The location of this file can be overridden - using the `metadata:=` flag - - If a metadata file is not available, the visualizer will default to - 1024x10. This can be overridden with the `lidar_mode` - parameter. Visualizer output will only be correct if the same `lidar_mode` - parameter is used for both recording and replay -* To display sensor output using built-in ROS tools (rviz): - - Follow the instructions above for running the example ROS code with a - sensor or recorded data - - To visualize output using rviz, run `rviz -d /path/to/ouster_ros/viz.rviz` - in another terminal - - To view lidar intensity/noise/range images, add `image:=true` to either of - the `roslaunch` commands above diff --git a/ouster_ros/include/ouster_ros/point.h b/ouster_ros/include/ouster_ros/point.h index aa0e270f..977a9b56 100644 --- a/ouster_ros/include/ouster_ros/point.h +++ b/ouster_ros/include/ouster_ros/point.h @@ -21,7 +21,7 @@ struct EIGEN_ALIGN16 Point { uint32_t t; uint16_t reflectivity; uint8_t ring; - uint16_t noise; + uint16_t ambient; uint32_t range; EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; @@ -37,7 +37,7 @@ POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_ros::Point, (std::uint32_t, t, t) (std::uint16_t, reflectivity, reflectivity) (std::uint8_t, ring, ring) - (std::uint16_t, noise, noise) + (std::uint16_t, ambient, ambient) (std::uint32_t, range, range) ) // clang-format on diff --git a/ouster_ros/include/ouster_ros/ros.h b/ouster_ros/include/ouster_ros/ros.h index e368f5ff..b3e1a428 100644 --- a/ouster_ros/include/ouster_ros/ros.h +++ b/ouster_ros/include/ouster_ros/ros.h @@ -15,8 +15,8 @@ #include #include "ouster/client.h" -#include "ouster/types.h" #include "ouster/lidar_scan.h" +#include "ouster/types.h" #include "ouster_ros/PacketMsg.h" #include "ouster_ros/point.h" @@ -85,6 +85,6 @@ sensor_msgs::PointCloud2 cloud_to_cloud_msg(const Cloud& cloud, ns timestamp, * @return ROS message suitable for publishing as a transform */ geometry_msgs::TransformStamped transform_to_tf_msg( - const sensor::mat4d& mat, const std::string& frame, + const ouster::mat4d& mat, const std::string& frame, const std::string& child_frame); } // namespace ouster_ros diff --git a/ouster_ros/ouster.launch b/ouster_ros/ouster.launch index b40cf419..39323766 100644 --- a/ouster_ros/ouster.launch +++ b/ouster_ros/ouster.launch @@ -9,7 +9,7 @@ - + @@ -40,7 +40,7 @@ - + diff --git a/ouster_ros/package.xml b/ouster_ros/package.xml index c1606897..64a7bb54 100644 --- a/ouster_ros/package.xml +++ b/ouster_ros/package.xml @@ -8,6 +8,7 @@ catkin libjsoncpp + libeigen3 roscpp message_generation std_msgs @@ -18,8 +19,6 @@ tf2 tf2_ros tf2_geometry_msgs - ouster_client - ouster_viz libjsoncpp roscpp @@ -29,7 +28,6 @@ geometry_msgs pcl_ros pcl_conversions - ouster_client - ouster_viz + diff --git a/ouster_ros/src/img_node.cpp b/ouster_ros/src/img_node.cpp index a6968d81..e6978a27 100644 --- a/ouster_ros/src/img_node.cpp +++ b/ouster_ros/src/img_node.cpp @@ -1,8 +1,8 @@ /** * @file - * @brief Example node to visualize range, noise and intensity images + * @brief Example node to visualize range, ambient and intensity images * - * Publishes ~/range_image, ~/noise_image, and ~/intensity_image. Please bear + * Publishes ~/range_image, ~/ambient_image, and ~/intensity_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 */ @@ -58,15 +58,15 @@ int main(int argc, char** argv) { ros::Publisher range_image_pub = nh.advertise("range_image", 100); - ros::Publisher noise_image_pub = - nh.advertise("noise_image", 100); + ros::Publisher ambient_image_pub = + nh.advertise("ambient_image", 100); ros::Publisher intensity_image_pub = nh.advertise("intensity_image", 100); ouster_ros::Cloud cloud{}; - viz::AutoExposure noise_ae, intensity_ae; - viz::BeamUniformityCorrector noise_buc; + viz::AutoExposure ambient_ae, intensity_ae; + viz::BeamUniformityCorrector ambient_buc; std::stringstream encoding_ss; encoding_ss << "mono" << bit_depth; @@ -76,7 +76,7 @@ int main(int argc, char** argv) { pcl::fromROSMsg(*m, cloud); sensor_msgs::Image range_image; - sensor_msgs::Image noise_image; + sensor_msgs::Image ambient_image; sensor_msgs::Image intensity_image; range_image.width = W; @@ -87,13 +87,13 @@ int main(int argc, char** argv) { (8 * sizeof(*range_image.data.data()))); range_image.header.stamp = m->header.stamp; - noise_image.width = W; - noise_image.height = H; - noise_image.step = W; - noise_image.encoding = encoding; - noise_image.data.resize(W * H * bit_depth / - (8 * sizeof(*noise_image.data.data()))); - noise_image.header.stamp = m->header.stamp; + ambient_image.width = W; + ambient_image.height = H; + ambient_image.step = W; + ambient_image.encoding = encoding; + ambient_image.data.resize(W * H * bit_depth / + (8 * sizeof(*ambient_image.data.data()))); + ambient_image.header.stamp = m->header.stamp; intensity_image.width = W; intensity_image.height = H; @@ -105,7 +105,7 @@ int main(int argc, char** argv) { using im_t = Eigen::Array; - im_t noise_image_eigen(H, W); + im_t ambient_image_eigen(H, W); im_t intensity_image_eigen(H, W); for (size_t u = 0; u < H; u++) { @@ -124,22 +124,23 @@ int main(int argc, char** argv) { std::min(std::round(pt.range * range_multiplier), static_cast(pixel_value_max)); } - noise_image_eigen(u, v) = pt.noise; + ambient_image_eigen(u, v) = pt.ambient; intensity_image_eigen(u, v) = pt.intensity; } } - noise_buc.correct(noise_image_eigen); - noise_ae(Eigen::Map(noise_image_eigen.data(), W * H)); + ambient_buc.correct(ambient_image_eigen); + ambient_ae( + Eigen::Map(ambient_image_eigen.data(), W * H)); intensity_ae( Eigen::Map(intensity_image_eigen.data(), W * H)); - noise_image_eigen = noise_image_eigen.sqrt(); + ambient_image_eigen = ambient_image_eigen.sqrt(); intensity_image_eigen = intensity_image_eigen.sqrt(); for (size_t u = 0; u < H; u++) { for (size_t v = 0; v < W; v++) { reinterpret_cast( - noise_image.data.data())[u * W + v] = - noise_image_eigen(u, v) * pixel_value_max; + ambient_image.data.data())[u * W + v] = + ambient_image_eigen(u, v) * pixel_value_max; reinterpret_cast( intensity_image.data.data())[u * W + v] = intensity_image_eigen(u, v) * pixel_value_max; @@ -147,7 +148,7 @@ int main(int argc, char** argv) { } range_image_pub.publish(range_image); - noise_image_pub.publish(noise_image); + ambient_image_pub.publish(ambient_image); intensity_image_pub.publish(intensity_image); }; diff --git a/ouster_ros/src/os_cloud_node.cpp b/ouster_ros/src/os_cloud_node.cpp index ceeb4beb..1bcfcf12 100644 --- a/ouster_ros/src/os_cloud_node.cpp +++ b/ouster_ros/src/os_cloud_node.cpp @@ -10,10 +10,11 @@ #include #include +#include #include +#include #include "ouster/lidar_scan.h" -#include "ouster/packet.h" #include "ouster/types.h" #include "ouster_ros/OSConfigSrv.h" #include "ouster_ros/PacketMsg.h" @@ -56,15 +57,20 @@ int main(int argc, char** argv) { Cloud cloud{W, H}; ouster::LidarScan ls{W, H}; - auto batch_and_publish = sensor::batch_to_scan( - W, pf, [&](std::chrono::nanoseconds scan_ts) mutable { - scan_to_cloud(xyz_lut, scan_ts, ls, cloud); - lidar_pub.publish( - ouster_ros::cloud_to_cloud_msg(cloud, scan_ts, sensor_frame)); - }); + ouster::ScanBatcher batch(W, pf); auto lidar_handler = [&](const PacketMsg& pm) mutable { - batch_and_publish(pm.buf.data(), ls); + 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()) { + scan_to_cloud(xyz_lut, h->timestamp, ls, cloud); + lidar_pub.publish(ouster_ros::cloud_to_cloud_msg( + cloud, h->timestamp, sensor_frame)); + } + } }; auto imu_handler = [&](const PacketMsg& p) { diff --git a/ouster_ros/src/os_node.cpp b/ouster_ros/src/os_node.cpp index 6cff5acf..72d324ae 100644 --- a/ouster_ros/src/os_node.cpp +++ b/ouster_ros/src/os_node.cpp @@ -16,6 +16,7 @@ #include #include +#include "ouster/build.h" #include "ouster/types.h" #include "ouster_ros/OSConfigSrv.h" #include "ouster_ros/PacketMsg.h" @@ -159,6 +160,8 @@ int main(int argc, char** argv) { return EXIT_FAILURE; } + ROS_INFO("Client version: %s", ouster::CLIENT_VERSION_FULL); + if (replay) { ROS_INFO("Running in replay mode"); diff --git a/ouster_ros/src/ros.cpp b/ouster_ros/src/ros.cpp index 16d62c38..5d690558 100644 --- a/ouster_ros/src/ros.cpp +++ b/ouster_ros/src/ros.cpp @@ -74,17 +74,15 @@ void scan_to_cloud(const ouster::XYZLut& xyz_lut, for (auto v = 0; v < ls.w; v++) { const auto xyz = points.row(u * ls.w + v); const auto pix = ls.data.row(u * ls.w + v); - const auto ts = (ls.ts[v] - scan_ts).count(); + const auto ts = (ls.header(v).timestamp - scan_ts).count(); cloud(v, u) = ouster_ros::Point{ - static_cast(xyz(0)), - static_cast(xyz(1)), - static_cast(xyz(2)), - 1.0f, + {{static_cast(xyz(0)), static_cast(xyz(1)), + static_cast(xyz(2)), 1.0f}}, static_cast(pix(ouster::LidarScan::INTENSITY)), static_cast(ts), static_cast(pix(ouster::LidarScan::REFLECTIVITY)), static_cast(u), - static_cast(pix(ouster::LidarScan::NOISE)), + static_cast(pix(ouster::LidarScan::AMBIENT)), static_cast(pix(ouster::LidarScan::REFLECTIVITY))}; } } @@ -100,7 +98,7 @@ sensor_msgs::PointCloud2 cloud_to_cloud_msg(const Cloud& cloud, ns timestamp, } geometry_msgs::TransformStamped transform_to_tf_msg( - const sensor::mat4d& mat, const std::string& frame, + const ouster::mat4d& mat, const std::string& frame, const std::string& child_frame) { Eigen::Affine3d aff; aff.linear() = mat.block<3, 3>(0, 0); diff --git a/ouster_ros/src/viz_node.cpp b/ouster_ros/src/viz_node.cpp index f4083e67..cf1069d2 100644 --- a/ouster_ros/src/viz_node.cpp +++ b/ouster_ros/src/viz_node.cpp @@ -14,7 +14,6 @@ #include "ouster/lidar_scan.h" #include "ouster/lidar_scan_viz.h" -#include "ouster/packet.h" #include "ouster/point_viz.h" #include "ouster/types.h" #include "ouster_ros/OSConfigSrv.h" @@ -56,12 +55,10 @@ int main(int argc, char** argv) { ouster::LidarScan ls(W, H); - auto batch_and_display = sensor::batch_to_scan( - W, packet_format, - [&](std::chrono::nanoseconds) mutable { lidar_scan_viz.draw(ls); }); + ouster::ScanBatcher batch(W, packet_format); auto lidar_handler = [&](const PacketMsg& pm) mutable { - batch_and_display(pm.buf.data(), ls); + if (batch(pm.buf.data(), ls)) lidar_scan_viz.draw(ls); }; auto lidar_packet_sub = nh.subscribe( diff --git a/ouster_ros/viz.rviz b/ouster_ros/viz.rviz index 4544527b..d001a450 100644 --- a/ouster_ros/viz.rviz +++ b/ouster_ros/viz.rviz @@ -82,7 +82,7 @@ Visualization Manager: Value: true - Class: rviz/Image Enabled: true - Image Topic: /img_node/noise_image + Image Topic: /img_node/ambient_image Max Value: 1 Median window: 5 Min Value: 0 diff --git a/ouster_viz/.gitignore b/ouster_viz/.gitignore deleted file mode 100644 index 567609b1..00000000 --- a/ouster_viz/.gitignore +++ /dev/null @@ -1 +0,0 @@ -build/ diff --git a/ouster_viz/CMakeLists.txt b/ouster_viz/CMakeLists.txt index 528cb080..fa421808 100644 --- a/ouster_viz/CMakeLists.txt +++ b/ouster_viz/CMakeLists.txt @@ -5,46 +5,18 @@ project(ouster_viz) # ==== Requirements ==== set(OpenGL_GL_PREFERENCE GLVND) -find_package(Threads) -find_package(Eigen3 REQUIRED) -find_package(ouster_client REQUIRED HINTS ../) find_package(OpenGL REQUIRED) +find_package(Threads) find_package(GLEW REQUIRED) - -# ==== Options ==== -set(CMAKE_CXX_STANDARD 11) - - -if(WIN32) - find_package(glfw3 CONFIG REQUIRED) - set(OUSTER_VIZ_ADDITIONAL_LIBS glfw GLEW::GLEW opengl32) - add_definitions(-D_USE_MATH_DEFINES) - add_compile_options(/W4) -elseif(APPLE) - find_package(glfw3 REQUIRED) - set(OUSTER_VIZ_ADDITIONAL_LIBS glfw GLEW::GLEW OpenGL::GL) - add_compile_options(-Wextra -Werror) - add_compile_options(-Wall) -else() - find_package(glfw3 REQUIRED) - set(OUSTER_VIZ_ADDITIONAL_LIBS glfw GLEW GL) - add_compile_options(-Wextra -Werror) - add_compile_options(-Wall) -endif() +find_package(glfw3 CONFIG REQUIRED) # ==== Libraries ==== -add_library(ouster_viz STATIC src/point_viz.cpp src/cloud.cpp src/camera.cpp - src/image.cpp) -target_link_libraries( - ouster_viz - PUBLIC ${CMAKE_THREAD_LIBS_INIT} - PRIVATE ouster_client ${OUSTER_VIZ_ADDITIONAL_LIBS}) +add_library(ouster_viz src/point_viz.cpp src/cloud.cpp src/camera.cpp src/image.cpp) +target_link_libraries(ouster_viz + PUBLIC Threads::Threads glfw GLEW::GLEW OpenGL::GL + PRIVATE ouster_client) target_include_directories(ouster_viz PUBLIC include) -target_include_directories(ouster_viz SYSTEM PRIVATE ${EIGEN3_INCLUDE_DIR}) # ==== Executables ==== add_executable(simple_viz src/main.cpp) -target_link_libraries(simple_viz ${CMAKE_THREAD_LIBS_INIT} - ${OUSTER_VIZ_ADDITIONAL_LIBS} ouster_client ouster_viz) -target_include_directories(simple_viz PRIVATE include) -target_include_directories(simple_viz SYSTEM PRIVATE ${EIGEN3_INCLUDE_DIR}) +target_link_libraries(simple_viz ouster_client ouster_viz) diff --git a/ouster_viz/CMakeSettings.json b/ouster_viz/CMakeSettings.json deleted file mode 100755 index 7e91e2c8..00000000 --- a/ouster_viz/CMakeSettings.json +++ /dev/null @@ -1,40 +0,0 @@ -{ - "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-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": [] - } - ] -} diff --git a/ouster_viz/README.md b/ouster_viz/README.md deleted file mode 100644 index 39a7f942..00000000 --- a/ouster_viz/README.md +++ /dev/null @@ -1,104 +0,0 @@ -# Example Visualizer - -## Contents -* `ouster_viz/` contains a basic visualizer that can be used to - display point clouds and range/intensity/ambient images -* Can be built both with and without ROS. See the instructions in - [ouster_ros](../ouster_ros/README.md) for building in a ROS environment - -## Operating System Support -The visualizer is cross-platform and works on Linux, Mac, and Windows. The current supported -platforms are: - -* Ubuntu 16.04 -* Ubuntu 18.04 -* Arch Linux - -Preliminary compatibility has been added for: - -* Windows 10 -* macOS 10.15 - -though these are not officially supported and not yet guaranteed to build and run correctly. - -# Build Dependencies -* The sample visualizer requires a compiler supporting C++11 or newer - and CMake 3.1 or newer -* Requires GLFW3, GLEW and Eigen3 libraries -* Using Ubuntu: `sudo apt-get install libglfw3-dev libglew-dev libeigen3-dev libjsoncpp-dev - libtclap-dev` -* Using Arch Linux: `sudo pacman -S cmake glfw glew eigen jsoncpp tclap` -* Using MacOS: install XCode and [Brew](https://brew.sh/) and then `brew install cmake pkg-config - glfw glew eigen jsoncpp tclap` -* Using Windows: install Visual Studio and [vcpkg](https://github.com/microsoft/vcpkg) and then - `vcpkg install glfw3 glew tclap jsoncpp eigen3` - -# Building the Visualizer -Before building, make sure all the build dependencies are installed. - -In the following instruction steps, `/path/to/ouster_example` is where you've cloned the repository - -## Linux -Run the following commmands: - -``` -export CMAKE_PREFIX_PATH=/path/to/ouster_example -cd /path/to/ouster_example/ouster_viz -mkdir build -cd build -cmake -DCMAKE_BUILD_TYPE=Release .. -make -``` - -## macOS -Run the following commands - -``` -export CMAKE_PREFIX_PATH=/path/to/ouster_example -cd /path/to/ouster_example/ouster_viz -mkdir build -cd build -cmake -DCMAKE_BUILD_TYPE=Release -DBUILD_SHARED_LIBS=OFF .. -make -``` - -## Running the Visualizer -* An executable called `simple_viz` is generated in the build directory -* Note: if compiling in an environment with ROS, the location of the - executable will be different -* To run: `./simple_viz ` -* For help, run `./simple_viz -h` - -## Command Line Arguments -* `` the hostname or IP address of the sensor -* `` the IP to which the sensor should send data -* `-m <512x10 | 512x20 | 1024x10 | 1024x20 | 2048x10>` flag to set the lidar - mode (horizontal resolution x rotation rate). Defaults to 1024x10. - -## Key bindings -| key | what it does | -| ----| ------------ | -| `p` | Increase point size | -| `o` | Decrease point size | -| `m` | Cycle point cloud coloring by intensity, noise, reflectivity, range | -| `v` | Toggle color cycling in range image | -| `n` | Toggle display ambient image from the sensor | -| `r` | Toggle auto-rotating | -| `shift + r` | Reset camera | -| `e` | Change range and intensity image size| -| `;` | Increase spacing in range markers | -| `'` | Decrease spacing in range markers | -| `r` | Toggle auto rotate | -| `w` | Camera pitch up | -| `s` | Camera pitch down | -| `a` | Camera yaw left | -| `d` | Camera yaw right | -| `1` | Toggle point cloud visibility | -| `0` | Toggle orthographic camera | -| `=` | Zoom in | -| `-` | Zoom out | - -## Mouse control -* Click and drag rotates the view -* Middle click and drag moves the view -* Scroll adjusts how far away the camera is from the vehicle diff --git a/ouster_viz/cmake/ouster_vizConfig.cmake b/ouster_viz/cmake/ouster_vizConfig.cmake deleted file mode 100644 index c66e99a4..00000000 --- a/ouster_viz/cmake/ouster_vizConfig.cmake +++ /dev/null @@ -1,5 +0,0 @@ -# Guard against multiple includes -if(NOT TARGET OUSTER_VIZ_INCLUDED) - add_custom_target(OUSTER_VIZ_INCLUDED) - add_subdirectory(${CMAKE_CURRENT_LIST_DIR}/../ ${CMAKE_CURRENT_BINARY_DIR}/ouster_viz) -endif() diff --git a/ouster_viz/include/ouster/autoexposure.h b/ouster_viz/include/ouster/autoexposure.h index d3a5ca79..169c661c 100644 --- a/ouster_viz/include/ouster/autoexposure.h +++ b/ouster_viz/include/ouster/autoexposure.h @@ -7,16 +7,7 @@ */ #pragma once -#if defined _WIN32 -#pragma warning(push, 2) -#endif - #include - -#if defined _WIN32 -#pragma warning(pop) -#endif - #include #include diff --git a/ouster_viz/include/ouster/beam_uniformity.h b/ouster_viz/include/ouster/beam_uniformity.h index b476ec5c..bffe114a 100644 --- a/ouster_viz/include/ouster/beam_uniformity.h +++ b/ouster_viz/include/ouster/beam_uniformity.h @@ -2,21 +2,12 @@ * @file * @brief Corrects beam uniformity by minimizing median difference between rows, * thereby correcting subtle horizontal line artifacts in images, - * especially the noise image. + * especially the ambient image. */ #pragma once -#if defined _WIN32 -#pragma warning(push, 2) -#endif - #include - -#if defined _WIN32 -#pragma warning(pop) -#endif - #include #include diff --git a/ouster_viz/include/ouster/lidar_scan_viz.h b/ouster_viz/include/ouster/lidar_scan_viz.h index 86cf3b51..5d9bde2f 100644 --- a/ouster_viz/include/ouster/lidar_scan_viz.h +++ b/ouster_viz/include/ouster/lidar_scan_viz.h @@ -19,21 +19,21 @@ namespace viz { class LidarScanViz { AutoExposure range_ae; AutoExposure intensity_ae; - AutoExposure noise_ae; + AutoExposure ambient_ae; AutoExposure reflectivity_ae; - BeamUniformityCorrector noise_buc; + BeamUniformityCorrector ambient_buc; const std::vector px_offset; const double aspect_ratio; const size_t h, w; std::vector imdata; - std::atomic_bool show_noise; + std::atomic_bool show_ambient; std::atomic_int display_mode; std::atomic_bool cycle_range; enum CloudDisplayMode { MODE_RANGE = 0, MODE_INTENSITY = 1, - MODE_NOISE = 2, + MODE_AMBIENT = 2, MODE_REFLECTIVITY = 3, NUM_MODES = 4 }; @@ -58,12 +58,12 @@ class LidarScanViz { h(info.format.pixels_per_column), w(info.format.columns_per_frame), imdata(3 * h * w), - show_noise(true), + show_ambient(true), display_mode(MODE_INTENSITY), cycle_range(false), point_viz(point_viz_) { point_viz.attachKeyHandler( - GLFW_KEY_N, [this]() { this->show_noise = !this->show_noise; }); + GLFW_KEY_N, [this]() { this->show_ambient = !this->show_ambient; }); point_viz.attachKeyHandler(GLFW_KEY_M, [this]() { this->display_mode = (this->display_mode + 1) % NUM_MODES; @@ -77,8 +77,8 @@ class LidarScanViz { case MODE_RANGE: std::cerr << "Coloring point cloud by range" << std::endl; break; - case MODE_NOISE: - std::cerr << "Coloring point cloud by noise" << std::endl; + case MODE_AMBIENT: + std::cerr << "Coloring point cloud by ambient" << std::endl; break; case MODE_REFLECTIVITY: std::cerr << "Coloring point cloud by reflectivity" @@ -102,56 +102,54 @@ class LidarScanViz { */ void draw(const LidarScan& ls, const size_t which_cloud = 0, const bool cloud_swap = true, const bool show_image = true) { - Eigen::ArrayXd range = ls.range().cast(); + using glmap_t = + Eigen::Map>; + + img_t range = ls.field(LidarScan::RANGE).cast(); if (show_image || display_mode == MODE_RANGE) { if (!cycle_range) { - range_ae(range); + range_ae( + Eigen::Map(range.data(), range.size())); } } - Eigen::ArrayXd intensity = ls.intensity().cast(); + img_t intensity = ls.field(LidarScan::INTENSITY).cast(); if (show_image || display_mode == MODE_INTENSITY) { - intensity_ae(intensity); + intensity_ae( + Eigen::Map(intensity.data(), intensity.size())); } - Eigen::ArrayXd noise = ls.noise().cast(); - Eigen::ArrayXd intensity_destaggered = - destagger(intensity, px_offset); - Eigen::ArrayXd range_destaggered = destagger(range, px_offset); + img_t ambient = ls.field(LidarScan::AMBIENT).cast(); + auto intensity_destaggered = destagger(intensity, px_offset); + auto range_destaggered = destagger(range, px_offset); if (cycle_range) { - Eigen::Map>(imdata.data(), - w * h) = + glmap_t(imdata.data(), h, w) = range_destaggered.cast().unaryExpr( [](const GLfloat x) -> GLfloat { return std::fmod(x * sensor::range_unit, 2.0) * 0.5; }); } else { - Eigen::Map>( - imdata.data(), w * h) = range_destaggered.cast(); + glmap_t(imdata.data(), h, w) = range_destaggered.cast(); } - Eigen::Map>( - imdata.data() + w * h, w * h) = + glmap_t(imdata.data() + w * h, h, w) = intensity_destaggered.cast(); - Eigen::ArrayXd noise_destaggered(h * w); - if ((show_image && show_noise) || display_mode == MODE_NOISE) { - // we need to destagger noise because the + img_t ambient_destaggered{h, w}; + if ((show_image && show_ambient) || display_mode == MODE_AMBIENT) { + // we need to destagger ambient because the // BeamUniformityCorrector only works on destaggered stuff - noise_destaggered = destagger(noise, px_offset); - - noise_buc.correct( - Eigen::Map>( - noise_destaggered.data(), h, w)); - noise_ae(noise_destaggered); + ambient_destaggered = destagger(ambient, px_offset); + ambient_buc.correct(ambient_destaggered); + ambient_ae(Eigen::Map(ambient_destaggered.data(), + ambient_destaggered.size())); - if (show_image && show_noise) { - Eigen::Map>( - imdata.data() + 2 * w * h, w * h) = - noise_destaggered.cast(); + if (show_image && show_ambient) { + glmap_t(imdata.data() + 2 * w * h, h, w) = + ambient_destaggered.cast(); } } if (show_image) { - if (show_noise) { + if (show_ambient) { point_viz.resizeImage(w, 3 * h); point_viz.setImageAspectRatio(3 * aspect_ratio); } else { @@ -162,24 +160,27 @@ class LidarScanViz { point_viz.imageSwap(); } + auto range_data = ls.field(LidarScan::RANGE).data(); + switch (+display_mode) { case MODE_INTENSITY: - point_viz.setRangeAndKey(which_cloud, ls.range().data(), + point_viz.setRangeAndKey(which_cloud, range_data, intensity.data()); break; case MODE_RANGE: - point_viz.setRangeAndKey(which_cloud, ls.range().data(), - range.data()); + point_viz.setRangeAndKey(which_cloud, range_data, range.data()); break; - case MODE_NOISE: - noise = destagger(noise_destaggered, px_offset); - point_viz.setRangeAndKey(which_cloud, ls.range().data(), - noise.data()); + case MODE_AMBIENT: + ambient = stagger(ambient_destaggered, px_offset); + point_viz.setRangeAndKey(which_cloud, range_data, + ambient.data()); break; case MODE_REFLECTIVITY: - Eigen::ArrayXd reflectivity = ls.reflectivity().cast(); - reflectivity_ae(reflectivity); - point_viz.setRangeAndKey(which_cloud, ls.range().data(), + img_t reflectivity = + ls.field(LidarScan::REFLECTIVITY).cast(); + reflectivity_ae(Eigen::Map( + reflectivity.data(), reflectivity.size())); + point_viz.setRangeAndKey(which_cloud, range_data, reflectivity.data()); break; } diff --git a/ouster_viz/include/ouster/point_viz.h b/ouster_viz/include/ouster/point_viz.h index 0b9bb02f..bdf7ff37 100644 --- a/ouster_viz/include/ouster/point_viz.h +++ b/ouster_viz/include/ouster/point_viz.h @@ -5,14 +5,16 @@ * Contains headers and some inline functions for the main PointViz class * as well as supporting classes such as Camera, Image, Cloud, etc. */ - #pragma once + // clang-format off // glew must be included first so we prevent clang-format from rearranging #include #include // clang-format on +#include "ouster/colormaps.h" + #include #include #include @@ -26,19 +28,9 @@ #include #include -#if defined _WIN32 -#pragma warning(push, 2) -#endif - #include #include -#if defined _WIN32 -#pragma warning(pop) -#endif - -#include "ouster/colormaps.h" - namespace ouster { namespace viz { namespace impl { @@ -46,6 +38,7 @@ namespace impl { // compilation options as internal C++ code, leading to problems. besides, the // performance here is not super critical using mat4d = Eigen::Matrix; +using mat4f = Eigen::Matrix; constexpr int default_window_width = 640; constexpr int default_window_height = 480; @@ -178,6 +171,25 @@ static const std::string ring_fragment_shader_code = void main() { gl_FragColor = vec4(0.15, 0.15, 0.15, 1); })SHADER"; +static const std::string cuboid_vertex_shader_code = + R"SHADER( + #version 120 + attribute vec3 cuboid_xyz; + uniform vec4 cuboid_rgba; + uniform mat4 pose; + uniform mat4 proj_view; + varying vec4 rgba; + void main(){ + gl_Position = proj_view * pose * vec4(cuboid_xyz, 1.0); + rgba = cuboid_rgba; + })SHADER"; +static const std::string cuboid_fragment_shader_code = + R"SHADER( + #version 120 + varying vec4 rgba; + void main() { + gl_FragColor = rgba; + })SHADER"; static const std::string image_vertex_shader_code = R"SHADER( #version 120 @@ -921,8 +933,7 @@ class Rings { void draw(const Camera& camera) { glUseProgram(ring_program_id); const float radius = std::pow(10.0f, static_cast(ring_size)); - Eigen::Matrix camera_data = - camera.proj_view().cast(); + mat4f camera_data = camera.proj_view().cast(); glUniformMatrix4fv(ring_proj_view_id, 1, GL_FALSE, camera_data.data()); glEnableVertexAttribArray(ring_xyz_id); glBindBuffer(GL_ARRAY_BUFFER, xyz_buffer); @@ -948,6 +959,108 @@ class Rings { */ ~Rings() { glDeleteProgram(ring_program_id); } }; + +/** + * struct to deal with a single cuboid + */ +struct Cuboid { + mat4f pose; + std::array rgba; +}; + +/** + * Class to deal with showing cuboids + */ +class Cuboids { + const std::array xyz; + const std::array indices; + const std::array edge_indices; + std::vector cuboids; + + GLuint cuboid_program_id; + GLuint cuboid_xyz_id; + GLuint cuboid_proj_view_id; + GLuint cuboid_pose_id; + GLuint cuboid_rgba_id; + GLuint xyz_buffer; + + public: + bool enabled; + + /** + * constructor + */ + Cuboids() + : xyz{+0.5, +0.5, +0.5, +0.5, +0.5, -0.5, +0.5, -0.5, + +0.5, +0.5, -0.5, -0.5, -0.5, +0.5, +0.5, -0.5, + +0.5, -0.5, -0.5, -0.5, +0.5, -0.5, -0.5, -0.5}, + indices{0, 1, 3, 2, 6, 7, 5, 4, 2, 3, 7, 6, + 4, 5, 1, 0, 0, 2, 6, 4, 5, 7, 3, 1}, + edge_indices{0, 1, 1, 3, 3, 2, 2, 0, 4, 5, 5, 7, + 7, 6, 6, 4, 0, 4, 1, 5, 2, 6, 3, 7}, + enabled{true} {} + + /** + * initializes shader program, vertex buffers and handles after OpenGL + * context has been created + */ + void initialize() { + cuboid_program_id = load_shaders(cuboid_vertex_shader_code, + cuboid_fragment_shader_code); + cuboid_xyz_id = glGetAttribLocation(cuboid_program_id, "cuboid_xyz"); + cuboid_proj_view_id = + glGetUniformLocation(cuboid_program_id, "proj_view"); + cuboid_pose_id = glGetUniformLocation(cuboid_program_id, "pose"); + cuboid_rgba_id = glGetUniformLocation(cuboid_program_id, "cuboid_rgba"); + + glGenBuffers(1, &xyz_buffer); + glBindBuffer(GL_ARRAY_BUFFER, xyz_buffer); + glBufferData(GL_ARRAY_BUFFER, sizeof(GLfloat) * 24, xyz.data(), + GL_STATIC_DRAW); + } + + void clear() { cuboids.clear(); } + void push(Cuboid&& c) { cuboids.push_back(std::move(c)); } + + /** + * draws the cuboids from the point of view of the camera. + * The cuboids are always centered on the camera's target. + * + * @param camera The camera + */ + void draw(const Camera& camera) { + glUseProgram(cuboid_program_id); + const mat4f camera_data = camera.proj_view_target().cast(); + glUniformMatrix4fv(cuboid_proj_view_id, 1, GL_FALSE, + camera_data.data()); + glEnableVertexAttribArray(cuboid_xyz_id); + glBindBuffer(GL_ARRAY_BUFFER, xyz_buffer); + glVertexAttribPointer(cuboid_xyz_id, + 3, // size + GL_FLOAT, // type + GL_FALSE, // normalized? + 0, // stride + (void*)0 // array buffer offset + ); + for (const auto& cuboid : cuboids) { + const mat4f pose = cuboid.pose; + glUniformMatrix4fv(cuboid_pose_id, 1, GL_FALSE, pose.data()); + glUniform4fv(cuboid_rgba_id, 1, cuboid.rgba.data()); + + glDrawElements(GL_QUADS, 24, GL_UNSIGNED_BYTE, indices.data()); + auto rgba = cuboid.rgba; + rgba[3] = 1; + glUniform4fv(cuboid_rgba_id, 1, rgba.data()); + glDrawElements(GL_LINES, 24, GL_UNSIGNED_BYTE, edge_indices.data()); + } + glDisableVertexAttribArray(cuboid_xyz_id); + }; + + /** + * destructor + */ + ~Cuboids() { glDeleteProgram(cuboid_program_id); } +}; } // namespace impl class PointViz { @@ -956,6 +1069,7 @@ class PointViz { std::vector clouds; impl::DoubleBuffer image; + impl::DoubleBuffer cuboids; std::string name; GLFWwindow* window; impl::Camera camera; @@ -1196,6 +1310,23 @@ class PointViz { */ void imageSwap() { image.swap(); } + /** + * add a cuboid + * + * @param a cuboid + */ + void addCuboid(impl::Cuboid&& cuboid) { + cuboids.write->push(std::move(cuboid)); + } + + /** + * swap double buffered cuboids + */ + void cuboidSwap() { + cuboids.read->clear(); + cuboids.swap(); + } + /** * set camera target. the camera will smoothly move towards it */ diff --git a/ouster_viz/src/image.cpp b/ouster_viz/src/image.cpp index 91aa051e..22ed3eab 100644 --- a/ouster_viz/src/image.cpp +++ b/ouster_viz/src/image.cpp @@ -6,6 +6,9 @@ namespace viz { namespace impl { void Image::draw(Camera& cam) { + // calling this before image has been resized causes flickering + if (data.size() <= 0) return; + glUseProgram(image_program_id); const GLuint vertex_id = glGetAttribLocation(image_program_id, "vertex"); const GLuint uv_id = glGetAttribLocation(image_program_id, "vertex_uv"); diff --git a/ouster_viz/src/main.cpp b/ouster_viz/src/main.cpp index 632ab1da..0a042377 100644 --- a/ouster_viz/src/main.cpp +++ b/ouster_viz/src/main.cpp @@ -1,15 +1,5 @@ #include -#if defined _WIN32 -#pragma warning(push, 2) -#endif - -#include - -#if defined _WIN32 -#pragma warning(pop) -#endif - #include #include #include @@ -18,36 +8,20 @@ #include #include #include -#include #include #include #include +#include "ouster/build.h" #include "ouster/client.h" -#include "ouster/compat.h" #include "ouster/lidar_scan.h" #include "ouster/lidar_scan_viz.h" -#include "ouster/packet.h" #include "ouster/point_viz.h" #include "ouster/types.h" namespace sensor = ouster::sensor; namespace viz = ouster::viz; -/** - * Print usage - */ -void print_help() { - std::cout - << "Usage: viz [options] [hostname] [udp_destination]\n" - << "Options:\n" - << " -m <512x10 | 512x20 | 1024x10 | 1024x20 | 2048x10> : lidar mode\n" - << " -l : use specified port for lidar data\n" - << " -i : use specified port for imu data \n" - << " -f : use provided metadata file; do not configure via TCP" - << std::endl; -} - int main(int argc, char** argv) { sensor::lidar_mode mode = sensor::MODE_UNSPEC; bool do_config = true; // send tcp commands to configure sensor @@ -59,7 +33,8 @@ int main(int argc, char** argv) { std::string udp_dest; try { - TCLAP::CmdLine cmd("Ouster Visualizer"); + TCLAP::CmdLine cmd("Ouster Visualizer", ' ', + ouster::CLIENT_VERSION_FULL); TCLAP::UnlabeledValueArg hostname_arg( "hostname", "hostname of the sensor", true, "", "string"); @@ -116,13 +91,12 @@ int main(int argc, char** argv) { hostname = hostname_arg.getValue(); udp_dest = udp_destination_arg.getValue(); } catch (const std::exception& ex) { - std::cout << "Invalid Argument Format: " << ex.what() << std::endl; - print_help(); + std::cout << "Unexpected error: " << ex.what() << std::endl; std::exit(EXIT_FAILURE); } std::shared_ptr cli; - socket_init(); + if (do_config) { std::cout << "Connecting to " << hostname << "; sending data to " << udp_dest << std::endl; @@ -139,7 +113,6 @@ int main(int argc, char** argv) { if (!cli) { std::cerr << "Failed to initialize client" << std::endl; - print_help(); std::exit(EXIT_FAILURE); } @@ -183,13 +156,7 @@ int main(int argc, char** argv) { std::unique_ptr ls_read(new ouster::LidarScan(W, H)); std::unique_ptr ls_write(new ouster::LidarScan(W, H)); - auto batch_and_display = sensor::batch_to_scan( - W, packet_format, [&](std::chrono::nanoseconds) mutable { - std::lock_guard lk(swap_mtx); - std::swap(ls_read, ls_write); - lidar_scan_ready = true; - cv.notify_one(); - }); + auto batch = ouster::ScanBatcher(W, packet_format); std::unique_ptr lidar_buf( new uint8_t[packet_format.lidar_packet_size + 1]); @@ -206,8 +173,14 @@ int main(int argc, char** argv) { } if (st & sensor::client_state::LIDAR_DATA) { if (sensor::read_lidar_packet(*cli, lidar_buf.get(), - packet_format)) - batch_and_display(lidar_buf.get(), *ls_write); + packet_format)) { + if (batch(lidar_buf.get(), *ls_write)) { + std::lock_guard lk(swap_mtx); + std::swap(ls_read, ls_write); + lidar_scan_ready = true; + cv.notify_one(); + } + } } if (st & sensor::client_state::IMU_DATA) { sensor::read_imu_packet(*cli, imu_buf.get(), packet_format); @@ -230,10 +203,11 @@ int main(int argc, char** argv) { lidar_scan_viz.draw(*ls_read); } }); + point_viz.drawLoop(); cv.notify_one(); // wake up update_draw thread for exit poll.join(); update_draw.join(); - socket_quit(); + return EXIT_SUCCESS; } diff --git a/ouster_viz/src/point_viz.cpp b/ouster_viz/src/point_viz.cpp index 3c5ba4c6..dabc4215 100644 --- a/ouster_viz/src/point_viz.cpp +++ b/ouster_viz/src/point_viz.cpp @@ -148,6 +148,8 @@ bool PointViz::initialize() { rings.initialize(); image.read->initialize(); image.write->initialize(); + cuboids.read->initialize(); + cuboids.write->initialize(); return true; } @@ -178,6 +180,22 @@ void PointViz::drawLoop() { rings.draw(camera); } + glEnable(GL_BLEND); + glBlendFunc(GL_SRC_ALPHA, GL_DST_ALPHA); + glBlendEquation(GL_FUNC_ADD); + + // enable culling + glEnable(GL_CULL_FACE); + glCullFace(GL_FRONT); + + // draw cuboids + if (cuboids.enabled) { + cuboids.draw(camera); + } + glDisable(GL_BLEND); + // enable culling + glDisable(GL_CULL_FACE); + // Swap buffers glfwSwapBuffers(window); glfwPollEvents();