Initial commit of the example visualizer

* Uses VTK6 to display point clouds and range/intensity/noise images
* No dependencies other than VTK6, Eigen3, and a C++11 compiler
* Currently only supports linux; tested on multiple distributions and platforms

ouster_client/CMakeLists.txt

@@ -1,7 +1,10 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.1.0)
 project(ouster_client)
 
-add_compile_options(-std=c++11 -Wall)
+set(CMAKE_CXX_STANDARD 11)
+set(CMAKE_CXX_FLAGS         "-Wall -Wextra")
+set(CMAKE_CXX_FLAGS_DEBUG   "-O0 -g")
+set(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG")
 
 find_package(catkin QUIET)
 if (catkin_FOUND)
@@ -16,8 +19,8 @@ else()
   message(STATUS "Catkin not found, compiling without ROS")
 endif()
 
-
-include_directories(include)
 add_library(ouster_client STATIC src/os1.cpp)
+target_include_directories(ouster_client PUBLIC include)
+
 add_executable(ouster_client_example src/main.cpp)
 target_link_libraries(ouster_client_example ouster_client)

ouster_client/include/ouster/os1_packet.h

@@ -4,6 +4,7 @@
 
 #pragma once
 
+#include <array>
 #include <cmath>
 #include <cstdint>
 #include <cstring>
@@ -21,7 +22,7 @@ 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] = {
+const std::array<float, OS1::pixels_per_column> beam_altitude_angles = {
     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,
@@ -31,8 +32,7 @@ const float v_angle[pixels_per_column] = {
     -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] = {
+const std::array<float, OS1::pixels_per_column> beam_azimuth_angles = {
     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,
@@ -44,21 +44,20 @@ const float h_offs[pixels_per_column] = {
 };
 
 struct trig_table_entry {
-    float sin_v_angle;
-    float cos_v_angle;
-    float h_offs;
+    float sin_beam_altitude_angles;
+    float cos_beam_altitude_angles;
+    float beam_azimuth_angles;
 };
 
 // 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};
+        trig_table[i] = {sinf(beam_altitude_angles[i] * 2 * M_PI / 360.0f),
+                         cosf(beam_altitude_angles[i] * 2 * M_PI / 360.0f),
+                         beam_azimuth_angles[i] * 2 * (float)M_PI / 360.0f};
     }
     return true;
 }
@@ -78,10 +77,28 @@ inline uint64_t col_timestamp(const uint8_t* col_buf) {
 
 inline float col_h_angle(const uint8_t* col_buf) {
     uint32_t ticks;
-    memcpy(&ticks, col_buf + 12, sizeof(uint32_t));
+    memcpy(&ticks, col_buf + (3 * 4), sizeof(uint32_t));
     return (2.0 * M_PI * ticks / (float)encoder_ticks_per_rev);
 }
 
+inline float col_h_encoder_count(const uint8_t* col_buf) {
+    uint32_t res;
+    std::memcpy(&res, col_buf + (3 * 4), sizeof(uint32_t));
+    return res;
+}
+
+inline uint32_t col_measurement_id(const uint8_t* col_buf) {
+    uint32_t res;
+    memcpy(&res, col_buf + (2 * 4), sizeof(uint32_t));
+    return res;
+}
+
+inline uint32_t col_valid(const uint8_t* col_buf) {
+    uint32_t res;
+    memcpy(&res, col_buf + (196 * 4), sizeof(uint32_t));
+    return res;
+}
+
 // lidar pixel fields
 inline const uint8_t* nth_px(int n, const uint8_t* col_buf) {
     return col_buf + 16 + (n * pixel_bytes);
@@ -106,6 +123,12 @@ inline uint16_t px_signal_photons(const uint8_t* px_buf) {
     return res;
 }
 
+inline uint16_t px_noise_photons(const uint8_t* px_buf) {
+    uint16_t res;
+    memcpy(&res, px_buf + 8, sizeof(uint16_t));
+    return res;
+}
+
 // imu packets
 inline uint64_t imu_sys_ts(const uint8_t* imu_buf) {
     uint64_t res;

ouster_client/readme.md

@@ -0,0 +1,15 @@
+README for OS1 Example Client
+
+## Contents
+* `ouster_client/` contains a simple C++ client for the OS1 sensor
+
+## 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`
+
+## Running the Sample Client
+* The sample client includes a small driver program that just prints some data to the terminal
+* Make sure the OS1 is connected to the network and has obtained a dhcp lease. See accompanying documentation for more details
+* You should see a binary called `ouster_client_example` in your build directory on success
+* Run `ouster_client_example <os1_hostname> <udp_data_dest_ip>` where `<os1_hostname>` is the hostname or IP address of the OS1 sensor, and `<udp_data_dest_ip>` is the IP to which the sensor should send lidar data
+

ouster_client/src/main.cpp

@@ -32,31 +32,25 @@ void handle_imu(uint8_t* 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;
+    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::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;
+        std::cerr << "Usage: ouster_client_example <os1_hostname> "
+                     "<data_destination_ip>"
+                  << std::endl;
         return 1;
     }
 
@@ -86,4 +80,4 @@ int main(int argc, char** argv) {
     }
 
     return 0;
-}
+}

ouster_ros/readme.md

@@ -0,0 +1,21 @@
+#README for OS1 Example ROS Node
+
+## Building the Sample ROS Node
+* Supports Ubuntu 16.04 with ROS Kinetic (for ouster_ros)
+* ROS installation instructions can be found [here](http://wiki.ros.org/kinetic/Installation/Ubuntu)
+* Additionally requires ros-kinetic-pcl-ros and, optionally, ros-kinetic-rviz for visualization
+* Be sure to source the ROS setup script before building. For example:`source /opt/ros/kinetic/setup.bash`
+* Both packages can be built by catkin by moving them into a catkin workspace
+* Build with `mkdir myworkspace && cd myworkspace && ln -s /path/to/ouster_example ./src && catkin_make`
+
+## Running the Sample ROS Node
+* Set up the ROS environment with `source /path/to/myworkspace/devel/setup.bash` in a new terminal for each command below
+* For use with a running sensor:
+  - To publish OS1 data as ROS topic `roslaunch ouster_ros os1.launch os1_hostname:=<os1_hostname> os1_udp_dest=<udp_data_dest_ip>` where `<os1_hostname>` can be the hostname or IP of the OS1 device and `<udp_data_dest_ip>` is the IP to which the sensor should send data
+  - To record raw sensor output, run `rosbag record /os1_node/imu_packets /os1_node/lidar_packets` in another terminal
+  - To visualize output, run `rviz -d /path/to/ouster_ros/viz.rviz` in another terminal
+* For use with recorded sensor data:
+  - To replay raw sensor output, run `roslaunch ouster_ros os1.launch replay:=true`
+  - In a second terminal, run `rosbag play --clock <bagfile>`
+  - To visualize output, run `rviz -d /path/to/ouster_ros/viz.rviz` in another terminal
+* Sample raw sensor output is available [here](https://data.ouster.io/ouster-os1-100sec.bag)

ouster_ros/src/os1_ros.cpp

@@ -75,19 +75,18 @@ static PointOS1 nth_point(int ind, const uint8_t* col_buf) {
     auto tte = trig_table[ind];
     const uint8_t* px_buf = nth_px(ind, col_buf);
     float r = px_range(px_buf) / 1000.0;
-    float h_angle = tte.h_offs + h_angle_0;
+    float h_angle = tte.beam_azimuth_angles + h_angle_0;
 
     PointOS1 point;
     point.reflectivity = px_reflectivity(px_buf);
     point.intensity = px_signal_photons(px_buf);
-    point.x = -r * tte.cos_v_angle * cosf(h_angle);
-    point.y = r * tte.cos_v_angle * sinf(h_angle);
-    point.z = r * tte.sin_v_angle;
+    point.x = -r * tte.cos_beam_altitude_angles * cosf(h_angle);
+    point.y = r * tte.cos_beam_altitude_angles * sinf(h_angle);
+    point.z = r * tte.sin_beam_altitude_angles;
     point.ring = ind;
 
     return point;
 }
-
 void add_packet_to_cloud(ns scan_start_ts, ns scan_duration,
                          const PacketMsg& pm, CloudOS1& cloud) {
     const uint8_t* buf = pm.buf.data();

ouster_viz/.gitignore

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

ouster_viz/CMakeLists.txt

@@ -0,0 +1,28 @@
+cmake_minimum_required(VERSION 3.1.0)
+project(ouster_viz)
+
+set(CMAKE_CXX_STANDARD 11)
+set(CMAKE_CXX_FLAGS         "-Wall -Wextra")
+set(CMAKE_CXX_FLAGS_DEBUG   "-O0 -g")
+set(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG")
+
+find_package(Threads)
+find_package(Eigen3 REQUIRED)
+find_package(VTK REQUIRED)
+
+add_subdirectory(../ouster_client ouster_client)
+
+include_directories(include)
+
+include_directories(SYSTEM
+  ${EIGEN3_INCLUDE_DIR}
+  ${VTK_INCLUDE_DIRS}
+  )
+
+add_executable(viz src/main.cpp src/viz.cpp)
+
+target_link_libraries(viz
+  ouster_client
+  ${VTK_LIBRARIES}
+  ${CMAKE_THREAD_LIBS_INIT}
+  )