Performance improvements and bug fixes

* Should fix packet dropping in 2048x10 mode
* Minor functionality changes to visualizer
* Client sets lidar_mode
* Client queries calibration values from sensor
* Publish frame transforms and ROS image topics
* See CHANGELOG.md for details

.gitignore

@@ -4,3 +4,4 @@ build/
 devel/
 logs/
 install/
+doc/

CHANGELOG.md

@@ -0,0 +1,41 @@
+# Changelog
+
+## [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)
+
+### 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

README.md

@@ -0,0 +1,14 @@
+# OS-1 Example Code
+Sample code for connecting to and configuring the OS-1, 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 the OS-1 sensor
+* [ouster_viz/](ouster_viz/README.md) contains a visualizer for the OS-1 sensor
+* [ouster_ros/](ouster_ros/README.md) contains example ROS nodes for publishing point cloud messages
+
+## Sample Data
+* Sample sensor output usable with the provided ROS code is available
+  [here](https://data.ouster.io/sample-data-2018-08-29)

ouster_client/CMakeLists.txt

@@ -6,21 +6,27 @@ 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)
-  message(STATUS "Catkin found, compiling with ROS")
+find_package(PkgConfig REQUIRED)
+pkg_check_modules(jsoncpp REQUIRED jsoncpp)
+
+if (DEFINED CATKIN_DEVEL_PREFIX)
+  find_package(catkin REQUIRED)
+  message(STATUS "Catkin env set, compiling ${PROJECT_NAME} with ROS")
 
   catkin_package(
     INCLUDE_DIRS include
     LIBRARIES ouster_client
+    DEPENDS jsoncpp
     CATKIN_DEPENDS
   )
 else()
-  message(STATUS "Catkin not found, compiling without ROS")
+  message(STATUS "Catkin env not set, compiling ${PROJECT_NAME} without ROS")
 endif()
 
-add_library(ouster_client STATIC src/os1.cpp)
+add_library(ouster_client STATIC src/os1.cpp src/os1_util.cpp)
+target_link_libraries(ouster_client jsoncpp)
 target_include_directories(ouster_client PUBLIC include)
+target_include_directories(ouster_client SYSTEM PRIVATE ${jsoncpp_INCLUDE_DIRS})
 
 add_executable(ouster_client_example src/main.cpp)
 target_link_libraries(ouster_client_example ouster_client)

ouster_client/README.md

@@ -0,0 +1,24 @@
+# OS-1 Example Client
+
+## Contents
+* `ouster_client/` contains a simple C++ client for the OS-1 sensor
+* can be built both with and without ROS. See the instructions in
+  `ouster_ros` 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`
+
+## Running the Sample Client
+* The sample client includes a small driver program that just prints
+  some data to the terminal
+* Make sure the OS-1 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 OS-1 sensor,
+  and `<udp_data_dest_ip>` is the IP to which the sensor should send
+  lidar data

ouster_client/include/ouster/os1.h

@@ -1,12 +1,14 @@
 /**
- * OS1 sample client
+ * @file
+ * @brief OS-1 sample client
  */
 
 #pragma once
 
 #include <cstdint>
 #include <memory>
 #include <string>
+#include <vector>
 
 namespace ouster {
 namespace OS1 {
@@ -16,7 +18,93 @@ const size_t imu_packet_bytes = 48;
 
 struct client;
 
-enum client_state { ERROR = 1, LIDAR_DATA = 2, IMU_DATA = 4 };
+enum client_state {
+    TIMEOUT = 0,
+    ERROR = 1,
+    LIDAR_DATA = 2,
+    IMU_DATA = 4,
+    EXIT = 8
+};
+
+enum lidar_mode {
+    MODE_512x10 = 1,
+    MODE_512x20,
+    MODE_1024x10,
+    MODE_1024x20,
+    MODE_2048x10
+};
+
+struct version {
+    int16_t major;
+    int16_t minor;
+    int16_t patch;
+};
+
+const version invalid_version = {0, 0, 0};
+
+/**
+ * Minimum supported version
+ */
+const OS1::version min_version = {1, 8, 0};
+
+inline bool operator==(const version& u, const version& v) {
+    return u.major == v.major && u.minor == v.minor && u.patch == v.patch;
+}
+
+inline bool operator<(const version& u, const version& v) {
+    return (u.major < v.major) || (u.major == v.major && u.minor < v.minor) ||
+           (u.major == v.major && u.minor == v.minor && u.patch < v.patch);
+}
+
+inline bool operator<=(const version& u, const version& v) {
+    return u < v || u == v;
+}
+
+struct sensor_info {
+    std::string hostname;
+    std::string sn;
+    version fw_rev;
+    lidar_mode mode;
+    std::vector<double> beam_azimuth_angles;
+    std::vector<double> beam_altitude_angles;
+    std::vector<double> imu_to_sensor_transform;
+    std::vector<double> lidar_to_sensor_transform;
+};
+
+/**
+ * Get string representation of a version
+ * @param version
+ * @return string representation of the version
+ */
+std::string to_string(version v);
+
+/**
+ * Get lidar mode from string
+ * @param string
+ * @return lidar mode corresponding to the string, or invalid_version on error
+ */
+version version_of_string(const std::string& s);
+
+/**
+ * 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
+ * @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
+ * @param lidar_mode
+ * @return number of columns per rotation for the mode
+ */
+int n_cols_of_lidar_mode(lidar_mode mode);
 
 /**
  * Connect to the sensor and start listening for data
@@ -28,7 +116,8 @@ enum client_state { ERROR = 1, LIDAR_DATA = 2, IMU_DATA = 4 };
  */
 std::shared_ptr<client> init_client(const std::string& hostname,
                                     const std::string& udp_dest_host,
-                                    int lidar_port, int imu_port);
+                                    lidar_mode mode = MODE_1024x10,
+                                    int lidar_port = 7502, int imu_port = 7503);
 
 /**
  * Block for up to a second until either data is ready or an error occurs.
@@ -45,7 +134,7 @@ client_state poll_client(const client& cli);
  * @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
+ * @return true if a lidar packet was successfully read
  */
 bool read_lidar_packet(const client& cli, uint8_t* buf);
 
@@ -55,8 +144,26 @@ bool read_lidar_packet(const client& cli, uint8_t* buf);
  * @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
+ * @return true if an imu packet was successfully read
  */
 bool read_imu_packet(const client& cli, uint8_t* buf);
+
+/**
+ * Get metadata text blob from the sensor
+ * @param cli client returned by init_client associated with the connection
+ * @return a text blob of metadata parseable into a sensor_info struct
+ */
+std::string get_metadata(const client& cli);
+
+/**
+ * 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 above
+ * @return a sensor_info struct populated with a subset of the metadata
+ */
+sensor_info parse_metadata(const std::string& metadata);
 }
 }

ouster_client/include/ouster/os1_packet.h

@@ -1,5 +1,6 @@
 /**
- * Utilities to parse lidar and imu packets
+ * @file
+ * @brief Utilities to parse lidar and imu packets
  */
 
 #pragma once
@@ -12,8 +13,6 @@
 namespace ouster {
 namespace OS1 {
 
-namespace {
-
 const int pixels_per_column = 64;
 const int columns_per_buffer = 16;
 
@@ -22,48 +21,6 @@ const int column_bytes = 16 + (pixels_per_column * pixel_bytes) + 4;
 
 const int encoder_ticks_per_rev = 90112;
 
-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,
-    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 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,
-    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_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(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;
-}
-
-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);
@@ -77,25 +34,32 @@ 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 + (3 * 4), sizeof(uint32_t));
+    memcpy(&ticks, col_buf + 12, 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) {
+inline uint32_t col_h_encoder_count(const uint8_t* col_buf) {
     uint32_t res;
-    std::memcpy(&res, col_buf + (3 * 4), sizeof(uint32_t));
+    std::memcpy(&res, col_buf + 12, 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));
+inline uint16_t col_measurement_id(const uint8_t* col_buf) {
+    uint16_t res;
+    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));
     return res;
 }
 
 inline uint32_t col_valid(const uint8_t* col_buf) {
     uint32_t res;
-    memcpy(&res, col_buf + (196 * 4), sizeof(uint32_t));
+    memcpy(&res, col_buf + (16 + pixels_per_column * pixel_bytes),
+           sizeof(uint32_t));
     return res;
 }