Provides a simple way to selectively turn on/off data processors

README.md

@@ -38,18 +38,19 @@ 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`                         |
-| `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`|
+| 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`       |
+| `timestamp_mode`         | String  | Method used to timestamp measurements, default `TIME_FROM_INTERNAL_OSC`    |
+| `os1_proc_mask`          | String  | Mask encoding which data processors to activate, default `IMG|PCL|IMU|SCAN`|
 
 </center>
 
@@ -102,6 +103,31 @@ particularly those traveling at higher speeds, it is not recommended to use
 this `timestamp_mode`. When running in this mode, the on-LiDAR `timestamp_mode`
 will not be set by this driver.
 
+### Parameterizing the Active Data Processors
+
+The `os1_proc_mask` parameter is set to a mask-like-string used to define the
+data processors that should be activated upon startup of the driver. This will
+determine the topics that are available for client applications to consume. The
+*de facto* reference for these values are defined in
+[processor_factories.hpp](ros2_ouster/include/ros2_ouster/OS1/processor_factories.hpp). It
+is recommended to only use the processors that you require for your application.
+
+The available data processors are:
+
+- **IMG** Provides 8-bit image topics encoding the noise, range, intensity, and
+  reflectivitiy from a scan.
+- **PCL** Provides a point cloud encoding of a LiDAR scan
+- **IMU** Provides a data stream from the LiDAR's integral IMU
+- **SCAN** Provides a 2D LaserScan from the closest to 0-degree azimuth ring
+
+To construct a valid string for the `os1_proc_mask` parameter, join the tokens
+from above (in any combination) with the pipe character (`|`). For example,
+valid strings include but are not limited to: `IMG|PCL`, `IMG|PCL|IMU`, `PCL`,
+etc. The default value is `IMG|PCL|IMU|SCAN`.
+
+More details about data processors in the driver is provided in the [Additional
+Lidar Processing](#additional-lidar-processing) section below.
+
 ## Extensions
 
 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.

ros2_ouster/include/ros2_ouster/OS1/processor_factories.hpp

@@ -14,12 +14,14 @@
 #ifndef ROS2_OUSTER__OS1__PROCESSOR_FACTORIES_HPP_
 #define ROS2_OUSTER__OS1__PROCESSOR_FACTORIES_HPP_
 
+#include <cstdint>
 #include <string>
 #include <map>
 #include <utility>
 
 #include "rclcpp/qos.hpp"
 #include "ros2_ouster/conversions.hpp"
+#include "ros2_ouster/string_utils.hpp"
 #include "ros2_ouster/OS1/processors/image_processor.hpp"
 #include "ros2_ouster/OS1/processors/imu_processor.hpp"
 #include "ros2_ouster/OS1/processors/pointcloud_processor.hpp"
@@ -28,6 +30,49 @@
 namespace ros2_ouster
 {
 
+constexpr std::uint32_t OS1_PROC_IMG = (1 << 0);
+constexpr std::uint32_t OS1_PROC_PCL = (1 << 1);
+constexpr std::uint32_t OS1_PROC_IMU = (1 << 2);
+constexpr std::uint32_t OS1_PROC_SCAN = (1 << 3);
+
+constexpr std::uint32_t OS1_DEFAULT_PROC_MASK =
+  OS1_PROC_IMG | OS1_PROC_PCL | OS1_PROC_IMU | OS1_PROC_SCAN;
+
+/**
+ * Transforms a data processor mask-like-string into a mask value
+ *
+ * We define a mask-like-string to be a pipe-separated list of
+ * data processor suffixes. For example, all of the following
+ * are valid:
+ *
+ * IMG|PCL|IMU|SCAN
+ * IMG|PCL
+ * PCL
+ *
+ * @param[in] mask_str The string to convert into a mask
+ * @return The mask obtained from the parsed input string.
+ */
+inline std::uint32_t
+toProcMask(const std::string & mask_str)
+{
+  std::uint32_t mask = 0x0;
+  auto tokens = ros2_ouster::split(mask_str, '|');
+
+  for (auto & token : tokens) {
+    if (token == "IMG") {
+      mask |= ros2_ouster::OS1_PROC_IMG;
+    } else if (token == "PCL") {
+      mask |= ros2_ouster::OS1_PROC_PCL;
+    } else if (token == "IMU") {
+      mask |= ros2_ouster::OS1_PROC_IMU;
+    } else if (token == "SCAN") {
+      mask |= ros2_ouster::OS1_PROC_SCAN;
+    }
+  }
+
+  return mask;
+}
+
 /**
  * @brief Factory method to get a pointer to a processor
  * to create the image (range, intensity, noise) interfaces
@@ -89,22 +134,34 @@ inline std::multimap<ClientState, DataProcessorInterface *> createProcessors(
   const ros2_ouster::Metadata & mdata,
   const std::string & imu_frame,
   const std::string & laser_frame,
-  const rclcpp::QoS & qos)
+  const rclcpp::QoS & qos,
+  std::uint32_t mask = ros2_ouster::OS1_DEFAULT_PROC_MASK)
 {
   std::multimap<ClientState, DataProcessorInterface *> data_processors;
 
-  data_processors.insert(std::pair<ClientState, DataProcessorInterface *>(
-      ClientState::IMU_DATA, createIMUProcessor(
-        node, mdata, imu_frame, qos)));
-  data_processors.insert(std::pair<ClientState, DataProcessorInterface *>(
-      ClientState::LIDAR_DATA, createPointcloudProcessor(
-        node, mdata, laser_frame, qos)));
-  data_processors.insert(std::pair<ClientState, DataProcessorInterface *>(
-      ClientState::LIDAR_DATA, createImageProcessor(
-        node, mdata, laser_frame, qos)));
-  data_processors.insert(std::pair<ClientState, DataProcessorInterface *>(
-      ClientState::LIDAR_DATA, createScanProcessor(
-        node, mdata, laser_frame, qos)));
+  if ((mask & ros2_ouster::OS1_PROC_IMG) == ros2_ouster::OS1_PROC_IMG) {
+    data_processors.insert(std::pair<ClientState, DataProcessorInterface *>(
+        ClientState::LIDAR_DATA, createImageProcessor(
+          node, mdata, laser_frame, qos)));
+  }
+
+  if ((mask & ros2_ouster::OS1_PROC_PCL) == ros2_ouster::OS1_PROC_PCL) {
+    data_processors.insert(std::pair<ClientState, DataProcessorInterface *>(
+        ClientState::LIDAR_DATA, createPointcloudProcessor(
+          node, mdata, laser_frame, qos)));
+  }
+
+  if ((mask & ros2_ouster::OS1_PROC_IMU) == ros2_ouster::OS1_PROC_IMU) {
+    data_processors.insert(std::pair<ClientState, DataProcessorInterface *>(
+        ClientState::IMU_DATA, createIMUProcessor(
+          node, mdata, imu_frame, qos)));
+  }
+
+  if ((mask & ros2_ouster::OS1_PROC_SCAN) == ros2_ouster::OS1_PROC_SCAN) {
+    data_processors.insert(std::pair<ClientState, DataProcessorInterface *>(
+        ClientState::LIDAR_DATA, createScanProcessor(
+          node, mdata, laser_frame, qos)));
+  }
 
   return data_processors;
 }

ros2_ouster/include/ros2_ouster/ouster_driver.hpp

@@ -143,6 +143,8 @@ class OusterDriver : public lifecycle_interface::LifecycleInterface
 
   bool _use_system_default_qos;
   bool _use_ros_time;
+
+  std::uint32_t _os1_proc_mask;
 };
 
 }  // namespace ros2_ouster

ros2_ouster/include/ros2_ouster/string_utils.hpp

@@ -0,0 +1,92 @@
+// Copyright 2020, Box Robotics, Inc.
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef ROS2_OUSTER__STRING_UTILS_HPP_
+#define ROS2_OUSTER__STRING_UTILS_HPP_
+
+#include <sstream>
+#include <string>
+#include <vector>
+
+namespace ros2_ouster
+{
+
+/**
+ * Trims whitespace from the left side of a string
+ *
+ * @param str The string to trim
+ * @param white Whitespace characters to erase
+ *
+ * @return A reference to the trimmed string
+ */
+inline std::string &
+ltrim(std::string & str, const std::string & white = "\t\n\v\f\r ")
+{
+  str.erase(0, str.find_first_not_of(white));
+  return str;
+}
+
+/**
+ * Trims whitespace from the right side of a string
+ *
+ * @param str The string to trim
+ * @param white Whitespace characters to erase
+ *
+ * @return A reference to the trimmed string
+ */
+inline std::string &
+rtrim(std::string & str, const std::string & white = "\t\n\v\f\r ")
+{
+  str.erase(str.find_last_not_of(white) + 1);
+  return str;
+}
+
+/**
+ * Trims whitespace from the front and back of a string
+ *
+ * @param str The string to trim
+ * @param white Whitespace characters to erase
+ *
+ * @return A reference to the trimmed string
+ */
+inline std::string &
+trim(std::string & str, const std::string & white = "\t\n\v\f\r ")
+{
+  return ros2_ouster::ltrim(ros2_ouster::rtrim(str, white), white);
+}
+
+/**
+ * Tokenizes a string based on a char delimeter
+ *
+ * @param[in] in The string to tokenize
+ * @param[in] delim The char delimeter to split the string on
+ *
+ * @return A vector of tokens
+ */
+inline std::vector<std::string>
+split(const std::string & in, char delim)
+{
+  std::vector<std::string> tokens;
+  std::string token;
+  std::istringstream ss(in);
+
+  while (std::getline(ss, token, delim)) {
+    tokens.push_back(token);
+  }
+
+  return tokens;
+}
+
+}  // namespace ros2_ouster
+
+#endif  // ROS2_OUSTER__STRING_UTILS_HPP_

ros2_ouster/params/os1.yaml

@@ -26,3 +26,27 @@ ouster_driver:
     # information).
     #
     timestamp_mode: TIME_FROM_INTERNAL_OSC
+
+    # Mask-like-string used to define the data processors that should be
+    # activated upon startup of the driver. This will determine the topics
+    # that are available for client applications to consume. The defacto
+    # reference for these values are defined in:
+    # `include/ros2_ouster/OS1/processor_factories.hpp`
+    #
+    # For convenience, the available data processors are:
+    #
+    # IMG   - Provides 8-bit image topics suitable for ML applications encoding
+    #         the noise, range, intensity, and reflectivity data from a scan
+    # PCL   - Provides a point cloud encoding of a LiDAR scan
+    # IMU   - Provides a data stream from the LiDARs integral IMU
+    # SCAN  - Provides a synthesized 2D LaserScan from the 3D LiDAR data
+    #
+    # To construct a valid string for this parameter join the tokens from above
+    # (in any combination) with the pipe character. For example, valid strings
+    # include (but are not limited to):
+    #
+    # IMG|PCL
+    # IMG|PCL|IMU|SCAN
+    # PCL
+    #
+    os1_proc_mask: IMG|PCL|IMU|SCAN

ros2_ouster/src/ouster_driver.cpp

@@ -48,6 +48,7 @@ OusterDriver::OusterDriver(
   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);
+  this->declare_parameter("os1_proc_mask", std::string("IMG|PCL|IMU|SCAN"));
 }
 
 OusterDriver::~OusterDriver() = default;
@@ -83,6 +84,9 @@ void OusterDriver::onConfigure()
   _imu_data_frame = get_parameter("imu_frame").as_string();
   _use_system_default_qos = get_parameter("use_system_default_qos").as_bool();
 
+  _os1_proc_mask =
+    ros2_ouster::toProcMask(get_parameter("os1_proc_mask").as_string());
+
   RCLCPP_INFO(this->get_logger(),
     "Connecting to sensor at %s.", lidar_config.lidar_ip.c_str());
   RCLCPP_INFO(this->get_logger(),
@@ -107,11 +111,11 @@ void OusterDriver::onConfigure()
       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());
+      rclcpp::SystemDefaultsQoS(), _os1_proc_mask);
   } else {
     _data_processors = ros2_ouster::createProcessors(
       shared_from_this(), mdata, _imu_data_frame, _laser_data_frame,
-      rclcpp::SensorDataQoS());
+      rclcpp::SensorDataQoS(), _os1_proc_mask);
   }
 
   _tf_b = std::make_unique<tf2_ros::StaticTransformBroadcaster>(