OS1 example client and ROS node

.gitignore

@@ -0,0 +1,6 @@
+/CMakeLists.txt
+.catkin_tools/
+build/
+devel/
+logs/
+install/

LICENSE

@@ -0,0 +1,29 @@
+BSD 3-Clause License
+
+Copyright (c) 2018, ouster-lidar
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

ouster_client/.gitignore

@@ -0,0 +1 @@
+build/

ouster_client/CMakeLists.txt

@@ -0,0 +1,23 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(ouster_client)
+
+add_compile_options(-std=c++11 -Wall)
+
+find_package(catkin QUIET)
+if (catkin_FOUND)
+  message(STATUS "Catkin found, compiling with ROS")
+
+  catkin_package(
+    INCLUDE_DIRS include
+    LIBRARIES ouster_client
+    CATKIN_DEPENDS
+  )
+else()
+  message(STATUS "Catkin not found, compiling without ROS")
+endif()
+
+
+include_directories(include)
+add_library(ouster_client STATIC src/os1.cpp)
+add_executable(ouster_client_example src/main.cpp)
+target_link_libraries(ouster_client_example ouster_client)

ouster_client/include/ouster/os1.h

@@ -0,0 +1,62 @@
+/**
+ * OS1 sample client
+ */
+
+#pragma once
+
+#include <cstdint>
+#include <memory>
+#include <string>
+
+namespace ouster {
+namespace OS1 {
+
+const size_t lidar_packet_bytes = 12608;
+const size_t imu_packet_bytes = 48;
+
+struct client;
+
+enum client_state { ERROR = 1, LIDAR_DATA = 2, IMU_DATA = 4 };
+
+/**
+ * Connect to 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
+ * @param imu_port port on which the sensor will send imu data
+ * @return pointer owning the resources associated with the connection
+ */
+std::shared_ptr<client> init_client(const std::string& hostname,
+                                    const std::string& udp_dest_host,
+                                    int lidar_port, int imu_port);
+
+/**
+ * Block for up to a second until either data is ready or an error occurs.
+ * @param cli client returned by init_client associated with the connection
+ * @return client_state s where (s & ERROR) is true if an error occured, (s &
+ * LIDAR_DATA) is true if lidar data is ready to read, and (s & IMU_DATA) is
+ * true if imu data is ready to read
+ */
+client_state poll_client(const client& cli);
+
+/**
+ * Read lidar data from the sensor. Will block for up to a second if no data is
+ * available.
+ * @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
+ * @returns true if a lidar packet was successfully read
+ */
+bool read_lidar_packet(const client& cli, uint8_t* buf);
+
+/**
+ * Read imu data from the sensor. Will block for up to a second if no data is
+ * available.
+ * @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
+ * @returns true if an imu packet was successfully read
+ */
+bool read_imu_packet(const client& cli, uint8_t* buf);
+}
+}

ouster_client/include/ouster/os1_packet.h

@@ -0,0 +1,166 @@
+/**
+ * Utilities to parse lidar and imu packets
+ */
+
+#pragma once
+
+#include <cmath>
+#include <cstdint>
+#include <cstring>
+
+namespace ouster {
+namespace OS1 {
+
+namespace {
+
+const int pixels_per_column = 64;
+const int columns_per_buffer = 16;
+
+const int pixel_bytes = 12;
+const int column_bytes = 16 + (pixels_per_column * pixel_bytes) + 4;
+
+const int encoder_ticks_per_rev = 90112;
+
+const float v_angle[pixels_per_column] = {
+    16.611,  16.084,  15.557,  15.029,  14.502,  13.975,  13.447,  12.920,
+    12.393,  11.865,  11.338,  10.811,  10.283,  9.756,   9.229,   8.701,
+    8.174,   7.646,   7.119,   6.592,   6.064,   5.537,   5.010,   4.482,
+    3.955,   3.428,   2.900,   2.373,   1.846,   1.318,   0.791,   0.264,
+    -0.264,  -0.791,  -1.318,  -1.846,  -2.373,  -2.900,  -3.428,  -3.955,
+    -4.482,  -5.010,  -5.537,  -6.064,  -6.592,  -7.119,  -7.646,  -8.174,
+    -8.701,  -9.229,  -9.756,  -10.283, -10.811, -11.338, -11.865, -12.393,
+    -12.920, -13.447, -13.975, -14.502, -15.029, -15.557, -16.084, -16.611,
+};
+
+const float h_offs[pixels_per_column] = {
+    3.164, 1.055, -1.055, -3.164, 3.164, 1.055, -1.055, -3.164,
+    3.164, 1.055, -1.055, -3.164, 3.164, 1.055, -1.055, -3.164,
+    3.164, 1.055, -1.055, -3.164, 3.164, 1.055, -1.055, -3.164,
+    3.164, 1.055, -1.055, -3.164, 3.164, 1.055, -1.055, -3.164,
+    3.164, 1.055, -1.055, -3.164, 3.164, 1.055, -1.055, -3.164,
+    3.164, 1.055, -1.055, -3.164, 3.164, 1.055, -1.055, -3.164,
+    3.164, 1.055, -1.055, -3.164, 3.164, 1.055, -1.055, -3.164,
+    3.164, 1.055, -1.055, -3.164, 3.164, 1.055, -1.055, -3.164,
+};
+
+struct trig_table_entry {
+    float sin_v_angle;
+    float cos_v_angle;
+    float h_offs;
+};
+
+// table of vertical angle cos, sin, and horizontal offset of each pixel
+static trig_table_entry trig_table[pixels_per_column];
+
+}
+
+static bool init_tables() {
+    for (int i = 0; i < pixels_per_column; i++) {
+        trig_table[i] = {sinf(v_angle[i] * 2 * M_PI / 360.0f),
+                         cosf(v_angle[i] * 2 * M_PI / 360.0f),
+                         h_offs[i] * 2 * (float)M_PI / 360.0f};
+    }
+    return true;
+}
+
+static bool tables_initialized = init_tables();
+
+// lidar column fields
+inline const uint8_t* nth_col(int n, const uint8_t* udp_buf) {
+    return udp_buf + (n * column_bytes);
+}
+
+inline uint64_t col_timestamp(const uint8_t* col_buf) {
+    uint64_t res;
+    memcpy(&res, col_buf, sizeof(uint64_t));
+    return res;
+}
+
+inline float col_h_angle(const uint8_t* col_buf) {
+    uint32_t ticks;
+    memcpy(&ticks, col_buf + 12, sizeof(uint32_t));
+    return (2.0 * M_PI * ticks / (float)encoder_ticks_per_rev);
+}
+
+// lidar pixel fields
+inline const uint8_t* nth_px(int n, const uint8_t* col_buf) {
+    return col_buf + 16 + (n * pixel_bytes);
+}
+
+inline uint32_t px_range(const uint8_t* px_buf) {
+    uint32_t res;
+    memcpy(&res, px_buf, sizeof(uint32_t));
+    res &= 0x000fffff;
+    return res;
+}
+
+inline uint16_t px_reflectivity(const uint8_t* px_buf) {
+    uint16_t res;
+    memcpy(&res, px_buf + 4, sizeof(uint16_t));
+    return res;
+}
+
+inline uint16_t px_signal_photons(const uint8_t* px_buf) {
+    uint16_t res;
+    memcpy(&res, px_buf + 6, sizeof(uint16_t));
+    return res;
+}
+
+// imu packets
+inline uint64_t imu_sys_ts(const uint8_t* imu_buf) {
+    uint64_t res;
+    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));
+    return res;
+}
+
+inline uint64_t imu_gyro_ts(const uint8_t* imu_buf) {
+    uint64_t res;
+    memcpy(&res, imu_buf + 16, sizeof(uint64_t));
+    return res;
+}
+
+// imu linear acceleration
+inline float imu_la_x(const uint8_t* imu_buf) {
+    float res;
+    memcpy(&res, imu_buf + 24, sizeof(float));
+    return res;
+}
+
+inline float imu_la_y(const uint8_t* imu_buf) {
+    float res;
+    memcpy(&res, imu_buf + 28, sizeof(float));
+    return res;
+}
+
+inline float imu_la_z(const uint8_t* imu_buf) {
+    float res;
+    memcpy(&res, imu_buf + 32, sizeof(float));
+    return res;
+}
+
+// imu angular velocity
+inline float imu_av_x(const uint8_t* imu_buf) {
+    float res;
+    memcpy(&res, imu_buf + 36, sizeof(float));
+    return res;
+}
+
+inline float imu_av_y(const uint8_t* imu_buf) {
+    float res;
+    memcpy(&res, imu_buf + 40, sizeof(float));
+    return res;
+}
+
+inline float imu_av_z(const uint8_t* imu_buf) {
+    float res;
+    memcpy(&res, imu_buf + 44, sizeof(float));
+    return res;
+}
+}
+}

ouster_client/package.xml

@@ -0,0 +1,11 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>ouster_client</name>
+  <version>0.1.0</version>
+  <description>The os1_client package</description>
+  <maintainer email="[email protected]">ouster developers</maintainer>
+  <license>BSD</license>
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <export></export>
+</package>

ouster_client/src/main.cpp

@@ -0,0 +1,89 @@
+#include <cstdint>
+#include <cstring>
+#include <iomanip>
+#include <iostream>
+
+#include "ouster/os1.h"
+#include "ouster/os1_packet.h"
+
+namespace OS1 = ouster::OS1;
+
+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) {
+    n_lidar_packets++;
+    lidar_col_0_ts = OS1::col_timestamp(OS1::nth_col(0, buf));
+    lidar_col_0_h_angle = OS1::col_h_angle(OS1::nth_col(0, buf));
+}
+
+void handle_imu(uint8_t* buf) {
+    n_imu_packets++;
+    imu_ts = OS1::imu_sys_ts(buf);
+    imu_av_z = OS1::imu_av_z(buf);
+    imu_la_y = OS1::imu_la_y(buf);
+}
+
+void print_headers() {
+    std::cout << std::setw(15) << "n_lidar_packets" 
+              << std::setw(15) << "col_0_azimuth"
+              << std::setw(15) << "col_0_ts" 
+              << std::setw(15) << "n_imu_packets"
+              << std::setw(15) << "im_av_z"
+              << std::setw(15) << "im_la_y"
+              << std::setw(15) << "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(15) << lidar_col_0_ts
+              << std::setw(15) << n_imu_packets
+              << std::setw(15) << imu_av_z
+              << std::setw(15) << imu_la_y
+              << std::setw(15) << imu_ts;
+    std::flush(std::cout);
+}
+
+int main(int argc, char** argv) {
+    if (argc != 3) {
+        std::cerr << "Usage: ouster_client_example <os1_hostname> <data_destination_ip>" << std::endl;
+        return 1;
+    }
+
+    auto cli = OS1::init_client(argv[1], argv[2], 7502, 7503);
+    if (!cli) {
+        std::cerr << "Failed to connect to client at: " << argv[1] << std::endl;
+        return 1;
+    }
+
+    uint8_t lidar_buf[OS1::lidar_packet_bytes + 1];
+    uint8_t imu_buf[OS1::imu_packet_bytes + 1];
+
+    print_headers();
+
+    while (true) {
+        OS1::client_state st = OS1::poll_client(*cli);
+        if (st & OS1::ERROR) {
+            return 1;
+        } else if (st & OS1::LIDAR_DATA) {
+            if (OS1::read_lidar_packet(*cli, lidar_buf))
+                handle_lidar(lidar_buf);
+        } else if (st & OS1::IMU_DATA) {
+            if (OS1::read_imu_packet(*cli, imu_buf)) handle_imu(imu_buf);
+        }
+
+        if (n_imu_packets % 50 == 0) print_stats();
+    }
+
+    return 0;
+}