Adding a Tins-based driver implementation (#77)

* Ported optional-lite source files * Added new functions to types.h/cpp * Function changes lidar.h/cpp * Added and changed functions in client.h/cpp * Bug fix from ouster_example/#246 * Added signal_multiplier parameter to client and types files * Updated changelog * Updated changelog * Updated changelog with a more detailed list of changes * Updated readme with FW version * Created outline of SensorTins class * Completed non-libtins related parts of SensorTins * Added core Libtins elements and dependency * Finished first design of SensorTins * Implemented the ability to switch between default and Tins sensor objects * Added an example metadata file * Tweaks and fixes to get SensorTins working * Improved the documentation around the metadata file, and added code to ensure the Sensor driver implementation writes to it. * Readme and changelog updates * Updated readme with commands for running the driver in Tins mode * Changed the implementation of SensorTins to more closely match Steves vision for how the user changes between driver types * Commented out some erroneously committed debugging code * Changed driver stucture to use different sets of launch and main files for the DefaultDriver and TinsDriver * Updated changelog and readme * Changed metadata filepath to be relative rather than absolute * Added an IP source filter to the Tins sniffer to avoid the sniffer being flooded with irrelevant packets * Updated readme with FW version * Added verbosity settings to launch files * Moved parameter getting to the sensor implementation from the driver implementation * Shifted parameter declaration to sensor::onConfigure functions and made sensor config files separate * Fixed bug caused by a missing parameter declaration * Added debug output to notify the user of how many packets were accumulated into the published point cloud * Added debug code to show the packet retrieval time * Minor tweaks to verbosity and metadata * Updated copyright header to reflect original author * Updated readme and param files * Updated changelog and removed some debugging code * Updated readme, reverted default IP values, removed unneccesary newline * Added missing package.xml dependency and reverted Tins driver verbosity back to INFO by default * Moved common sensor parameters back to Driver onCOnfigure * Renamed default driver class and files names * Changed the getMetadata service to save data to a file when specified * Bug fixes for default driver launch file and metadata filepath printing * Updated changelog * Updated changelog again because nested lists don't seem to work with .rst files * Updated the changelog again to fix formatting * Added example metadata files and removed old ones * Updated readme and GetMetadata service * Moved sniffer to a unique pointer from raw pointer. Also updated metadata filepath processing * Updated CMakeLists file to remove unnecessary libtins include line * Put LiDAR and computer IP getting back inside a try/catch block * First pass from the ROS2 linter * Final changes from ament_uncrustify and ament_cpplint * Added caveats to explain that the ipv6 discovery only works with the default driver * Changelog and readme updates * Shifted INFO outputs that are sensor specific to the configure function for those sensor implementations
ros-drivers · Aug 4, 2021 · 593dc2b · 593dc2b
1 parent 261fb3f
commit 593dc2b
Show file tree

Hide file tree

Showing 30 changed files with 1,865 additions and 99 deletions.
diff --git a/CHANGELOG.rst b/CHANGELOG.rst
@@ -1,33 +1,48 @@
 Changelog
 =========
 
-[unreleased] Ported client changes for FW 2.0/2.1 (2021-06-23)
+[0.3.0] Added a new SensorTins implementation (2021-06-23)
+-------------
+
+**General Changes**
+
+* Added example metadata files for OS0-128 in 1024x10 and 2048x10 mode.
+* Added/exposed the functions for loading and saving metadata to a json file.
+* Added metadatat file saving to ``getMetadata`` service.
+* Contibutors: Matthew Young (Trimble Inc).
+
+**TinsDriver**
+
+* Added a new SensorImplementation called SensorTins, and a Driver implementation that uses it called TinsDriver. This implementation uses the Tins library to find LiDAR and IMU packets, and is intended to be used with replayed pcap files rather than a real LiDAR. 
+* Added a new driver called TinsDriver which utilizes the existing OusterDriver class with the SensorTins class.
+* Added an new set of launch, param and main files for the TinsDriver.
+* Updated readme and parameter file to explain usage of the TinsDriver implementation.
+* Added some additional debug code to print the number of packets that have been accumulated into a cloud when it is published.
+* Contibutors: Matthew Young (Trimble Inc).
+
+[0.3.0] Ported client changes for FW 2.0/2.1 (2021-06-23)
 -------------
 
 **ouster_client**
 
-* From [ouster_example #239](https://github.com/ouster-lidar/ouster_example/commit/e1176f427f68eb0807bb15ccabe34aea47a1c5d3). 
-  * Ported `Optional` code: 
-    * Optional header and license file are now present in their own folder: `include/ros2_ouster/client/optional-lite/`
-    * Ported some mode-getting functions that use Optional to types.cpp
-  * Ported the `sensor_config` struct 
-    * Ported sensor_config struct to types.hpp
-    * Ported the get/set_config functions for sensor_config to client.cpp/.hpp
-  * Ported some documentation updates, and a != operator for LidarScan objects to lidar_scan.h.
-  * Ported/expanded data objects added by ouster_example #239  
-    * Ported several additional enumerated types (e.g. OperatingMode and MultipurposeIOMode) to types.h
-    * Ported parsing functions for additional enumerated types to types.cpp
+* From ouster_example (`#239 <https://github.com/ouster-lidar/ouster_example/commit/e1176f427f68eb0807bb15ccabe34aea47a1c5d3>`_). 
+    * Optional header and license file are now present in their own folder: ``include/ros2_ouster/client/optional-lite/``.
+    * Ported some mode-getting functions that use Optional to types.cpp.
+    * Ported sensor_config struct to types.hpp.
+    * Ported the get/set_config functions for sensor_config to client.cpp/.hpp.
+    * Ported some documentation updates, and a != operator for LidarScan objects to lidar_scan.h.
+    * Ported several additional enumerated types (e.g. OperatingMode and MultipurposeIOMode) to types.h.
+    * Ported parsing functions for additional enumerated types to types.cpp.
     * Ported ColumnWindow definition and added it as a parameter to data_format struct, which is the only existing struct changed.
-  * In lidar_scan.cpp, ported some changes to `make_xyz_lut` and `cartesian` functions  
-* From [ouster_example/#246](https://github.com/ouster-lidar/ouster_example/commit/2b49e6a2f3dbd0462c557974a3f428915067fd2f)
-  * Removed a json include incorrectly added to types.h  
-* From [ouster_example #259](https://github.com/ouster-lidar/ouster_example/commit/b8b23c35d7b719d69341a438d386a801688aa6a4#diff-db793e44a91d87bc6e0d94870833d5d9eeb4420e2fbbec4e2468717a359a651f)
-  * Increased default timeout time from 30 to 60 seconds for `init_client` and `read_imu_packet`
-  * In netcompat.cpp, changed non_blocking_mode from off to on (0 to 1) if using Win32.
-  * Ported signal_multiplier parameter 
-    * Added parameter to `sensor_config` in types.h
-    * Added signal_multiplier parameter setting to some functions in client.cpp and types.cpp
-* Updated changelog
+    * In lidar_scan.cpp, ported some changes to ``make_xyz_lut`` and ``cartesian`` functions.  
+* From ouster_example (`#246 <https://github.com/ouster-lidar/ouster_example/commit/2b49e6a2f3dbd0462c557974a3f428915067fd2f>`_).
+    * Removed a json include incorrectly added to types.h.  
+* From ouster_example (`#259 <https://github.com/ouster-lidar/ouster_example/commit/b8b23c35d7b719d69341a438d386a801688aa6a4#diff-db793e44a91d87bc6e0d94870833d5d9eeb4420e2fbbec4e2468717a359a651f>`_).
+    * Increased default timeout time from 30 to 60 seconds for ``init_client`` and ``read_imu_packet``.
+    * In netcompat.cpp, changed non_blocking_mode from off to on (0 to 1) if using Win32.
+    * Added ``signal_multiplier`` parameter to ``sensor_config`` in types.h.
+    * Added ``signal_multiplier`` parameter setting to some functions in client.cpp and types.cpp.
+* Updated changelog.
 
 **General Changes**
 

diff --git a/README.md b/README.md
@@ -36,21 +36,22 @@ See design doc in `design/*` directory [here](ros2_ouster/design/design_doc.md).
 | Service           | Type                    | Description                       |
 |-------------------|-------------------------|-----------------------------------|
 | `reset`           | std_srvs/Empty          | Reset the sensor's connection     |
-| `GetMetadata`     | ouster_msgs/GetMetadata | Get information about the sensor  |
+| `GetMetadata`     | ouster_msgs/GetMetadata | Get information about the sensor. A optional filepath can be specified to save the metadata to a local file.  |
 
 | Parameter                | Type    | Description                                                                                                 |
 |--------------------------|---------|-------------------------------------------------------------------------------------------------------------|
 | `lidar_ip`               | String  | IP or hostname of lidar (ex. 10.5.5.87, os1-serialno.local)                                                 |
-| `computer_ip`            | String  | IP or hostname of computer to get data (ex. 10.5.5.1) or broadcast (ex. 255.255.255.255) or "" for automatic detection                   |
+| `computer_ip`            | String  | IP or hostname of computer to get data (ex. 10.5.5.1) or broadcast (ex. 255.255.255.255) or if using the default driver, "" for automatic detection                   |
 | `lidar_mode`             | String  | Mode of data capture, default `512x10`                                                                      |
 | `imu_port`               | int     | Port of IMU data, default 7503                                                                              |
 | `lidar_port`             | int     | Port of laser data, default 7502                                                                            |
 | `sensor_frame`           | String  | TF frame of sensor, default `laser_sensor_frame`                                                            |
 | `laser_frame`            | String  | TF frame of laser data, default `laser_data_frame`                                                          |
 | `imu_frame`              | String  | TF frame of imu data, default `imu_data_frame`                                                              |
+| `ethernet_device`        | String  | An ethernet device (e.g. eth0 or eno1) on which the Tins driver will listen for packets. Note that this is only a parameter for the Tins driver, and is only specified in the config file for that driver. |                              |
 | `use_system_default_qos` | bool    | Publish data with default QoS for rosbag2 recording, default `False`                                        |
 | `timestamp_mode`         | String  | Method used to timestamp measurements, default `TIME_FROM_INTERNAL_OSC`                                     |
-| `os1_proc_mask`          | String  | Mask encoding data processors to activate, default <code>IMG &#124; PCL &#124; IMU &#124; SCAN</code> |
+| `os1_proc_mask`          | String  | Mask encoding data processors to activate, default <code>IMG &#124; PCL &#124; IMU &#124; SCAN</code>       |
 
 </center>
 
@@ -283,19 +284,61 @@ lidar_ip: "fe80::be0f:a7ff:fe00:2861%eth2"
 computer_ip: ""
 ```
 
-### ROS Connection
+Note that this feature is only available with the default driver version, configured by `driver_config.yaml`. When running the Tins-based driver (see the following sections), both the LiDAR and computer IP address must be specified in `tins_driver_config.yaml`.
 
-Now that we have a connection over the network, lets view some data. After building your colcon workspace with this package, source the install space. Run
+### Usage with the default driver
+
+Now that we have a connection over the network, lets view some data. After building your colcon workspace with this package, source the install space, then run:
 
 ```
-ros2 launch ros2_ouster os1_launch.py
+ros2 launch ros2_ouster driver_launch.py
 ```
 
 Make sure to update your parameters file if you don't use the default IPs (10.5.5.1, 10.5.5.87). You may also use the `.local` version of your ouster lidar. To find your IPs, see the `dnsmasq` output or check with `nmap -SP 10.5.5.*/24`.
-An alternative tool is [avahi-browse](https://linux.die.net/man/1/avahi-browse) ```
+An alternative tool is [avahi-browse](https://linux.die.net/man/1/avahi-browse): 
+
+```
 avahi-browse -arlt
 ```
 
-Now that your connection is up (hopefully), you can view this information in RViz. Open an RViz session and subscribe to the points, images, and IMU topics in the laser frame.
+Now that your connection is up, you can view this information in RViz. Open an RViz session and subscribe to the points, images, and IMU topics in the laser frame. When trying to visualize the point clouds, be sure to change the Fixed Frame under Global Options to "laser_data_frame" as this is the default parent frame of the point cloud headers.
+
+When the driver configures itself, it will automatically read the metadata parameters from the Ouster. If you wish to save these parameters and use them with captured data (see the next section) then you can save the data to a specific location using the `getMetadata` service that the driver offers. To use it, run the driver with a real Ouster LiDAR and then make the following service call:
+
+```
+ros2 service call /ouster_driver/get_metadata ouster_msgs/srv/GetMetadata "{metadata_filepath: "/path/to/your/metadata.json"}"
+```
+
+The driver will then save all the required metadata to the specified file and dump the same metadata as a JSON string to the terminal. Alternatively, the service can be called without specifying a filepath (see below) in which case no file will be saved, and the metadata will still be printed to terminal. Copying this string and manually saving it to a .json file is also a valid way to generate a metadata file.  
+
+```
+ros2 service call /ouster_driver/get_metadata ouster_msgs/srv/GetMetadata
+```
 
 Have fun!
+
+### Usage with Tins-based driver
+
+If you want to use the driver to read data from a pcap file, you can use the `Tins`-based driver. To do this, open the `tins_driver_config.yaml` file and edit the following parameter:
+
+* `ethernet_device`: Change this to a working ethernet device on your computer that you plan to replay data through (e.g. "eth1").
+
+You can run the Tins driver with the command below. This will use the default `ouster_os0128_1024_metadata.json` file:
+
+```
+ros2 launch ros2_ouster tins_driver_launch.py
+```
+
+Alternatively, you can change the metadata being used by specifying the metadata filepath as shown below. You can generate a metadata file using the `getMetadata` service as shown in the previous section. Or you can use one of the example metadata files provided, which come from an Ouster OS0-128 in either 1024x10 or 2048x10 mode.
+
+```
+ros2 launch ros2_ouster tins_driver_launch.py metadata_filepath:=/path/to/metadata.json
+```
+
+After launching the driver, in a new terminal, you can replay a pcap file of recorded ouster data using the following command (as an example):
+
+```
+sudo tcpreplay --intf1=eth1 saved_ouster_data.pcap 
+```
+
+You may need to run this command with `sudo`. Note that this driver version will also work with a live Ouster sensor, provided the data is coming into the correct ethernet device, and the parameters in the metadata file match those of the sensor. However it is recommended that you run the default drive with a real sensor, as this will guarantee that the metadata settings are correct.
diff --git a/ouster_msgs/srv/GetMetadata.srv b/ouster_msgs/srv/GetMetadata.srv
@@ -1,3 +1,4 @@
 
+string metadata_filepath
 ---
 ouster_msgs/Metadata metadata
diff --git a/ros2_ouster/CMakeLists.txt b/ros2_ouster/CMakeLists.txt
@@ -1,6 +1,8 @@
 cmake_minimum_required(VERSION 3.5)
 project(ros2_ouster)
 
+# Default to compiling in Release mode. If not compiled in Release mode the 
+# driver WILL fail to hit its desired output publish rate.
 if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES)
   set(CMAKE_BUILD_TYPE Release)
 endif()
@@ -19,15 +21,18 @@ find_package(tf2_ros REQUIRED)
 find_package(pcl_conversions REQUIRED)
 find_package(ouster_msgs REQUIRED)
 find_package(PCL REQUIRED COMPONENTS common)
-
 find_package(jsoncpp REQUIRED)
 
 include_directories(
   include
   ${PCL_INCLUDE_DIRS}
 )
 
+# Create separate executables for the default and Tins drivers.
 set(executable_name ouster_driver)
+set(tins_executable_name ouster_tins_driver)
+
+# Create a single library target to use for both default and Tins driver.
 set(library_name ${executable_name}_core)
 
 set(dependencies
@@ -50,6 +55,7 @@ add_library(${library_name} SHARED
   src/ouster_driver.cpp
   src/lifecycle_interface.cpp
   src/sensor.cpp
+  src/sensor_tins.cpp
   src/client/client.cpp
   src/client/lidar_scan.cpp
   src/client/netcompat.cpp
@@ -62,19 +68,27 @@ ament_target_dependencies(${library_name}
 
 target_link_libraries(${library_name}
   jsoncpp_lib
+  tins
   ${PCL_LIBRARIES}
 )
 
 add_executable(${executable_name}
   src/main.cpp
 )
+add_executable(${tins_executable_name}
+  src/main_tins.cpp
+)
 
 target_link_libraries(${executable_name} ${library_name})
+target_link_libraries(${tins_executable_name} ${library_name})
+
+rclcpp_components_register_nodes(${library_name} "${PROJECT_NAME}::Driver")
+rclcpp_components_register_nodes(${library_name} "${PROJECT_NAME}::TinsDriver")
 
-rclcpp_components_register_nodes(ouster_driver_core "${PROJECT_NAME}::Driver")
 set(node_plugins "${node_plugins}${PROJECT_NAME}::Driver;$<TARGET_FILE:ouster_driver>\n")
+set(node_plugins "${node_plugins}${PROJECT_NAME}::TinsDriver;$<TARGET_FILE:ouster_driver>\n")
 
-install(TARGETS ${executable_name} ${library_name}
+install(TARGETS ${executable_name} ${tins_executable_name} ${library_name}
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION lib/${PROJECT_NAME}
@@ -100,4 +114,4 @@ endif()
 ament_export_include_directories(include)
 ament_export_libraries(${library_name})
 ament_export_dependencies(${dependencies})
-ament_package()
+ament_package()
diff --git a/ros2_ouster/include/ros2_ouster/driver_types.hpp b/ros2_ouster/include/ros2_ouster/driver_types.hpp
@@ -19,12 +19,33 @@
 namespace ros2_ouster
 {
 
+/**
+ * @brief The default Ouster driver for connecting to, configuring, and reading
+ *        data from a real Ouster LiDAR.
+ */
 class Driver : public OusterDriver
 {
 public:
   explicit Driver(rclcpp::NodeOptions options);
 };
 
+
+/**
+ * @brief An alternative Ouster driver based on the open-source Tins library.
+ *        This driver listens to an ethernet interface for packets with
+ *        matching metadata, so it can be used with data captured in a pcap file
+ *        and replayed after the fact.
+ * @details This driver differs from the implementation of Driver only in
+ *          that it is initialized with, and uses the SensorTins class instead
+ *          of the Sensor class. The utilization of the data accumulators and
+ *          processor classes is otherwise the same.
+ */
+class TinsDriver : public OusterDriver
+{
+public:
+  explicit TinsDriver(rclcpp::NodeOptions options);
+};
+
 }  // namespace ros2_ouster
 
 

diff --git a/ros2_ouster/include/ros2_ouster/full_rotation_accumulator.hpp b/ros2_ouster/include/ros2_ouster/full_rotation_accumulator.hpp
@@ -84,8 +84,11 @@ class FullRotationAccumulator
   {
     if (_batchReady) {
       _batchReady = false;
+      _packets_accumulated = 0;
     }
 
+    _packets_accumulated++;
+
     if (_batch->operator()(data, *_ls)) {
       auto h = std::find_if(
         _ls->headers.begin(), _ls->headers.end(), [](const auto & h) {
@@ -98,12 +101,19 @@ class FullRotationAccumulator
     }
   }
 
+  uint64_t getPacketsAccumulated()
+  {
+    return _packets_accumulated;
+  }
+
 private:
   bool _batchReady;
   std::chrono::nanoseconds _timestamp;
   std::unique_ptr<ouster::ScanBatcher> _batch;
   std::shared_ptr<ouster::LidarScan> _ls;
   ouster::sensor::packet_format _pf;
+
+  uint64_t _packets_accumulated = 0;
 };
 
 }  // namespace sensor

diff --git a/ros2_ouster/include/ros2_ouster/interfaces/configuration.hpp b/ros2_ouster/include/ros2_ouster/interfaces/configuration.hpp
@@ -29,6 +29,8 @@ struct Configuration
   int lidar_port;
   std::string lidar_mode;
   std::string timestamp_mode;
+  std::string metadata_filepath;
+  std::string ethernet_device;
 };
 
 }  // namespace ros2_ouster

diff --git a/ros2_ouster/include/ros2_ouster/interfaces/metadata.hpp b/ros2_ouster/include/ros2_ouster/interfaces/metadata.hpp
@@ -16,10 +16,10 @@
 
 #include <vector>
 #include <string>
+#include <iostream>
 #include "ros2_ouster/client/types.h"
 #include "ros2_ouster/client/version.h"
 #include "ros2_ouster/client/client.h"
-#include <iostream>
 
 namespace ros2_ouster
 {

diff --git a/ros2_ouster/include/ros2_ouster/interfaces/sensor_interface.hpp b/ros2_ouster/include/ros2_ouster/interfaces/sensor_interface.hpp
@@ -54,14 +54,20 @@ class SensorInterface
   /**
    * @brief Reset lidar sensor
    * @param configuration file to use
+   * @param node pointer to the driver node, which provides access to ROS params
    */
-  virtual void reset(const ros2_ouster::Configuration & config) = 0;
+  virtual void reset(
+    ros2_ouster::Configuration & config,
+    rclcpp_lifecycle::LifecycleNode::SharedPtr node) = 0;
 
   /**
    * @brief Configure lidar sensor
    * @param configuration file to use
+   * @param node pointer to the driver node, which provides access to ROS params
    */
-  virtual void configure(const ros2_ouster::Configuration & config) = 0;
+  virtual void configure(
+    ros2_ouster::Configuration & config,
+    rclcpp_lifecycle::LifecycleNode::SharedPtr node) = 0;
 
   /**
    * @brief Ask sensor to get its current state for data collection

diff --git a/ros2_ouster/include/ros2_ouster/ouster_driver.hpp b/ros2_ouster/include/ros2_ouster/ouster_driver.hpp
@@ -17,6 +17,8 @@
 #include <memory>
 #include <map>
 #include <string>
+#include <fstream>
+#include <sstream>
 
 #include "ros2_ouster/conversions.hpp"
 

diff --git a/ros2_ouster/include/ros2_ouster/processors/image_processor.hpp b/ros2_ouster/include/ros2_ouster/processors/image_processor.hpp
@@ -19,6 +19,7 @@
 #include <string>
 #include <utility>
 #include <algorithm>
+#include <limits>
 
 #include "rclcpp/qos.hpp"
 
@@ -128,7 +129,6 @@ class ImageProcessor : public ros2_ouster::DataProcessorInterface
 
     for (size_t u = 0; u < _height; u++) {
       for (size_t v = 0; v < _width; v++) {
-
         const size_t vv = (v + _width - _px_offset[u]) % _width;
         const size_t index = u * _width + vv;
 
@@ -220,7 +220,8 @@ class ImageProcessor : public ros2_ouster::DataProcessorInterface
   size_t _bit_depth = 8 * sizeof(uint8_t);
   const ouster::sensor::packet_format & _pf;
   const size_t _pixel_value_max = std::numeric_limits<uint8_t>::max();
-  double _range_multiplier = ouster::sensor::range_unit * (1.0 / 200.0);  // assuming 200 m range typical
+  // assuming 200 m range typical
+  double _range_multiplier = ouster::sensor::range_unit * (1.0 / 200.0);
   viz::AutoExposure _ambient_ae, _intensity_ae;
   viz::BeamUniformityCorrector _ambient_buc;
   ouster::LidarScan _ls;