Runtime selectable QoS to allow for recording sensor data with rosbag2 (

#26) * Rule-of-Zero on DataProcessorInterface * Added runtime-selectable QoS support to allow for recording sensor data via rosbag2 in Eloquent. Apparently, as of this writing, rosbag2 cannot record topic data whose QoS has a reliability level of anything but `RELIABLE`. See: ros2/rosbag2#125. The `rmw_qos_profile_sensor_data` (which we are using in this project *a priori*) employs `BEST_EFFORT` reliability. I've added a new parameter `use_system_default_qos_for_sensor_data` to the node and parameter file. By default this is set to `False` which maintains the original behavior of this package. However, if set to `True`, rather than `rmw_qos_profile_sensor_data` we use ``rmw_qos_profile_default` as our QoS profile for the sensor topics. This (supposedly) most closely mimics ROS (classic) behavior but more importantly, allows rosbag2 to record data from these topics. Hopefully this issue with rosbag2 gets addressed by Foxy and we can deprecate this feature. But for now, not having the ability to record data for offline analysis is very limiting and we need something like this in this driver package. * Style fix * more style fixes * Resolved comments from Steve on PR #26 * Lowered log level from WARN to INFO when using system defaults QoS
ros-drivers · Mar 13, 2020 · a468520 · a468520
1 parent 3e60bb4
commit a468520
Show file tree

Hide file tree

Showing 10 changed files with 88 additions and 56 deletions.
diff --git a/README.md b/README.md
@@ -1,6 +1,6 @@
 # ROS2 Ouster Drivers
 
-These are an implementation of ROS2 drivers for the Ouster OS-1 3D lidars. This includes all models of the OS-1 from 16 to 128 beams. 
+These are an implementation of ROS2 drivers for the Ouster OS-1 3D lidars. This includes all models of the OS-1 from 16 to 128 beams.
 
 You can find a few videos looking over the sensor below. They both introduce the ROS1 driver but are extremely useful references regardless:
 
@@ -9,11 +9,11 @@ OS-1 Networking Setup      |  OS-1 Data Overview
 [![IMAGE ALT TEXT HERE](http://img.youtube.com/vi/92ajXjIxDGM/0.jpg)](http://www.youtube.com/watch?v=92ajXjIxDGM) | [![IMAGE ALT TEXT HERE](http://img.youtube.com/vi/4VgGG8Xe4IA/0.jpg)](http://www.youtube.com/watch?v=4VgGG8Xe4IA)
 
 
-I also happen to run a YouTube channel, [Robots for Robots](https://www.youtube.com/channel/UCZT16dToD1ov6lnoEcPL6rw), focusing on robotics, sensors, and industry insights. If you like this work and want to hear about some other things I am working on, I'd appreciate if you watch, like, and subscribe! 
+I also happen to run a YouTube channel, [Robots for Robots](https://www.youtube.com/channel/UCZT16dToD1ov6lnoEcPL6rw), focusing on robotics, sensors, and industry insights. If you like this work and want to hear about some other things I am working on, I'd appreciate if you watch, like, and subscribe!
 
 ## Documentation
 
-Documentation can be generated using Doxygen. 
+Documentation can be generated using Doxygen.
 
 Run `doxygen` in the root of this repository. It will generate a `/doc/*` directory containing the documentation. Entrypoint in a browser is `index.html`.
 
@@ -40,16 +40,17 @@ See design doc in `design/*` directory [here](ros2_ouster/design/design_doc.md).
 | `reset`           | std_srvs/Empty          | Reset the sensor's connection     |
 | `GetMetadata`     | ouster_msgs/GetMetadata | Get information about the sensor  |
 
-| Parameter         | Type                    | Description                                         |
-|-------------------|-------------------------|-----------------------------------------------------|
-| `lidar_ip`        | String                  | IP of lidar (ex. 10.5.5.87)                         |
-| `computer_ip`     | String                  | IP of computer to get data (ex. 10.5.5.1)           |
-| `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`      |
+| Parameter                | Type    | Description                                                          |
+|--------------------------|---------|----------------------------------------------------------------------|
+| `lidar_ip`               | String  | IP of lidar (ex. 10.5.5.87)                                          |
+| `computer_ip`            | String  | IP of computer to get data (ex. 10.5.5.1)                            |
+| `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`                       |
+| `use_system_default_qos` | bool    | Publish data with default QoS for rosbag2 recording, default `False` |
 
 </center>
 
@@ -60,7 +61,7 @@ Note: TF will provide you the transformations from the sensor frame to each of t
 This package was intentionally designed for new capabilities to be added. Whether that being supporting new classes of Ouster lidars (OS1-custom, OS2, ...) or supporting new ways of processing the data packets.
 
 ### Additional Lidar Processing
-It can be imagined that if you have a stream of lidar or IMU packets, you may want to process them differently. If you're working with a high speed vehicle, you may want the packets projected into a pointcloud and published with little batching inside the driver. If you're working with pointclouds for machine learning, you may only want the pointcloud to include the `XYZ` information and not the intensity, reflectivity, and noise information to reduce dimensionality. 
+It can be imagined that if you have a stream of lidar or IMU packets, you may want to process them differently. If you're working with a high speed vehicle, you may want the packets projected into a pointcloud and published with little batching inside the driver. If you're working with pointclouds for machine learning, you may only want the pointcloud to include the `XYZ` information and not the intensity, reflectivity, and noise information to reduce dimensionality.
 
 In any case, I provide a set of logical default processing implementations on the lidar and IMU packets. These are implementations of the `ros2_ouster::DataProcessorInterface` class in the `interfaces` directory. To create your own processor to change the pointcloud type, buffering methodology, or some new cool thing, you must create an implementation of a data processor.
 
@@ -77,7 +78,7 @@ Some examples:
 ### Additional Lidar Units
 To create a new lidar for this driver, you only need to make an implementation of the `ros2_ouster::SensorInterface` class and include any required SDKs. Then, in the `driver_types.hpp` file, add your new interface as a template of the `OusterDriver` and you're good to go.
 
-You may need to add an additional `main` method for the new templated program, depending if you're using components. If it uses another underlying SDK other than `OS1` you will also need to create new processors for it as the processors are bound to a specific unit as the data formatting may be different. If they are the same, you can reuse the `OS1` processors. 
+You may need to add an additional `main` method for the new templated program, depending if you're using components. If it uses another underlying SDK other than `OS1` you will also need to create new processors for it as the processors are bound to a specific unit as the data formatting may be different. If they are the same, you can reuse the `OS1` processors.
 
 ## Lifecycle
 
@@ -113,29 +114,29 @@ The `[eth name]` is the nework interface you're connecting to. On older Linux sy
  ip addr show dev [eth name]
 ```
 
-The output you see from `show` should look something like `[eth name] ... state DOWN ...`. Its only important that you see `DOWN` and not `UP`. Next, lets setup a static IP address for your machine so you can rely on this in the future. Ouster uses the 10.5.5.* range, and I don't see a compelling reason to argue with it. 
+The output you see from `show` should look something like `[eth name] ... state DOWN ...`. Its only important that you see `DOWN` and not `UP`. Next, lets setup a static IP address for your machine so you can rely on this in the future. Ouster uses the 10.5.5.* range, and I don't see a compelling reason to argue with it.
 
 ```
 sudo ip addr add 10.5.5.1/24 dev [eth name]
 
 ```
 
-Now, lets setup the connection. At this point you may now plug in and power on your sensor. 
+Now, lets setup the connection. At this point you may now plug in and power on your sensor.
 
 ```
 sudo ip link set [eth name] up
 sudo addr show dev [eth name]
 ```
 
-The output you see from `show` should look something like `[eth name] ... state UP ...`. Its only important that you see `UP` now and not `DOWN`. At this point, you've setup the networking needed for the one time setup. 
+The output you see from `show` should look something like `[eth name] ... state UP ...`. Its only important that you see `UP` now and not `DOWN`. At this point, you've setup the networking needed for the one time setup.
 
 ### Connection
 
 We can setup the network connection to the sensor now with the proper settings. Note: This command could take up to 30 seconds to setup, be patient. If after a minute you see no results, then you probably have an issue. Start the instructions above over. Lets set up the network
 
 ```
 sudo dnsmasq -C /dev/null -kd -F 10.5.5.50,10.5.5.100 -i [eth name] --bind-dynamic
-``` 
+```
 
 Instantly you should see something similar to:
 
@@ -176,8 +177,8 @@ Now that we have a connection over the network, lets view some data. After build
 ros2 launch ros2_ouster os1_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`. 
+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`.
 
 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.
 
-Have fun!
+Have fun!
diff --git a/ros2_ouster/include/ros2_ouster/OS1/processor_factories.hpp b/ros2_ouster/include/ros2_ouster/OS1/processor_factories.hpp
@@ -18,6 +18,7 @@
 #include <map>
 #include <utility>
 
+#include "rclcpp/qos.hpp"
 #include "ros2_ouster/conversions.hpp"
 #include "ros2_ouster/OS1/processors/image_processor.hpp"
 #include "ros2_ouster/OS1/processors/imu_processor.hpp"
@@ -35,9 +36,10 @@ namespace ros2_ouster
 inline ros2_ouster::DataProcessorInterface * createImageProcessor(
   const rclcpp_lifecycle::LifecycleNode::SharedPtr node,
   const ros2_ouster::Metadata & mdata,
-  const std::string & frame)
+  const std::string & frame,
+  const rclcpp::QoS & qos)
 {
-  return new OS1::ImageProcessor(node, mdata, frame);
+  return new OS1::ImageProcessor(node, mdata, frame, qos);
 }
 
 /**
@@ -48,9 +50,10 @@ inline ros2_ouster::DataProcessorInterface * createImageProcessor(
 inline ros2_ouster::DataProcessorInterface * createPointcloudProcessor(
   const rclcpp_lifecycle::LifecycleNode::SharedPtr node,
   const ros2_ouster::Metadata & mdata,
-  const std::string & frame)
+  const std::string & frame,
+  const rclcpp::QoS & qos)
 {
-  return new OS1::PointcloudProcessor(node, mdata, frame);
+  return new OS1::PointcloudProcessor(node, mdata, frame, qos);
 }
 
 /**
@@ -61,9 +64,10 @@ inline ros2_ouster::DataProcessorInterface * createPointcloudProcessor(
 inline ros2_ouster::DataProcessorInterface * createIMUProcessor(
   const rclcpp_lifecycle::LifecycleNode::SharedPtr node,
   const ros2_ouster::Metadata & mdata,
-  const std::string & frame)
+  const std::string & frame,
+  const rclcpp::QoS & qos)
 {
-  return new OS1::IMUProcessor(node, mdata, frame);
+  return new OS1::IMUProcessor(node, mdata, frame, qos);
 }
 
 /**
@@ -74,31 +78,33 @@ inline ros2_ouster::DataProcessorInterface * createIMUProcessor(
 inline ros2_ouster::DataProcessorInterface * createScanProcessor(
   const rclcpp_lifecycle::LifecycleNode::SharedPtr node,
   const ros2_ouster::Metadata & mdata,
-  const std::string & frame)
+  const std::string & frame,
+  const rclcpp::QoS & qos)
 {
-  return new OS1::ScanProcessor(node, mdata, frame);
+  return new OS1::ScanProcessor(node, mdata, frame, qos);
 }
 
 inline std::multimap<ClientState, DataProcessorInterface *> createProcessors(
   const rclcpp_lifecycle::LifecycleNode::SharedPtr node,
   const ros2_ouster::Metadata & mdata,
   const std::string & imu_frame,
-  const std::string & laser_frame)
+  const std::string & laser_frame,
+  const rclcpp::QoS & qos)
 {
   std::multimap<ClientState, DataProcessorInterface *> data_processors;
 
   data_processors.insert(std::pair<ClientState, DataProcessorInterface *>(
       ClientState::IMU_DATA, createIMUProcessor(
-        node, mdata, imu_frame)));
+        node, mdata, imu_frame, qos)));
   data_processors.insert(std::pair<ClientState, DataProcessorInterface *>(
       ClientState::LIDAR_DATA, createPointcloudProcessor(
-        node, mdata, laser_frame)));
+        node, mdata, laser_frame, qos)));
   data_processors.insert(std::pair<ClientState, DataProcessorInterface *>(
       ClientState::LIDAR_DATA, createImageProcessor(
-        node, mdata, laser_frame)));
+        node, mdata, laser_frame, qos)));
   data_processors.insert(std::pair<ClientState, DataProcessorInterface *>(
       ClientState::LIDAR_DATA, createScanProcessor(
-        node, mdata, laser_frame)));
+        node, mdata, laser_frame, qos)));
 
   return data_processors;
 }

diff --git a/ros2_ouster/include/ros2_ouster/OS1/processors/image_processor.hpp b/ros2_ouster/include/ros2_ouster/OS1/processors/image_processor.hpp
@@ -20,6 +20,8 @@
 #include <utility>
 #include <algorithm>
 
+#include "rclcpp/qos.hpp"
+
 #include "ros2_ouster/conversions.hpp"
 
 #include "sensor_msgs/msg/image.hpp"
@@ -50,7 +52,8 @@ class ImageProcessor : public ros2_ouster::DataProcessorInterface
   ImageProcessor(
     const rclcpp_lifecycle::LifecycleNode::SharedPtr node,
     const ros2_ouster::Metadata & mdata,
-    const std::string & frame)
+    const std::string & frame,
+    const rclcpp::QoS & qos)
   : DataProcessorInterface(), _node(node), _frame(frame)
   {
     _height = OS1::pixels_per_column;
@@ -61,13 +64,13 @@ class ImageProcessor : public ros2_ouster::DataProcessorInterface
         mdata.beam_altitude_angles);
 
     _range_image_pub = _node->create_publisher<sensor_msgs::msg::Image>(
-      "range_image", rclcpp::SensorDataQoS());
+      "range_image", qos);
     _intensity_image_pub = _node->create_publisher<sensor_msgs::msg::Image>(
-      "intensity_image", rclcpp::SensorDataQoS());
+      "intensity_image", qos);
     _noise_image_pub = _node->create_publisher<sensor_msgs::msg::Image>(
-      "noise_image", rclcpp::SensorDataQoS());
+      "noise_image", qos);
     _reflectivity_image_pub = _node->create_publisher<sensor_msgs::msg::Image>(
-      "reflectivity_image", rclcpp::SensorDataQoS());
+      "reflectivity_image", qos);
 
     _range_image.width = _width;
     _range_image.height = _height;

diff --git a/ros2_ouster/include/ros2_ouster/OS1/processors/imu_processor.hpp b/ros2_ouster/include/ros2_ouster/OS1/processors/imu_processor.hpp
@@ -20,6 +20,7 @@
 
 #include "ros2_ouster/conversions.hpp"
 
+#include "rclcpp/qos.hpp"
 #include "rclcpp_lifecycle/lifecycle_node.hpp"
 #include "sensor_msgs/msg/imu.hpp"
 
@@ -45,11 +46,11 @@ class IMUProcessor : public ros2_ouster::DataProcessorInterface
   IMUProcessor(
     const rclcpp_lifecycle::LifecycleNode::SharedPtr node,
     const ros2_ouster::Metadata & mdata,
-    const std::string & frame)
+    const std::string & frame,
+    const rclcpp::QoS & qos)
   : DataProcessorInterface(), _node(node), _frame(frame)
   {
-    _pub = node->create_publisher<sensor_msgs::msg::Imu>(
-      "imu", rclcpp::SensorDataQoS());
+    _pub = node->create_publisher<sensor_msgs::msg::Imu>("imu", qos);
   }
 
   /**

diff --git a/ros2_ouster/include/ros2_ouster/OS1/processors/pointcloud_processor.hpp b/ros2_ouster/include/ros2_ouster/OS1/processors/pointcloud_processor.hpp
@@ -18,6 +18,8 @@
 #include <memory>
 #include <string>
 
+#include "rclcpp/qos.hpp"
+
 #include "ros2_ouster/conversions.hpp"
 
 #include "sensor_msgs/msg/point_cloud2.hpp"
@@ -46,17 +48,19 @@ class PointcloudProcessor : public ros2_ouster::DataProcessorInterface
   PointcloudProcessor(
     const rclcpp_lifecycle::LifecycleNode::SharedPtr node,
     const ros2_ouster::Metadata & mdata,
-    const std::string & frame)
+    const std::string & frame,
+    const rclcpp::QoS & qos)
   : DataProcessorInterface(), _node(node), _frame(frame)
   {
     _height = OS1::pixels_per_column;
     _width = OS1::n_cols_of_lidar_mode(
       OS1::lidar_mode_of_string(mdata.mode));
     _xyz_lut = OS1::make_xyz_lut(_width, _height, mdata.beam_azimuth_angles,
         mdata.beam_altitude_angles);
-    _cloud = std::make_shared<pcl::PointCloud<point_os::PointOS>>(_width, _height);
+    _cloud =
+      std::make_shared<pcl::PointCloud<point_os::PointOS>>(_width, _height);
     _pub = _node->create_publisher<sensor_msgs::msg::PointCloud2>(
-      "points", rclcpp::SensorDataQoS());
+      "points", qos);
 
     _batch_and_publish =
       OS1::batch_to_iter<pcl::PointCloud<point_os::PointOS>::iterator>(

diff --git a/ros2_ouster/include/ros2_ouster/OS1/processors/scan_processor.hpp b/ros2_ouster/include/ros2_ouster/OS1/processors/scan_processor.hpp
@@ -18,6 +18,8 @@
 #include <memory>
 #include <string>
 
+#include "rclcpp/qos.hpp"
+
 #include "ros2_ouster/conversions.hpp"
 
 #include "sensor_msgs/msg/laser_scan.hpp"
@@ -49,12 +51,12 @@ class ScanProcessor : public ros2_ouster::DataProcessorInterface
   ScanProcessor(
     const rclcpp_lifecycle::LifecycleNode::SharedPtr node,
     const ros2_ouster::Metadata & mdata,
-    const std::string & frame)
+    const std::string & frame,
+    const rclcpp::QoS & qos)
   : DataProcessorInterface(), _node(node), _frame(frame)
   {
     _mdata = mdata;
-    _pub = _node->create_publisher<sensor_msgs::msg::LaserScan>(
-      "scan", rclcpp::SensorDataQoS());
+    _pub = _node->create_publisher<sensor_msgs::msg::LaserScan>("scan", qos);
     _height = OS1::pixels_per_column;
     _width = OS1::n_cols_of_lidar_mode(
       OS1::lidar_mode_of_string(mdata.mode));

diff --git a/ros2_ouster/include/ros2_ouster/interfaces/data_processor_interface.hpp b/ros2_ouster/include/ros2_ouster/interfaces/data_processor_interface.hpp
@@ -38,11 +38,6 @@ class DataProcessorInterface
    */
   DataProcessorInterface() {}
 
-  /**
-   * @brief Destructor of the data processor interface
-   */
-  ~DataProcessorInterface() {}
-
   /**
    * @brief Process a packet with the lidar-specific APIs
    * @param data packet input

diff --git a/ros2_ouster/include/ros2_ouster/ouster_driver.hpp b/ros2_ouster/include/ros2_ouster/ouster_driver.hpp
@@ -136,6 +136,8 @@ class OusterDriver : public lifecycle_interface::LifecycleInterface
 
   std::string _laser_sensor_frame, _laser_data_frame, _imu_data_frame;
   std::unique_ptr<tf2_ros::StaticTransformBroadcaster> _tf_b;
+
+  bool _use_system_default_qos;
 };
 
 }  // namespace ros2_ouster

diff --git a/ros2_ouster/params/os1.yaml b/ros2_ouster/params/os1.yaml
@@ -8,3 +8,8 @@ ouster_driver:
     sensor_frame: laser_sensor_frame
     laser_frame: laser_data_frame
     imu_frame: imu_data_frame
+
+    # if False, data are published with sensor data QoS. This is preferrable
+    # for production but default QoS is needed for rosbag.
+    # See: https://github.com/ros2/rosbag2/issues/125
+    use_system_default_qos: False
diff --git a/ros2_ouster/src/ouster_driver.cpp b/ros2_ouster/src/ouster_driver.cpp
@@ -17,6 +17,7 @@
 #include <memory>
 #include <utility>
 
+#include "rclcpp/qos.hpp"
 #include "ros2_ouster/exception.hpp"
 #include "ros2_ouster/driver_types.hpp"
 
@@ -40,6 +41,7 @@ OusterDriver<SensorT>::OusterDriver(const rclcpp::NodeOptions & options)
   this->declare_parameter("sensor_frame", std::string("laser_sensor_frame"));
   this->declare_parameter("laser_frame", std::string("laser_data_frame"));
   this->declare_parameter("imu_frame", std::string("imu_data_frame"));
+  this->declare_parameter("use_system_default_qos", false);
 }
 
 template<typename SensorT>
@@ -68,6 +70,7 @@ void OusterDriver<SensorT>::onConfigure()
   _laser_sensor_frame = get_parameter("sensor_frame").as_string();
   _laser_data_frame = get_parameter("laser_frame").as_string();
   _imu_data_frame = get_parameter("imu_frame").as_string();
+  _use_system_default_qos = get_parameter("use_system_default_qos").as_bool();
 
   RCLCPP_INFO(this->get_logger(),
     "Connecting to sensor at %s.", lidar_config.lidar_ip.c_str());
@@ -90,10 +93,20 @@ void OusterDriver<SensorT>::onConfigure()
 
   ros2_ouster::Metadata mdata = _sensor->getMetadata();
 
-  _data_processors = ros2_ouster::createProcessors(
-    shared_from_this(), mdata, _imu_data_frame, _laser_data_frame);
+  if (_use_system_default_qos) {
+    RCLCPP_INFO(
+      this->get_logger(), "Using system defaults QoS for sensor data");
+    _data_processors = ros2_ouster::createProcessors(
+      shared_from_this(), mdata, _imu_data_frame, _laser_data_frame,
+      rclcpp::SystemDefaultsQoS());
+  } else {
+    _data_processors = ros2_ouster::createProcessors(
+      shared_from_this(), mdata, _imu_data_frame, _laser_data_frame,
+      rclcpp::SensorDataQoS());
+  }
 
-  _tf_b = std::make_unique<tf2_ros::StaticTransformBroadcaster>(shared_from_this());
+  _tf_b = std::make_unique<tf2_ros::StaticTransformBroadcaster>(
+    shared_from_this());
   broadcastStaticTransforms(mdata);
 }