From af0ccbe9f7680af0e79e764c1d8d253dd76ec4c2 Mon Sep 17 00:00:00 2001
From: Dima Garbuzov <dima.garbuzov@ouster.io>
Date: Wed, 28 Jul 2021 09:14:08 -0700
Subject: [PATCH] ouster-ros Bugfix update. See changelog for details

* Add support for automatic UDP destination in simple_viz and ROS (#255)
* Add a read timeout for TCP sockets (#258)
* Fall back to ipv4 when ipv6 is disabled via kernel parameters (#261)
* Fix open3d example crash on macos (#267)
---
 drivers/ouster-ros/CMakeLists.txt             |  6 +++++-
 drivers/ouster-ros/Dockerfile                 |  5 +----
 drivers/ouster-ros/include/ouster_ros/point.h |  2 +-
 drivers/ouster-ros/ouster.launch              |  2 +-
 drivers/ouster-ros/package.xml                |  4 +++-
 drivers/ouster-ros/src/os_node.cpp            | 16 ++++++++++------
 6 files changed, 21 insertions(+), 14 deletions(-)

diff --git a/drivers/ouster-ros/CMakeLists.txt b/drivers/ouster-ros/CMakeLists.txt
index 4e762e4e..5041d5ec 100644
--- a/drivers/ouster-ros/CMakeLists.txt
+++ b/drivers/ouster-ros/CMakeLists.txt
@@ -61,6 +61,9 @@ set(BUILD_SHARED_LIBS ${_SAVE_BUILD_SHARED_LIBS})
 # catkin adds all include dirs to a single variable, don't try to use targets
 include_directories(${_ouster_ros_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS})
 
+# use only MPL-licensed parts of eigen
+add_definitions(-DEIGEN_MPL2_ONLY)
+
 add_library(ouster_ros src/ros.cpp)
 target_link_libraries(ouster_ros PUBLIC ${catkin_LIBRARIES} ouster_build pcl_common PRIVATE
   -Wl,--whole-archive ouster_client -Wl,--no-whole-archive)
@@ -88,4 +91,5 @@ install(
 install(
   DIRECTORY ${_ouster_ros_INCLUDE_DIRS}
   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})
-install(FILES ouster.launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+install(FILES ../LICENSE ../LICENSE-bin ouster.launch viz.rviz
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
diff --git a/drivers/ouster-ros/Dockerfile b/drivers/ouster-ros/Dockerfile
index fc497516..f404cd98 100644
--- a/drivers/ouster-ros/Dockerfile
+++ b/drivers/ouster-ros/Dockerfile
@@ -34,10 +34,7 @@ RUN set -xe \
 && rosdep install -y --from-paths ${OUSTER_SDK_PATH}
 
 # Set up build environment
-COPY --chown=build:build cmake ${OUSTER_SDK_PATH}/cmake
-COPY --chown=build:build ouster_client ${OUSTER_SDK_PATH}/ouster_client
-COPY --chown=build:build ouster_viz ${OUSTER_SDK_PATH}/ouster_viz
-COPY --chown=build:build ouster_ros ${OUSTER_SDK_PATH}/ouster_ros
+COPY --chown=build:build . ${OUSTER_SDK_PATH}
 
 USER build:build
 WORKDIR ${BUILD_HOME}
diff --git a/drivers/ouster-ros/include/ouster_ros/point.h b/drivers/ouster-ros/include/ouster_ros/point.h
index 977a9b56..37726da9 100644
--- a/drivers/ouster-ros/include/ouster_ros/point.h
+++ b/drivers/ouster-ros/include/ouster_ros/point.h
@@ -7,7 +7,7 @@
 #define PCL_NO_PRECOMPILE
 #include <pcl/point_types.h>
 
-#include <Eigen/Eigen>
+#include <Eigen/Dense>
 #include <chrono>
 #include <functional>
 
diff --git a/drivers/ouster-ros/ouster.launch b/drivers/ouster-ros/ouster.launch
index 0a208c29..c754da11 100644
--- a/drivers/ouster-ros/ouster.launch
+++ b/drivers/ouster-ros/ouster.launch
@@ -7,7 +7,7 @@
   <arg name="replay" default="false" doc="do not connect to a sensor; expect /os_node/{lidar,imu}_packets from replay"/>
   <arg name="lidar_mode" default="" doc="resolution and rate: either 512x10, 512x20, 1024x10, 1024x20, or 2048x10"/>
   <arg name="timestamp_mode" default="" doc="method used to timestamp measurements: TIME_FROM_INTERNAL_OSC, TIME_FROM_SYNC_PULSE_IN, TIME_FROM_PTP_1588"/>
-  <arg name="metadata" default="" doc="override default metadata file for replays"/>
+  <arg name="metadata" default="" doc="path to read or write metadata file when replaying or receiving sensor data, respectively"/>
   <arg name="viz" default="false" doc="whether to run a rviz"/>
   <arg name="rviz_config" default="-d $(find ouster_ros)/viz.rviz" doc="optional rviz config file"/>
   <arg name="tf_prefix" default="" doc="namespace for tf transforms"/>
diff --git a/drivers/ouster-ros/package.xml b/drivers/ouster-ros/package.xml
index 9ad1606f..4d5e31c7 100644
--- a/drivers/ouster-ros/package.xml
+++ b/drivers/ouster-ros/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>ouster_ros</name>
-  <version>0.2.0</version>
+  <version>0.2.1</version>
   <description>Ouster example ROS client</description>
   <maintainer email="oss@ouster.io">ouster developers</maintainer>
   <license>BSD</license>
@@ -25,5 +25,7 @@
   <exec_depend>message_runtime</exec_depend>
   <exec_depend>topic_tools</exec_depend>
 
+  <test_depend>gtest</test_depend>
+
   <export></export>
 </package>
diff --git a/drivers/ouster-ros/src/os_node.cpp b/drivers/ouster-ros/src/os_node.cpp
index e01fa35a..8a2f02b6 100644
--- a/drivers/ouster-ros/src/os_node.cpp
+++ b/drivers/ouster-ros/src/os_node.cpp
@@ -166,8 +166,8 @@ int main(int argc, char** argv) {
         return EXIT_FAILURE;
     }
 
-    if (!replay && (!hostname.size() || !udp_dest.size())) {
-        ROS_ERROR("Must specify both hostname and udp destination");
+    if (!replay && !hostname.size()) {
+        ROS_ERROR("Must specify a sensor hostname");
         return EXIT_FAILURE;
     }
 
@@ -193,9 +193,12 @@ int main(int argc, char** argv) {
             ROS_ERROR("Error when running in replay mode: %s", e.what());
         }
     } else {
-        ROS_INFO("Connecting to %s; sending data to %s", hostname.c_str(),
-                 udp_dest.c_str());
-        ROS_INFO("Waiting for sensor to initialize ...");
+        if (udp_dest.size()) {
+            ROS_INFO("Sending UDP data to %s", udp_dest.c_str());
+        } else {
+            ROS_INFO("Using automatic UDP destination");
+        }
+        ROS_INFO("Waiting for sensor %s to initialize ...", hostname.c_str());
 
         auto cli = sensor::init_client(hostname, udp_dest, lidar_mode,
                                        timestamp_mode, lidar_port, imu_port);
@@ -206,7 +209,8 @@ int main(int argc, char** argv) {
         }
         ROS_INFO("Sensor initialized successfully");
 
-        // write metadata file to cwd (usually ~/.ros)
+        // write metadata file. If metadata_path is relative, will use cwd
+        // (usually ~/.ros)
         auto metadata = sensor::get_metadata(*cli);
         if (!write_metadata(meta_file, metadata)) {
             ROS_ERROR("Exiting because of failure to write metadata path");