Update for fw 1.12. See changelog for details

* Firmware >= 1.12 will flip the sign of accelerations reported by the IMU * Add install directives for ouster_ros CMakeLists.txt * Fix visualizer issue with pointcloud not displaying with newer Eigen
SUZhaoyu · May 8, 2019 · e890d32 · e890d32
1 parent 4ce8e4f
commit e890d32
Show file tree

Hide file tree

Showing 9 changed files with 137 additions and 73 deletions.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -1,5 +1,17 @@
 # Changelog
 
+## [1.12.0] - 2019-05-02
+### Added
+- install directives for `ouster_ros` build (addresses #50)
+
+### Changed
+- flip the sign on IMU acceleration output to follow usual conventions
+- increase the update rate in the visualizer to ~60hz
+
+### Fixed
+- visualizer issue where the point cloud would occasionally occasionally not be
+  displayed using newer versions of Eigen
+
 ## [1.11.0] - 2019-03-26
 ### Added
 - allow renaming tf ids using the `tf_prefix` parameter

diff --git a/README.md b/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-1.10)
+  [here](https://data.ouster.io/sample-data-1.12)
diff --git a/ouster_client/CMakeLists.txt b/ouster_client/CMakeLists.txt
@@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.1.0)
 project(ouster_client)
 
 set(CMAKE_CXX_STANDARD 11)
-set(CMAKE_CXX_FLAGS         "-Wall -Wextra")
+set(CMAKE_CXX_FLAGS         "${CMAKE_CXX_FLAGS} -Wall -Wextra")
 set(CMAKE_CXX_FLAGS_DEBUG   "-O0 -g")
 set(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG")
 

diff --git a/ouster_client/include/ouster/os1.h b/ouster_client/include/ouster/os1.h
@@ -107,7 +107,15 @@ lidar_mode lidar_mode_of_string(const std::string& s);
 int n_cols_of_lidar_mode(lidar_mode mode);
 
 /**
- * Connect to the sensor and start listening for data
+ * Listen for OS1 data on the specified ports
+ * @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(int lidar_port = 7502, int imu_port = 7503);
+
+/**
+ * Connect to and configure 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
@@ -120,17 +128,17 @@ std::shared_ptr<client> init_client(const std::string& hostname,
                                     int lidar_port = 7502, int imu_port = 7503);
 
 /**
- * Block for up to a second until either data is ready or an error occurs.
+ * Block for up to timeout_sec until either data is ready or an error occurs.
  * @param cli client returned by init_client associated with the connection
+ * @param timeout_sec seconds to block while waiting for data
  * @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);
+client_state poll_client(const client& cli, int timeout_sec = 1);
 
 /**
- * Read lidar data from the sensor. Will block for up to a second if no data is
- * available.
+ * Read lidar data from the sensor. Will not block.
  * @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
@@ -139,8 +147,7 @@ client_state poll_client(const client& cli);
 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.
+ * Read imu data from the sensor. Will not block.
  * @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

diff --git a/ouster_client/src/os1.cpp b/ouster_client/src/os1.cpp
@@ -282,11 +282,21 @@ sensor_info parse_metadata(const std::string& meta) {
     return info;
 }
 
+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;
+    return cli;
+}
+
 std::shared_ptr<client> init_client(const std::string& hostname,
                                     const std::string& udp_dest_host,
                                     lidar_mode mode, int lidar_port,
                                     int imu_port) {
-    auto cli = std::make_shared<client>();
+    auto cli = init_client(lidar_port, imu_port);
 
     int sock_fd = cfg_socket(hostname.c_str());
 
@@ -346,24 +356,22 @@ std::shared_ptr<client> init_client(const std::string& hostname,
     cli->meta["hostname"] = hostname;
     cli->meta["lidar_mode"] = to_string(mode);
 
-    if (!success) return std::shared_ptr<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;
-    return cli;
+    return success ? cli : std::shared_ptr<client>();
 }
 
-client_state poll_client(const client& c) {
+client_state poll_client(const client& c, const int timeout_sec) {
     fd_set rfds;
     FD_ZERO(&rfds);
     FD_SET(c.lidar_fd, &rfds);
     FD_SET(c.imu_fd, &rfds);
 
+    timeval tv;
+    tv.tv_sec = timeout_sec;
+    tv.tv_usec = 0;
+
     int max_fd = std::max(c.lidar_fd, c.imu_fd);
 
-    int retval = select(max_fd + 1, &rfds, NULL, NULL, NULL);
+    int retval = select(max_fd + 1, &rfds, NULL, NULL, &tv);
 
     client_state res = client_state(0);
     if (retval == -1 && errno == EINTR) {
@@ -377,6 +385,7 @@ client_state poll_client(const client& c) {
     }
     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)

diff --git a/ouster_ros/CMakeLists.txt b/ouster_ros/CMakeLists.txt
@@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.1.0)
 project(ouster_ros)
 
 set(CMAKE_CXX_STANDARD 11)
-set(CMAKE_CXX_FLAGS         "-Wall -Wextra")
+set(CMAKE_CXX_FLAGS         "${CMAKE_CXX_FLAGS} -Wall -Wextra")
 set(CMAKE_CXX_FLAGS_DEBUG   "-O0 -g")
 set(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG")
 
@@ -61,3 +61,13 @@ 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)
+
+install(TARGETS os1_node
+                os1_cloud_node
+                viz_node
+                img_node
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+install(FILES os1.launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
diff --git a/ouster_viz/CMakeLists.txt b/ouster_viz/CMakeLists.txt
@@ -2,7 +2,7 @@ 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         "${CMAKE_CXX_FLAGS} -Wall -Wextra")
 set(CMAKE_CXX_FLAGS_DEBUG   "-O0 -g")
 set(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG")
 
@@ -43,10 +43,7 @@ if (DEFINED CATKIN_DEVEL_PREFIX)
 else()
   message(STATUS "Catkin env not set, compiling ${PROJECT_NAME} without ROS")
 
-  if(NOT TARGET ouster_client)
-    add_subdirectory(../ouster_client ouster_client EXCLUDE_FROM_ALL)
-  endif()
-
+  add_subdirectory(../ouster_client ouster_client EXCLUDE_FROM_ALL)
   set(viz_LINK_LIBRARIES ouster_client)
 endif()
 

diff --git a/ouster_viz/src/main.cpp b/ouster_viz/src/main.cpp
@@ -5,9 +5,11 @@
 #include <cstdlib>
 
 #include <Eigen/Eigen>
+#include <fstream>
 #include <iostream>
 #include <iterator>
 #include <memory>
+#include <sstream>
 #include <string>
 #include <thread>
 #include <vector>
@@ -24,21 +26,41 @@ namespace viz = ouster::viz;
  * Print usage
  */
 void print_help() {
-    std::cout << "Usage: viz [options] [hostname] [udp_destination]\n"
-              << "Options:\n"
-              << "  -m <512x10 | 512x20 | 1024x10 | 1024x20 | 2048x10> "
-                 "lidar mode, default: 1024x10"
-              << std::endl;
+    std::cout
+        << "Usage: viz [options] [hostname] [udp_destination]\n"
+        << "Options:\n"
+        << "  -m <512x10 | 512x20 | 1024x10 | 1024x20 | 2048x10> : lidar mode, "
+           "default 1024x10\n"
+        << "  -f <path> : use provided metadata file; do not configure via TCP"
+        << std::endl;
+}
+
+std::string read_metadata(const std::string& meta_file) {
+    std::stringstream buf{};
+    std::ifstream ifs{};
+    ifs.open(meta_file);
+    buf << ifs.rdbuf();
+    ifs.close();
+
+    if (!ifs) {
+        std::cerr << "Failed to read " << meta_file
+                  << "; check that the path is valid" << std::endl;
+        std::exit(EXIT_FAILURE);
+    }
+
+    return buf.str();
 }
 
 int main(int argc, char** argv) {
     int W = 1024;
     int H = OS1::pixels_per_column;
     OS1::lidar_mode mode = OS1::MODE_1024x10;
+    bool do_config = true;  // send tcp commands to configure sensor
+    std::string metadata{};
 
     try {
         int c = 0;
-        while ((c = getopt(argc, argv, "hm:")) != -1) {
+        while ((c = getopt(argc, argv, "hm:f:")) != -1) {
             switch (c) {
                 case 'h':
                     print_help();
@@ -56,6 +78,10 @@ int main(int argc, char** argv) {
                         std::exit(EXIT_FAILURE);
                     }
                     break;
+                case 'f':
+                    do_config = false;
+                    metadata = read_metadata(optarg);
+                    break;
                 case '?':
                     std::cout << "Invalid Argument Format" << std::endl;
                     print_help();
@@ -69,19 +95,24 @@ int main(int argc, char** argv) {
         std::exit(EXIT_FAILURE);
     }
 
-    if (argc != optind + 2) {
+    if (do_config && argc != optind + 2) {
         std::cerr << "Expected 2 arguments after options" << std::endl;
         print_help();
         std::exit(EXIT_FAILURE);
     }
 
-    std::cout << "Sensor: " << argv[optind]
-              << " UDP Destination:" << argv[optind + 1] << std::endl;
     std::shared_ptr<OS1::client> cli;
-    cli = OS1::init_client(argv[optind], argv[optind + 1], mode);
+    if (do_config) {
+        std::cout << "Configuring sensor: " << argv[optind]
+                  << " UDP Destination:" << argv[optind + 1] << std::endl;
+        cli = OS1::init_client(argv[optind], argv[optind + 1], mode);
+    } else {
+        std::cout << "Listening for sensor data" << std::endl;
+        cli = OS1::init_client();
+    }
+
     if (!cli) {
-        std::cerr << "Failed to connect to client at: " << argv[optind]
-                  << std::endl;
+        std::cerr << "Failed to initialize client" << std::endl;
         print_help();
         std::exit(EXIT_FAILURE);
     }
@@ -93,7 +124,9 @@ int main(int argc, char** argv) {
 
     auto vh = viz::init_viz(W, H);
 
-    auto info = OS1::parse_metadata(get_metadata(*cli));
+    if (do_config) metadata = OS1::get_metadata(*cli);
+
+    auto info = OS1::parse_metadata(metadata);
 
     auto xyz_lut = OS1::make_xyz_lut(W, H, info.beam_azimuth_angles,
                                      info.beam_altitude_angles);
@@ -106,8 +139,8 @@ int main(int argc, char** argv) {
 
     // callback that calls update with filled lidar scan
     auto batch_and_update = OS1::batch_to_iter<ouster::LidarScan::iterator>(
-        xyz_lut, W, H, {}, &ouster::LidarScan::make_val,
-        [&](uint64_t) {
+        xyz_lut, W, H, ouster::LidarScan::Point::Zero(),
+        &ouster::LidarScan::make_val, [&](uint64_t) {
             // swap lidar scan and point it to new buffer
             viz::update(*vh, ls);
             it = ls->begin();