SW-5466: Support Velodyne and other point types in ouster-ros driver

ouster-ros/CMakeLists.txt

@@ -154,7 +154,7 @@ rclcpp_components_register_node(os_image_component
   EXECUTABLE os_image
 )
 
-# ==== os_sensor_component ====
+# ==== os_driver_component ====
 create_ros2_component(os_driver_component
   "src/os_sensor_node_base.cpp;src/os_sensor_node.cpp;src/os_driver_node.cpp"
   "std_srvs"
@@ -168,11 +168,22 @@ rclcpp_components_register_node(os_driver_component
 # ==== Test ====
 if(BUILD_TESTING)
   find_package(ament_cmake_gtest REQUIRED)
-  ament_add_gtest(${PROJECT_NAME}_test test/ring_buffer_test.cpp)
+  ament_add_gtest(${PROJECT_NAME}_test
+    test/ring_buffer_test.cpp
+    src/os_ros.cpp
+    test/point_accessor_test.cpp
+    test/point_transform_test.cpp
+  )
+  ament_target_dependencies(${PROJECT_NAME}_test
+    rclcpp
+    ouster_msgs
+  )
   target_include_directories(${PROJECT_NAME}_test PUBLIC
+    ${_ouster_ros_INCLUDE_DIRS}
     $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
     $<INSTALL_INTERFACE:include>
   )
+  target_link_libraries(${PROJECT_NAME}_test ouster_ros_library)
 endif()
 
 

ouster-ros/config/driver_params.yaml

@@ -67,3 +67,18 @@ ouster/os_driver:
     # 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
+     # point_type[optional]: choose from: {original, native, xyz, xyzi, xyzir}
+     # Here is a breif description of each option:
+     #  - original: ouster_ros::Point
+     #  - native: directly maps all fields as published by the sensor to an
+     #          equivalent point cloud representation with the additon of ring
+     #          and timestamp fields.
+     #  - xyz: the simplest point type, only has {x, y, z}
+     #  - xyzi: same as xyz point type but adds intensity(signal) field. this
+     #          type is not compatible with the low data profile.
+     #  - xyzi: same as xyzi type but adds ring(channel) field.
+     #          this type is same as Velodyne point cloud type
+     #          this type is not compatible with the low data profile.
+     # for more details about the fields of each point type and their data refer
+     # to ouster_ros/os_point.h header.
+    point_type: original

ouster-ros/config/os_sensor_cloud_image_params.yaml

@@ -27,5 +27,6 @@ ouster/os_cloud:
     use_system_default_qos: False # needs to match the value defined for os_sensor node
     scan_ring: 0  # Use this parameter in conjunction with the SCAN flag and choose a
                   # value the range [0, sensor_beams_count)
+    point_type: original # choose from: {original, native, xyz, xyzi, xyzir}
 ouster/os_image:
     use_system_default_qos: False # needs to match the value defined for os_sensor node

ouster-ros/include/ouster_ros/common_point_types.h

@@ -0,0 +1,72 @@
+/**
+ * Copyright (c) 2018-2023, Ouster, Inc.
+ * All rights reserved.
+ *
+ * @file common_point_types.h
+ * @brief common PCL point datatype for use with ouster sensors
+ */
+
+#pragma once
+
+#include <pcl/point_types.h>
+
+namespace ouster_ros {
+
+/* The following are pcl point representations that are common/standard point
+   representation that we readily support.
+ */
+
+/*
+ * Same as Velodyne point cloud type
+ * @remark XYZIR point type is not compatible with RNG15_RFL8_NIR8/LOW_DATA
+ * udp lidar profile.
+ */
+struct EIGEN_ALIGN16 _PointXYZIR {
+    PCL_ADD_POINT4D;
+    float intensity;
+    uint16_t ring;
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+struct PointXYZIR : public _PointXYZIR {
+
+    inline PointXYZIR(const _PointXYZIR& pt)
+    {
+      x = pt.x; y = pt.y; z = pt.z; data[3] = 1.0f;
+      intensity = pt.intensity; ring = pt.ring;
+    }
+
+    inline PointXYZIR()
+    {
+      x = y = z = 0.0f; data[3] = 1.0f;
+      intensity = 0.0f; ring = 0;
+    }
+
+    inline const auto as_tuple() const {
+        return std::tie(x, y, z, intensity, ring);
+    }
+
+    inline auto as_tuple() {
+        return std::tie(x, y, z, intensity, ring);
+    }
+
+    template<size_t I>
+    inline auto& get() {
+        return std::get<I>(as_tuple());
+    }
+};
+
+}   // namespace ouster_ros
+
+// clang-format off
+
+/* common point types */
+POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_ros::PointXYZIR,
+    (float, x, x)
+    (float, y, y)
+    (float, z, z)
+    (float, intensity, intensity)
+    (std::uint16_t, ring, ring)
+)
+
+// clang-format on

ouster-ros/include/ouster_ros/os_point.h

@@ -10,36 +10,79 @@
 
 #include <pcl/point_types.h>
 
-#include <Eigen/Core>
-#include <chrono>
-
-#include <ouster/lidar_scan.h>
-
 namespace ouster_ros {
 
-struct EIGEN_ALIGN16 Point {
+// The default/original represntation of the point cloud since the driver
+// inception. This shouldn't be confused with Point_LEGACY which provides exact
+// mapping of the fields of Ouster LidarScan of the Legacy Profile, copying the 
+// the same order and using the same bit representation. For example, this Point
+// struct uses float data type to represent intensity (aka signal); however, the
+// sensor sends the signal channel either as UINT16 or UINT32 depending on the
+// active udp lidar profile.
+struct EIGEN_ALIGN16 _Point {
     PCL_ADD_POINT4D;
-    float intensity;
+    float intensity;        // equivalent signal
     uint32_t t;
     uint16_t reflectivity;
-    uint16_t ring;
-    uint16_t ambient;
+    uint16_t ring;          // equivalent to channel
+    uint16_t ambient;       // equivalent to near_ir
     uint32_t range;
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
-}  // namespace ouster_ros
+
+struct Point : public _Point {
+
+    inline Point(const _Point& pt)
+    {
+      x = pt.x; y = pt.y; z = pt.z; data[3] = 1.0f;
+      intensity = pt.intensity;
+      t = pt.t;
+      reflectivity = pt.reflectivity; 
+      ring = pt.ring;
+      ambient = pt.ambient;
+      range = pt.range;
+    }
+
+    inline Point()
+    {
+      x = y = z = 0.0f; data[3] = 1.0f;
+      intensity = 0.0f;
+      t = 0;
+      reflectivity = 0;
+      ring = 0;
+      ambient = 0;
+      range = 0;
+    }
+
+    inline const auto as_tuple() const {
+        return std::tie(x, y, z, intensity, t, reflectivity, ring, ambient, range);
+    }
+
+    inline auto as_tuple() {
+        return std::tie(x, y, z, intensity, t, reflectivity, ring, ambient, range);
+    }
+
+    template<size_t I>
+    inline auto& get() {
+        return std::get<I>(as_tuple());
+    }
+};
+
+}   // namespace ouster_ros
 
 // clang-format off
+
+// DEFAULT/ORIGINAL
 POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_ros::Point,
     (float, x, x)
     (float, y, y)
     (float, z, z)
     (float, intensity, intensity)
-    // use std::uint32_t to avoid conflicting with pcl::uint32_t
     (std::uint32_t, t, t)
     (std::uint16_t, reflectivity, reflectivity)
     (std::uint16_t, ring, ring)
     (std::uint16_t, ambient, ambient)
     (std::uint32_t, range, range)
 )
+
 // clang-format on

ouster-ros/include/ouster_ros/os_ros.h

@@ -19,20 +19,16 @@
 #include <pcl/point_types.h>
 
 #include <sensor_msgs/msg/imu.hpp>
-#include <sensor_msgs/msg/point_cloud2.hpp>
 #include <sensor_msgs/msg/laser_scan.hpp>
 
 #include <chrono>
 #include <string>
 
 #include "ouster_msgs/msg/packet_msg.hpp"
-#include "ouster_ros/os_point.h"
 
 namespace ouster_ros {
 
 namespace sensor = ouster::sensor;
-using Cloud = pcl::PointCloud<Point>;
-using ns = std::chrono::nanoseconds;
 
 /**
  * Checks sensor_info if it currently represents a legacy udp lidar profile
@@ -90,74 +86,6 @@ sensor_msgs::msg::Imu packet_to_imu_msg(const ouster_msgs::msg::PacketMsg& pm,
                                         const std::string& frame,
                                         const sensor::packet_format& pf);
 
-/**
- * Populate a PCL point cloud from a LidarScan.
- * @param[in, out] points The points parameters is used to store the results of
- * the cartesian product before it gets packed into the cloud object.
- * @param[in] lut_direction the direction of the xyz lut (with single precision)
- * @param[in] lut_offset the offset of the xyz lut (with single precision)
- * @param[in] scan_ts scan start used to caluclate relative timestamps for
- * points.
- * @param[in] lidar_scan input lidar data
- * @param[out] cloud output pcl pointcloud to populate
- * @param[in] return_index index of return desired starting at 0
- */
-[[deprecated("use the 2nd version of scan_to_cloud_f")]] void scan_to_cloud_f(
-    ouster::PointsF& points, const ouster::PointsF& lut_direction,
-    const ouster::PointsF& lut_offset, std::chrono::nanoseconds scan_ts,
-    const ouster::LidarScan& lidar_scan, ouster_ros::Cloud& cloud,
-    int return_index);
-
-/**
- * Populate a PCL point cloud from a LidarScan.
- * @param[in, out] points The points parameters is used to store the results of
- * the cartesian product before it gets packed into the cloud object.
- * @param[in] lut_direction the direction of the xyz lut (with single precision)
- * @param[in] lut_offset the offset of the xyz lut (with single precision)
- * @param[in] scan_ts scan start used to caluclate relative timestamps for
- * points
- * @param[in] lidar_scan input lidar data
- * @param[out] cloud output pcl pointcloud to populate
- * @param[in] return_index index of return desired starting at 0
- */
-void scan_to_cloud_f(ouster::PointsF& points,
-                     const ouster::PointsF& lut_direction,
-                     const ouster::PointsF& lut_offset, uint64_t scan_ts,
-                     const ouster::LidarScan& lidar_scan,
-                     ouster_ros::Cloud& cloud, int return_index);
-
-/**
- * Populate a destaggered PCL point cloud from a LidarScan
- * @param[out] cloud output pcl pointcloud to populate
- * @param[in, out] points The points parameters is used to store the results of
- * the cartesian product before it gets packed into the cloud object.
- * @param[in] lut_direction the direction of the xyz lut (with single precision)
- * @param[in] lut_offset the offset of the xyz lut (with single precision)
- * @param[in] scan_ts scan start used to caluclate relative timestamps for
- * points
- * @param[in] lidar_scan input lidar data
- * @param[in] pixel_shift_by_row pixel shifts by row
- * @param[in] return_index index of return desired starting at 0
- */
-void scan_to_cloud_f_destaggered(ouster_ros::Cloud& cloud,
-                     ouster::PointsF& points,
-                     const ouster::PointsF& lut_direction,
-                     const ouster::PointsF& lut_offset, uint64_t scan_ts,
-                     const ouster::LidarScan& ls,
-                     const std::vector<int>& pixel_shift_by_row,
-                     int return_index);
-
-/**
- * Serialize a PCL point cloud to a ROS message
- * @param[in] cloud the PCL point cloud to convert
- * @param[in] timestamp the timestamp to apply to the resulting ROS message
- * @param[in] frame the frame to set in the resulting ROS message
- * @return a ROS message containing the point cloud
- */
-sensor_msgs::msg::PointCloud2 cloud_to_cloud_msg(const Cloud& cloud,
-                                                 const rclcpp::Time& timestamp,
-                                                 const std::string& frame);
-
 /**
  * Convert transformation matrix return by sensor to ROS transform
  * @param[in] mat transformation matrix return by sensor
@@ -203,10 +131,7 @@ struct read_and_cast {
 template <typename T>
 inline ouster::img_t<T> get_or_fill_zero(sensor::ChanField field,
                                          const ouster::LidarScan& ls) {
-    if (!ls.field_type(field)) {
-        return ouster::img_t<T>::Zero(ls.h, ls.w);
-    }
-
+    if (!ls.field_type(field)) return ouster::img_t<T>::Zero(ls.h, ls.w);
     ouster::img_t<T> result{ls.h, ls.w};
     ouster::impl::visit_field(ls, field, read_and_cast(), result);
     return result;
@@ -221,6 +146,13 @@ inline uint64_t ts_safe_offset_add(uint64_t ts, int64_t offset) {
     return offset < 0 && ts < static_cast<uint64_t>(std::abs(offset)) ? 0 : ts + offset;
 }
 
+std::set<std::string> parse_tokens(const std::string& input, char delim);
+
+inline bool check_token(const std::set<std::string>& tokens,
+                        const std::string& token) {
+    return tokens.find(token) != tokens.end();
+}
+
 
 } // namespace impl