Feature/firmware 1 13 support

CHANGELOG.md

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

README.md

@@ -1,14 +1,14 @@
-# OS-1 Example Code
-Sample code for connecting to and configuring the OS-1, reading and visualizing
+# OS1 Example Code
+Sample code for connecting to and configuring the OS1, 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_client/](ouster_client/README.md) contains an example C++ client for the OS1 sensor
+* [ouster_viz/](ouster_viz/README.md) contains a visualizer for the OS1 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-1.12)
+  [here](https://data.ouster.io/sample-data-1.13)

ouster_client/CMakeLists.txt

@@ -26,6 +26,7 @@ endif()
 add_library(ouster_client STATIC
   src/os1.cpp
   src/os1_util.cpp)
+set_target_properties(ouster_client PROPERTIES POSITION_INDEPENDENT_CODE ON)
 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/README.md

@@ -1,24 +1,24 @@
-# OS-1 Example Client
+# OS1 Example Client
 
 ## Contents
-* `ouster_client/` contains a simple C++ client for the OS-1 sensor
+* `ouster_client/` contains a simple C++ client for the OS1 sensor that
+  prints lidar data to the terminal
 * can be built both with and without ROS. See the instructions in
-  `ouster_ros` for building in a ROS environment
+  [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`
+  && cd build && cmake .. && make` where `/path/to/ouster_example` is where you've cloned the repository 
 
 ## 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
+* Make sure the OS1 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 <os1_hostname> <udp_data_dest_ip>` where
-  `<os1_hostname>` is the hostname or IP address of the OS-1 sensor,
+* 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/include/ouster/os1.h

@@ -35,6 +35,12 @@ enum class lidar_mode {
     MODE_INVALID
 };
 
+enum class timestamp_mode {
+    TIME_FROM_INTERNAL_OSC = 1,
+    TIME_FROM_SYNC_PULSE_IN,
+    TIME_FROM_PTP_1588
+};
+
 struct version {
     int16_t major;
     int16_t minor;
@@ -107,6 +113,20 @@ lidar_mode lidar_mode_of_string(const std::string& s);
  */
 int n_cols_of_lidar_mode(lidar_mode 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
+ * @param string
+ * @return timestamp mode corresponding to the string, or 0 on error
+ */
+timestamp_mode timestamp_mode_of_string(const std::string& s);
+
 /**
  * Listen for OS1 data on the specified ports
  * @param lidar_port port on which the sensor will send lidar data
@@ -126,8 +146,8 @@ std::shared_ptr<client> init_client(int lidar_port = 7502, int imu_port = 7503);
 std::shared_ptr<client> init_client(const std::string& hostname,
                                     const std::string& udp_dest_host,
                                     lidar_mode mode = lidar_mode::MODE_1024x10,
-                                    const uint16_t lidar_port = 7502u,
-                                    const uint16_t imu_port = 7503u);
+                                    timestamp_mode ts_mode = timestamp_mode::TIME_FROM_INTERNAL_OSC,
+                                    int lidar_port = 7502u, int imu_port = 7503u);
 
 /**
  * Block for up to timeout_sec until either data is ready or an error occurs.
@@ -167,8 +187,7 @@ 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).
+ * 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

ouster_client/include/ouster/os1_util.h

@@ -82,14 +82,14 @@ std::vector<int> get_px_offset(int W);
 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) {
+    const typename std::iterator_traits<iterator_type>::value_type& empty,
+    C&& c, F&& f) {
     int next_m_id{W};
     int32_t cur_f_id{-1};
 
     int64_t scan_ts{-1L};
 
     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);

ouster_client/package.xml

@@ -5,9 +5,9 @@
   <description>The os1_client package</description>
   <maintainer email="[email protected]">ouster developers</maintainer>
   <license>BSD</license>
-  <build_depend>jsoncpp</build_depend>
+  <build_depend>libjsoncpp</build_depend>
   <buildtool_depend>catkin</buildtool_depend>
 
-  <exec_depend>jsoncpp</exec_depend>
+  <exec_depend>libjsoncpp</exec_depend>
   <export></export>
 </package>

ouster_client/src/os1.cpp

@@ -46,6 +46,29 @@ const std::array<std::pair<lidar_mode, std::string>, 5> lidar_mode_strings = {
      {lidar_mode::MODE_1024x20, "1024x20"},
      {lidar_mode::MODE_2048x10, "2048x10"}}};
 
+const std::array<std::pair<timestamp_mode, std::string>, 3>
+    timestamp_mode_strings = {
+        {{timestamp_mode::TIME_FROM_INTERNAL_OSC, "TIME_FROM_INTERNAL_OSC"},
+         {timestamp_mode::TIME_FROM_SYNC_PULSE_IN, "TIME_FROM_SYNC_PULSE_IN"},
+         {timestamp_mode::TIME_FROM_PTP_1588, "TIME_FROM_PTP_1588"}}};
+
+int32_t get_sock_port(int sock_fd) {
+    struct sockaddr_storage ss;
+    socklen_t addrlen = sizeof ss;
+
+    if (getsockname(sock_fd, (struct sockaddr*)&ss, &addrlen) < 0) {
+        std::cerr << "udp getsockname(): " << std::strerror(errno) << std::endl;
+        return -1;
+    }
+
+    if (ss.ss_family == AF_INET)
+        return ntohs(((struct sockaddr_in*)&ss)->sin_port);
+    else if (ss.ss_family == AF_INET6)
+        return ntohs(((struct sockaddr_in6*)&ss)->sin6_port);
+    else
+        return -1;
+}
+
 static int udp_data_socket(int port) {
     struct addrinfo hints, *info_start, *ai;
 
@@ -91,6 +114,7 @@ static int udp_data_socket(int port) {
 
     if (fcntl(sock_fd, F_SETFL, fcntl(sock_fd, F_GETFL, 0) | O_NONBLOCK) < 0) {
         std::cerr << "udp fcntl(): " << std::strerror(errno) << std::endl;
+        close(sock_fd);
         return -1;
     }
 
@@ -173,7 +197,7 @@ bool do_tcp_cmd(int sock_fd, const std::vector<std::string>& cmd_tokens,
 void update_json_obj(Json::Value& dst, const Json::Value& src) {
     for (const auto& key : src.getMemberNames()) dst[key] = src[key];
 }
-}
+}  // namespace
 
 std::string to_string(version v) {
     if (v == invalid_version) return "UNKNOWN";
@@ -232,6 +256,28 @@ int n_cols_of_lidar_mode(lidar_mode mode) {
     }
 }
 
+std::string to_string(timestamp_mode mode) {
+    auto end = timestamp_mode_strings.end();
+    auto res =
+        std::find_if(timestamp_mode_strings.begin(), end,
+                     [&](const std::pair<timestamp_mode, std::string>& p) {
+                         return p.first == mode;
+                     });
+
+    return res == end ? "UNKNOWN" : res->second;
+}
+
+timestamp_mode timestamp_mode_of_string(const std::string& s) {
+    auto end = timestamp_mode_strings.end();
+    auto res =
+        std::find_if(timestamp_mode_strings.begin(), end,
+                     [&](const std::pair<timestamp_mode, std::string>& p) {
+                         return p.second == s;
+                     });
+
+    return res == end ? timestamp_mode(0) : res->first;
+}
+
 std::string get_metadata(const client& cli) {
     Json::StreamWriterBuilder builder;
     builder["enableYAMLCompatibility"] = true;
@@ -285,18 +331,26 @@ sensor_info parse_metadata(const std::string& meta) {
 std::shared_ptr<client> init_client(int lidar_port, int imu_port) {
     auto cli = std::make_shared<client>();
 
-    int lidar_fd = udp_data_socket(lidar_port);
-    int imu_fd = udp_data_socket(imu_port);
-    cli->lidar_fd = lidar_fd;
-    cli->imu_fd = imu_fd;
+    cli->lidar_fd = udp_data_socket(lidar_port);
+    cli->imu_fd = udp_data_socket(imu_port);
+
+    if (cli->lidar_fd < 0 || cli->imu_fd < 0)
+        return std::shared_ptr<client>();
+
     return cli;
 }
 
 std::shared_ptr<client> init_client(const std::string& hostname,
                                     const std::string& udp_dest_host,
-                                    lidar_mode mode, const uint16_t lidar_port,
-                                    const uint16_t imu_port) {
-        auto cli = init_client(lidar_port, imu_port);
+                                    lidar_mode mode, timestamp_mode ts_mode,
+                                    int lidar_port, int imu_port) {
+    auto cli = init_client(lidar_port, imu_port);
+    if (!cli) return std::shared_ptr<client>();
+
+    // update requested ports to actual bound ports
+    lidar_port = get_sock_port(cli->lidar_fd);
+    imu_port = get_sock_port(cli->imu_fd);
+    if (lidar_port == -1 || imu_port == -1) return std::shared_ptr<client>();
 
     int sock_fd = cfg_socket(hostname.c_str());
 
@@ -314,9 +368,10 @@ std::shared_ptr<client> init_client(const std::string& hostname,
         do_tcp_cmd(sock_fd, {"set_config_param", "udp_ip", udp_dest_host}, res);
     success &= res == "set_config_param";
 
-    success &= do_tcp_cmd(sock_fd, {"set_config_param", "udp_port_lidar",
-                                    std::to_string(lidar_port)},
-                          res);
+    success &= do_tcp_cmd(
+        sock_fd,
+        {"set_config_param", "udp_port_lidar", std::to_string(lidar_port)},
+        res);
     success &= res == "set_config_param";
 
     success &= do_tcp_cmd(
@@ -328,6 +383,11 @@ std::shared_ptr<client> init_client(const std::string& hostname,
         sock_fd, {"set_config_param", "lidar_mode", to_string(mode)}, res);
     success &= res == "set_config_param";
 
+    success &= do_tcp_cmd(
+        sock_fd, {"set_config_param", "timestamp_mode", to_string(ts_mode)},
+        res);
+    success &= res == "set_config_param";
+
     success &= do_tcp_cmd(sock_fd, {"get_sensor_info"}, res);
     success &= reader->parse(res.c_str(), res.c_str() + res.size(), &cli->meta,
                              &errors);
@@ -380,15 +440,15 @@ client_state poll_client(const client& c, const int timeout_sec) {
         std::cerr << "select: " << std::strerror(errno) << std::endl;
         res = client_state(res | client_state::ERROR);
     } else if (retval) {
-        if (FD_ISSET(c.lidar_fd, &rfds)) res = client_state(res | client_state::LIDAR_DATA);
-        if (FD_ISSET(c.imu_fd, &rfds)) res = client_state(res | client_state::IMU_DATA);
+        if (FD_ISSET(c.lidar_fd, &rfds)) res = client_state(res | LIDAR_DATA);
+        if (FD_ISSET(c.imu_fd, &rfds)) res = client_state(res | IMU_DATA);
     }
     return res;
 }
 
-static bool recv_fixed(int fd, void* buf, size_t len) {
-    ssize_t n = recvfrom(fd, buf, len + 1, 0, NULL, NULL);
-    if (n == (ssize_t)len)
+static bool recv_fixed(int fd, void* buf, ssize_t len) {
+    ssize_t n = recv(fd, (char*)buf, len + 1, 0);
+    if (n == len)
         return true;
     else if (n == -1)
         std::cerr << "recvfrom: " << std::strerror(errno) << std::endl;
@@ -404,5 +464,6 @@ bool read_lidar_packet(const client& cli, uint8_t* buf) {
 bool read_imu_packet(const client& cli, uint8_t* buf) {
     return recv_fixed(cli.imu_fd, buf, imu_packet_bytes);
 }
-}
-}
+
+}  // namespace OS1
+}  // namespace ouster

ouster_ros/CMakeLists.txt

@@ -48,6 +48,7 @@ catkin_package(
 )
 
 add_library(ouster_ros STATIC src/os1_ros.cpp)
+set_target_properties(ouster_ros PROPERTIES POSITION_INDEPENDENT_CODE ON)
 target_link_libraries(ouster_ros ${catkin_LIBRARIES})
 add_dependencies(ouster_ros ${PROJECT_NAME}_gencpp)