Improve scan batching robustness, assorted bugs

* Use frame id for batching to handle packet reordering
* USe integer timestamps in PointOS1
* Avoid corrupting reflectivity in os1_cloud_node output
* Add missing timestamps on image topics
* Fix missing dependencies

CHANGELOG.md

@@ -1,5 +1,19 @@
 # Changelog
 
+## [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
@@ -27,6 +41,8 @@
 - 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

README.md

@@ -11,4 +11,4 @@ See the `README.md` in each subdirectory for details.
 
 ## Sample Data
 * Sample sensor output usable with the provided ROS code is available
-  [here](https://data.ouster.io/sample-data-2018-08-29)
+  [here](https://data.ouster.io/sample-data-1.10)

ouster_client/CMakeLists.txt

@@ -23,7 +23,9 @@ else()
   message(STATUS "Catkin env not set, compiling ${PROJECT_NAME} without ROS")
 endif()
 
-add_library(ouster_client STATIC src/os1.cpp src/os1_util.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})

ouster_client/include/ouster/os1.h

@@ -45,7 +45,7 @@ const version invalid_version = {0, 0, 0};
 /**
  * Minimum supported version
  */
-const OS1::version min_version = {1, 8, 0};
+const OS1::version min_version = {1, 9, 0};
 
 inline bool operator==(const version& u, const version& v) {
     return u.major == v.major && u.minor == v.minor && u.patch == v.patch;
@@ -63,7 +63,7 @@ inline bool operator<=(const version& u, const version& v) {
 struct sensor_info {
     std::string hostname;
     std::string sn;
-    version fw_rev;
+    std::string fw_rev;
     lidar_mode mode;
     std::vector<double> beam_azimuth_angles;
     std::vector<double> beam_altitude_angles;

ouster_client/include/ouster/os1_util.h

@@ -58,73 +58,83 @@ std::vector<double> make_xyz_lut(
 std::vector<int> get_px_offset(int W);
 
 /**
- * Make a function that batches a single scan (revolution) of data to
- * an output iterator. The callback f() is invoked with the timestamp
- * of the first column in the scan before adding data from a new
- * scan. Timestamps for each column are ns relative to the scan
- * timestamp. XYZ coordinates in meters are computed using the
- * provided lookup.
+ * Make a function that batches a single scan (revolution) of data to a
+ * random-access iterator. The callback f() is invoked with the timestamp of the
+ * first column in the scan before adding data from a new scan. Timestamps for
+ * each column are ns relative to the scan timestamp. XYZ coordinates in meters
+ * are computed using the provided lookup table.
+ *
  * The value type is assumed to be constructed from 9 values: x, y, z,
  * (padding), intensity, ts, reflectivity, noise, range (in mm) and
  * default-constructible. It should be compatible with PointOS1 in the
  * ouster_ros package.
- * @param xyz_lut: a lookup table generated from make_xyz_lut, above
+ *
+ * @param xyz_lut a lookup table generated from make_xyz_lut, above
  * @param W number of columns in the lidar scan. One of 512, 1024, or 2048.
  * @param H number of rows in the lidar scan. 64 for the OS1 family of sensors.
+ * @param empty value to insert for mossing data
+ * @param c function to construct a value from x, y, z (m), i, ts, reflectivity,
+ * ring, noise, range (mm). Needed to use with Eigen datatypes.
  * @param f callback invoked when batching a scan is done.
- * @return a function taking a lidar packet buffer and output iterator to which
- * data is added for every point in the scan.
+ * @return a function taking a lidar packet buffer and random-access iterator to
+ * which data is added for every point in the scan.
  */
-template <typename iterator_type, typename value_type, typename F>
-std::function<void(const uint8_t*, iterator_type& it)> batch_to_iter(
-    const std::vector<double>& xyz_lut, int W, int H, F&& f) {
+template <typename iterator_type, typename F, typename C>
+std::function<void(const uint8_t*, iterator_type it)> batch_to_iter(
+    const std::vector<double>& xyz_lut, int W, int H,
+    const typename iterator_type::value_type& empty, C&& c, F&& f) {
     int next_m_id{W};
-    int64_t scan_ts{-1L};
+    int32_t cur_f_id{-1};
 
-    return [=](const uint8_t* packet_buf, iterator_type& it) mutable {
+    int64_t scan_ts{-1L};
 
-        using T = value_type;
+    return [=](const uint8_t* packet_buf, iterator_type it) mutable {
 
         for (int icol = 0; icol < OS1::columns_per_buffer; icol++) {
             const uint8_t* col_buf = OS1::nth_col(icol, packet_buf);
             const uint16_t m_id = OS1::col_measurement_id(col_buf);
+            const uint16_t f_id = OS1::col_frame_id(col_buf);
             const uint64_t ts = OS1::col_timestamp(col_buf);
+            const bool valid = OS1::col_valid(col_buf) == 0xffffffff;
 
             // drop invalid / out-of-bounds data in case of misconfiguration
-            if (OS1::col_valid(col_buf) != 0xffffffff || m_id >= W) continue;
+            if (!valid || m_id >= W || f_id + 1 == cur_f_id) continue;
 
-            if (m_id < next_m_id) {
+            if (f_id != cur_f_id) {
                 // if not initializing with first packet
                 if (scan_ts != -1) {
                     // zero out remaining missing columns
-                    it = std::fill_n(it, H * (W - next_m_id), T{});
+                    std::fill(it + (H * next_m_id), it + (H * W), empty);
                     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) {
+                std::fill(it + (H * next_m_id), it + (H * m_id), empty);
+                next_m_id = m_id + 1;
             }
 
-            // fill zero out missing columns
-            it = std::fill_n(it, H * (m_id - next_m_id), T{});
-            next_m_id = m_id + 1;
+            // index of the first point in current packet
+            const int idx = H * m_id;
 
             for (uint8_t ipx = 0; ipx < H; ipx++) {
                 const uint8_t* px_buf = OS1::nth_px(ipx, col_buf);
                 uint32_t r = OS1::px_range(px_buf);
-                int ind = 3 * (m_id * H + ipx);
-
-                // x, y, z, (padding), i, ts, reflectivity, noise, range (mm)
-                *it++ = T{r * 0.001f * static_cast<float>(xyz_lut[ind + 0]),
-                          r * 0.001f * static_cast<float>(xyz_lut[ind + 1]),
-                          r * 0.001f * static_cast<float>(xyz_lut[ind + 2]),
-                          0.0f,
-                          static_cast<float>(OS1::px_signal_photons(px_buf)),
-                          static_cast<float>(ts - scan_ts),
-                          OS1::px_reflectivity(px_buf),
-                          ipx,
-                          OS1::px_noise_photons(px_buf),
-                          static_cast<uint32_t>(r)};
+                int ind = 3 * (idx + ipx);
+
+                // x, y, z(m), i, ts, reflectivity, ring, noise, range (mm)
+                it[idx + ipx] = c(r * 0.001f * xyz_lut[ind + 0],
+                                  r * 0.001f * xyz_lut[ind + 1],
+                                  r * 0.001f * xyz_lut[ind + 2],
+                                  OS1::px_signal_photons(px_buf), ts - scan_ts,
+                                  OS1::px_reflectivity(px_buf), ipx,
+                                  OS1::px_noise_photons(px_buf), r);
             }
         }
     };

ouster_client/src/os1.cpp

@@ -13,6 +13,7 @@
 #include <vector>
 
 #include <arpa/inet.h>
+#include <fcntl.h>
 #include <netdb.h>
 #include <sys/socket.h>
 #include <sys/types.h>
@@ -46,29 +47,50 @@ const std::array<std::pair<lidar_mode, std::string>, 5> lidar_mode_strings = {
      {MODE_2048x10, "2048x10"}}};
 
 int udp_data_socket(int port) {
-    int sock_fd = socket(PF_INET, SOCK_DGRAM, 0);
-    if (sock_fd < 0) {
-        std::cerr << "socket: " << std::strerror(errno) << std::endl;
+    struct addrinfo hints, *info_start, *ai;
+
+    memset(&hints, 0, sizeof hints);
+    hints.ai_family = AF_INET6;
+    hints.ai_socktype = SOCK_DGRAM;
+    hints.ai_flags = AI_PASSIVE;
+
+    auto port_s = std::to_string(port);
+
+    int ret = getaddrinfo(NULL, port_s.c_str(), &hints, &info_start);
+    if (ret != 0) {
+        std::cerr << "getaddrinfo(): " << gai_strerror(ret) << std::endl;
+        return -1;
+    }
+    if (info_start == NULL) {
+        std::cerr << "getaddrinfo: empty result" << std::endl;
         return -1;
     }
 
-    struct sockaddr_in my_addr;
-    memset((char*)&my_addr, 0, sizeof(my_addr));
-    my_addr.sin_family = AF_INET;
-    my_addr.sin_port = htons(port);
-    my_addr.sin_addr.s_addr = INADDR_ANY;
+    int 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 (sock_fd < 0) {
+            std::cerr << "udp socket(): " << std::strerror(errno) << std::endl;
+            continue;
+        }
+
+        if (bind(sock_fd, ai->ai_addr, ai->ai_addrlen) < 0) {
+            close(sock_fd);
+            std::cerr << "udp bind(): " << std::strerror(errno) << std::endl;
+            continue;
+        }
+
+        break;
+    }
 
-    if (bind(sock_fd, (struct sockaddr*)&my_addr, sizeof(my_addr)) < 0) {
-        std::cerr << "bind: " << std::strerror(errno) << std::endl;
+    freeaddrinfo(info_start);
+    if (ai == NULL) {
+        close(sock_fd);
         return -1;
     }
 
-    struct timeval timeout;
-    timeout.tv_sec = 1;
-    timeout.tv_usec = 0;
-    if (setsockopt(sock_fd, SOL_SOCKET, SO_RCVTIMEO, &timeout,
-                   sizeof(timeout)) < 0) {
-        std::cerr << "setsockopt: " << std::strerror(errno) << std::endl;
+    if (fcntl(sock_fd, F_SETFL, fcntl(sock_fd, F_GETFL, 0) | O_NONBLOCK) < 0) {
+        std::cerr << "udp fcntl(): " << std::strerror(errno) << std::endl;
         return -1;
     }
 
@@ -233,7 +255,7 @@ sensor_info parse_metadata(const std::string& meta) {
                         {},        {},        {}, {}};
     info.hostname = root["hostname"].asString();
     info.sn = root["prod_sn"].asString();
-    info.fw_rev = version_of_string(root["build_rev"].asString());
+    info.fw_rev = root["build_rev"].asString();
 
     info.mode = lidar_mode_of_string(root["lidar_mode"].asString());
 

ouster_client/src/os1_util.cpp

@@ -45,7 +45,7 @@ std::vector<double> make_xyz_lut(int W, int H,
         for (int ipx = 0; ipx < H; ipx++) {
             int ind = 3 * (icol * H + ipx);
             double h_angle =
-                std::sin(azimuth_angles.at(ipx) * 2 * M_PI / 360.0) + h_angle_0;
+                (azimuth_angles.at(ipx) * 2 * M_PI / 360.0) + h_angle_0;
 
             xyz[ind + 0] = std::cos(altitude_angles[ipx] * 2 * M_PI / 360.0) *
                            std::cos(h_angle);

ouster_ros/CMakeLists.txt

@@ -6,6 +6,9 @@ set(CMAKE_CXX_FLAGS         "-Wall -Wextra")
 set(CMAKE_CXX_FLAGS_DEBUG   "-O0 -g")
 set(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG")
 
+find_package(PkgConfig REQUIRED)
+pkg_check_modules(jsoncpp REQUIRED jsoncpp)
+
 find_package(catkin REQUIRED COMPONENTS
   message_generation
   std_msgs
@@ -45,12 +48,16 @@ add_dependencies(ouster_ros ${PROJECT_NAME}_gencpp)
 
 add_executable(os1_node src/os1_node.cpp)
 target_link_libraries(os1_node ouster_ros ${catkin_LIBRARIES})
+add_dependencies(os1_node ${PROJECT_NAME}_gencpp)
 
 add_executable(os1_cloud_node src/os1_cloud_node.cpp)
 target_link_libraries(os1_cloud_node ouster_ros ${catkin_LIBRARIES})
+add_dependencies(os1_cloud_node ${PROJECT_NAME}_gencpp)
 
 add_executable(viz_node src/viz_node.cpp)
 target_link_libraries(viz_node ${catkin_LIBRARIES})
+add_dependencies(viz_node ${PROJECT_NAME}_gencpp)
 
 add_executable(img_node src/img_node.cpp)
 target_link_libraries(img_node ouster_ros ${catkin_LIBRARIES})
+add_dependencies(img_node ${PROJECT_NAME}_gencpp)

ouster_ros/README.md

@@ -9,8 +9,8 @@
 * 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`, `ros-kinetic-tf2-ros` and,
-  optionally, `ros-kinetic-rviz` for visualization using ROS
+* Additionally requires `ros-kinetic-pcl-ros`, `ros-kinetic-tf2-geometry-msgs`
+  and, optionally, `ros-kinetic-rviz` for visualization using ROS
 * Be sure to source the ROS setup script before building. For example:`source
   /opt/ros/kinetic/setup.bash`
 * Build with `mkdir -p myworkspace/src && cd myworkspace && ln -s
@@ -30,7 +30,7 @@
      /os1_node/lidar_packets` in another terminal
 * To publish ROS topics from recorded data:
     - Run `roslaunch ouster_ros os1.launch replay:=true
-     os1_hostname:=<os1_hostname>`
+      os1_hostname:=<os1_hostname>`
     - In a second terminal run `rosbag play --clock <bagfile>`
     - Note: `os1_node` reads and writes metadata to `${ROS_HOME}` to enable
       accurately replaying raw data. By default, the name of this file is based
@@ -45,7 +45,7 @@
       visualizer, add `viz:=true` to either of the `roslaunch` commands above
     - A window should open and start displaying data after a few seconds. This
       should work with a running sensor or replayed data
-    - See the [README.md](ouster_viz/README.md) in the `ouster_viz` directory
+    - See the [README.md](../ouster_viz/README.md) in the `ouster_viz` directory
       for details on keyboard and mouse controls
 * To display sensor output using ROS tools (rviz):
     - Follow the instructions above for running the example ROS code with a

ouster_ros/include/ouster_ros/point_os1.h

@@ -13,12 +13,18 @@ namespace OS1 {
 struct EIGEN_ALIGN16 PointOS1 {
     PCL_ADD_POINT4D;
     float intensity;
-    float t;
+    uint32_t t;
     uint16_t reflectivity;
     uint8_t ring;
     uint16_t noise;
     uint32_t range;
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+    static inline PointOS1 make(float x, float y, float z, float intensity,
+                                uint32_t t, uint16_t reflectivity, uint8_t ring,
+                                uint16_t noise, uint32_t range) {
+        return {x, y, z, 0.0, intensity, t, reflectivity, ring, noise, range};
+    }
 };
 }
 }
@@ -29,9 +35,9 @@ POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_ros::OS1::PointOS1,
     (float, y, y)
     (float, z, z)
     (float, intensity, intensity)
-    (float, t, t)
-    (uint8_t, ring, ring)
+    (uint32_t, t, t)
     (uint16_t, reflectivity, reflectivity)
+    (uint8_t, ring, ring)
     (uint16_t, noise, noise)
     (uint32_t, range, range)
 )