Support FUSA dual returns udp profile [HUMBLE/IRON] (#335)

* Add support for FUSA profile + set xyz to NaNs on zero range

CHANGELOG.rst

@@ -9,6 +9,8 @@ Changelog
 * [BUGFIX]: LaserScan does not work when using dual mode
 * [BUGFIX]: ROS2 crashes when standby mode is set and then set to normal
 * [BUGFIX]: Implement lock free ring buffer with throttling to reduce partial frames
+* add support for FUSA udp profile ``FUSA_RNG15_RFL8_NIR8_DUAL``.
+* [BREAKING]: Set xyz values of individual points in the PointCloud to NaNs when range is zero.
 
 
 ouster_ros v0.12.0

ouster-ros/config/driver_params.yaml

@@ -31,9 +31,10 @@ ouster/os_driver:
     ptp_utc_tai_offset: -37.0
     # udp_profile_lidar[optional]: lidar packet profile; possible values:
     # - LEGACY: not recommended
-    # - RNG19_RFL8_SIG16_NIR16_DUAL
     # - RNG19_RFL8_SIG16_NIR16
     # - RNG15_RFL8_NIR8
+    # - RNG19_RFL8_SIG16_NIR16_DUAL
+    # - FUSA_RNG15_RFL8_NIR8_DUAL
     udp_profile_lidar: ''
     # metadata[optional]: path to save metadata file to, if left empty a file
     # with the sensor hostname or ip will be crearted in ~/.ros folder.

ouster-ros/include/ouster_ros/sensor_point_types.h

@@ -46,7 +46,7 @@ static constexpr ChanFieldTable<4> Profile_LEGACY{{
 // auto=LEGACY
 struct EIGEN_ALIGN16 _Point_LEGACY {
     PCL_ADD_POINT4D;
-    uint32_t t;             // timestamp relative to frame
+    uint32_t t;             // timestamp in nanoseconds relative to frame start
     uint16_t ring;          // equivalent to channel
     uint32_t range;
     uint32_t signal;        // equivalent to intensity
@@ -133,7 +133,7 @@ static constexpr ChanFieldTable<4> Profile_RNG19_RFL8_SIG16_NIR16_DUAL_2ND_RETUR
 // auto=RNG19_RFL8_SIG16_NIR16_DUAL
 struct EIGEN_ALIGN16 _Point_RNG19_RFL8_SIG16_NIR16_DUAL {
     PCL_ADD_POINT4D;
-    uint32_t t;             // timestamp relative to frame start time
+    uint32_t t;             // timestamp in nanoseconds relative to frame start
     uint16_t ring;          // equivalent channel
     uint32_t range;
     uint16_t signal;        // equivalent to intensity
@@ -205,7 +205,7 @@ static constexpr ChanFieldTable<4> Profile_RNG19_RFL8_SIG16_NIR16{{
 // auto=RNG19_RFL8_SIG16_NIR16
 struct EIGEN_ALIGN16 _Point_RNG19_RFL8_SIG16_NIR16 {
     PCL_ADD_POINT4D;
-    uint32_t t;             // timestamp relative to frame start time
+    uint32_t t;             // timestamp in nanoseconds relative to frame start
     uint16_t ring;          // equivalent channel
     uint32_t range;
     uint16_t signal;        // equivalent to intensity
@@ -277,7 +277,7 @@ static constexpr ChanFieldTable<3> Profile_RNG15_RFL8_NIR8{{
 struct EIGEN_ALIGN16 _Point_RNG15_RFL8_NIR8 {
     PCL_ADD_POINT4D;
     // No signal/intensity in low data mode
-    uint32_t t;             // timestamp relative to frame
+    uint32_t t;             // timestamp in nanoseconds relative to frame start
     uint16_t ring;          // equivalent to channel
     uint32_t range;
     uint16_t reflectivity;
@@ -334,3 +334,91 @@ POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_ros::Point_RNG15_RFL8_NIR8,
 )
 
 // clang-format on
+
+
+namespace ouster_ros {
+
+// Profile_FUSA_RNG15_RFL8_NIR8_DUAL: aka fusa dual returns
+// This profile is definied differently from PROFILE_FUSA_RNG15_RFL8_NIR8_DUAL of how
+// the sensor actually sends the data. The actual PROFILE_FUSA_RNG15_RFL8_NIR8_DUAL
+// has 5 fields not 3, but this profile is defined differently in ROS because
+// we build and publish a point cloud for each return separately. However, it
+// might be desireable to some of the users to choose a point cloud
+// representation which combines parts of the the two or more returns. This isn't
+// something that the current framework could deal with as of now.
+static constexpr ChanFieldTable<3> Profile_FUSA_RNG15_RFL8_NIR8_DUAL {{
+    {sensor::ChanField::RANGE, sensor::ChanFieldType::UINT32},
+    {sensor::ChanField::REFLECTIVITY, sensor::ChanFieldType::UINT8},
+    {sensor::ChanField::NEAR_IR, sensor::ChanFieldType::UINT16},
+}};
+
+// Note: this is one way to implement the processing of 2nd return
+// This should be an exact copy of Profile_FUSA_RNG15_RFL8_NIR8_DUAL with the
+// exception of ChanField values for the first three fields. NEAR_IR is same for both
+static constexpr ChanFieldTable<3> Profile_FUSA_RNG15_RFL8_NIR8_DUAL_2ND_RETURN {{
+    {sensor::ChanField::RANGE2, sensor::ChanFieldType::UINT32},
+    {sensor::ChanField::REFLECTIVITY2, sensor::ChanFieldType::UINT8},
+    {sensor::ChanField::NEAR_IR, sensor::ChanFieldType::UINT16},
+}};
+
+// auto=RNG19_RFL8_SIG16_NIR16_DUAL
+struct EIGEN_ALIGN16 _Point_FUSA_RNG15_RFL8_NIR8_DUAL {
+    PCL_ADD_POINT4D;
+    uint32_t t;             // timestamp in nanoseconds relative to frame start
+    uint16_t ring;          // equivalent to channel
+    uint32_t range;
+    uint8_t reflectivity;
+    uint16_t near_ir;       // equivalent to ambient
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+struct Point_FUSA_RNG15_RFL8_NIR8_DUAL : public _Point_FUSA_RNG15_RFL8_NIR8_DUAL {
+
+    inline Point_FUSA_RNG15_RFL8_NIR8_DUAL(const _Point_FUSA_RNG15_RFL8_NIR8_DUAL& pt)
+    {
+      x = pt.x; y = pt.y; z = pt.z; data[3] = 1.0f;
+      t = pt.t; ring = pt.ring;
+      range = pt.range;
+      reflectivity = pt.reflectivity;
+      near_ir = pt.near_ir;
+    }
+
+    inline Point_FUSA_RNG15_RFL8_NIR8_DUAL()
+    {
+      x = y = z = 0.0f; data[3] = 1.0f;
+      t = 0; ring = 0;
+      range = 0;
+      reflectivity = 0;
+      near_ir = 0;
+    }
+
+    inline const auto as_tuple() const {
+        return std::tie(x, y, z, t, ring, range, reflectivity, near_ir);
+    }
+
+    inline auto as_tuple() {
+        return std::tie(x, y, z, t, ring, range, reflectivity, near_ir);
+    }
+
+    template<size_t I>
+    inline auto& get() {
+        return std::get<I>(as_tuple());
+    }
+};
+
+}   // namespce ouster_ros
+
+// clang-format off
+
+POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_ros::Point_FUSA_RNG15_RFL8_NIR8_DUAL,
+    (float, x, x)
+    (float, y, y)
+    (float, z, z)
+    (std::uint32_t, t, t)
+    (std::uint16_t, ring, ring)
+    (std::uint32_t, range, range)
+    (std::uint8_t, reflectivity, reflectivity)
+    (std::uint16_t, near_ir, near_ir)
+)
+
+// clang-format on

ouster-ros/launch/record.composite.launch.xml

@@ -13,9 +13,10 @@
   <arg name="udp_profile_lidar" default=""
     description="lidar packet profile; possible values: {
     LEGACY,
-    RNG19_RFL8_SIG16_NIR16_DUAL,
     RNG19_RFL8_SIG16_NIR16,
-    RNG15_RFL8_NIR8
+    RNG15_RFL8_NIR8,
+    RNG19_RFL8_SIG16_NIR16_DUAL,
+    FUSA_RNG15_RFL8_NIR8_DUAL
     }"/>
   <arg name="lidar_mode" default=""
     description="resolution and rate; possible values: {

ouster-ros/launch/sensor.composite.launch.xml

@@ -13,9 +13,10 @@
   <arg name="udp_profile_lidar" default=""
     description="lidar packet profile; possible values: {
     LEGACY,
-    RNG19_RFL8_SIG16_NIR16_DUAL,
     RNG19_RFL8_SIG16_NIR16,
-    RNG15_RFL8_NIR8
+    RNG15_RFL8_NIR8,
+    RNG19_RFL8_SIG16_NIR16_DUAL,
+    FUSA_RNG15_RFL8_NIR8_DUAL
     }"/>
   <arg name="lidar_mode" default=""
     description="resolution and rate; possible values: {

ouster-ros/launch/sensor.independent.launch.xml

@@ -13,9 +13,10 @@
   <arg name="udp_profile_lidar" default=""
     description="lidar packet profile; possible values: {
     LEGACY,
-    RNG19_RFL8_SIG16_NIR16_DUAL,
     RNG19_RFL8_SIG16_NIR16,
-    RNG15_RFL8_NIR8
+    RNG15_RFL8_NIR8,
+    RNG19_RFL8_SIG16_NIR16_DUAL,
+    FUSA_RNG15_RFL8_NIR8_DUAL    
     }"/>
   <arg name="lidar_mode" default=""
     description="resolution and rate; possible values: {

ouster-ros/launch/sensor_mtp.launch.xml

@@ -21,9 +21,10 @@
   <arg name="udp_profile_lidar" default=""
     description="lidar packet profile; possible values: {
     LEGACY,
-    RNG19_RFL8_SIG16_NIR16_DUAL,
     RNG19_RFL8_SIG16_NIR16,
-    RNG15_RFL8_NIR8
+    RNG15_RFL8_NIR8,
+    RNG19_RFL8_SIG16_NIR16_DUAL,
+    FUSA_RNG15_RFL8_NIR8_DUAL    
     }"/>
   <arg name="lidar_mode" default=""
     description="resolution and rate; possible values: {

ouster-ros/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>ouster_ros</name>
-  <version>0.12.3</version>
+  <version>0.12.4</version>
   <description>Ouster ROS2 driver</description>
   <maintainer email="oss@ouster.io">ouster developers</maintainer>
   <license file="LICENSE">BSD</license>

ouster-ros/src/impl/cartesian.h

@@ -0,0 +1,64 @@
+#pragma once
+
+#include <ouster/lidar_scan.h>
+
+namespace ouster {
+
+// TODO: move to the sdk client
+
+/**
+ * This is the same function as the cartesianT method defined in the client but
+ * allows the user choose a specific value for range values of zero.
+ *
+ * @param[in, out] points The resulting point cloud, should be pre-allocated and
+ * have the same dimensions as the direction array.
+ * @param[in] range a range image in the same format as the RANGE field of a
+ * LidarScan.
+ * @param[in] direction the direction of an xyz lut.
+ * @param[in] offset the offset of an xyz lut.
+ * @param[in] invalid the value to assign of an xyz lut.
+ *
+ * @return Cartesian points where ith row is a 3D point which corresponds
+ *         to ith pixel in LidarScan where i = row * w + col.
+ */
+template <typename T>
+void cartesianT(PointsT<T>& points,
+                const Eigen::Ref<const img_t<uint32_t>>& range,
+                const PointsT<T>& direction, const PointsT<T>& offset,
+                T invalid) {
+    assert(points.rows() == direction.rows() &&
+           "points & direction row count mismatch");
+    assert(points.rows() == offset.rows() &&
+           "points & offset row count mismatch");
+    assert(points.rows() == range.size() &&
+           "points and range image size mismatch");
+
+    const auto pts = points.data();
+    const auto* const rng = range.data();
+    const auto* const dir = direction.data();
+    const auto* const ofs = offset.data();
+
+    const auto N = range.size();
+    const auto col_x = 0 * N;  // 1st column of points (x)
+    const auto col_y = 1 * N;  // 2nd column of points (y)
+    const auto col_z = 2 * N;  // 3rd column of points (z)
+
+#ifdef __OUSTER_UTILIZE_OPENMP__
+#pragma omp parallel for schedule(static)
+#endif
+    for (auto i = 0; i < N; ++i) {
+        const auto r = rng[i];
+        const auto idx_x = col_x + i;
+        const auto idx_y = col_y + i;
+        const auto idx_z = col_z + i;
+        if (r == 0) {
+            pts[idx_x] = pts[idx_y] = pts[idx_z] = invalid;
+        } else {
+            pts[idx_x] = r * dir[idx_x] + ofs[idx_x];
+            pts[idx_y] = r * dir[idx_y] + ofs[idx_y];
+            pts[idx_z] = r * dir[idx_z] + ofs[idx_z];
+        }
+    }
+}
+
+}  // namespace ouster

ouster-ros/src/os_cloud_node.cpp

@@ -122,9 +122,12 @@ class OusterCloud : public OusterProcessingNodeBase {
 
             // warn about profile incompatibility
             if (PointCloudProcessorFactory::point_type_requires_intensity(point_type) &&
-                info.format.udp_profile_lidar == UDPProfileLidar::PROFILE_RNG15_RFL8_NIR8) {
-                RCLCPP_WARN_STREAM(get_logger(),
-                    "selected point type '" << point_type << "' is not compatible with the current udp profile: RNG15_RFL8_NIR8");
+                !PointCloudProcessorFactory::profile_has_intensity(info.format.udp_profile_lidar)) {
+                RCLCPP_WARN_STREAM(
+                    get_logger(),
+                    "selected point type '" << point_type
+                    << "' is not compatible with the udp profile: "
+                    << to_string(info.format.udp_profile_lidar));
             }
         }
 

ouster-ros/src/os_driver_node.cpp

@@ -95,9 +95,12 @@ class OusterDriver : public OusterSensor {
 
             // warn about profile incompatibility
             if (PointCloudProcessorFactory::point_type_requires_intensity(point_type) &&
-                info.format.udp_profile_lidar == UDPProfileLidar::PROFILE_RNG15_RFL8_NIR8) {
-                RCLCPP_WARN_STREAM(get_logger(),
-                    "selected point type '" << point_type << "' is not compatible with the current udp profile: RNG15_RFL8_NIR8");
+                !PointCloudProcessorFactory::profile_has_intensity(info.format.udp_profile_lidar)) {
+                RCLCPP_WARN_STREAM(
+                    get_logger(),
+                    "selected point type '" << point_type
+                    << "' is not compatible with the udp profile: "
+                    << to_string(info.format.udp_profile_lidar));
             }
         }
 

ouster-ros/src/os_ros.cpp

@@ -37,10 +37,11 @@ bool is_legacy_lidar_profile(const sensor::sensor_info& info) {
 
 int get_n_returns(const sensor::sensor_info& info) {
     using sensor::UDPProfileLidar;
-    return info.format.udp_profile_lidar ==
-                   UDPProfileLidar::PROFILE_RNG19_RFL8_SIG16_NIR16_DUAL
-               ? 2
-               : 1;
+    if (info.format.udp_profile_lidar == UDPProfileLidar::PROFILE_RNG19_RFL8_SIG16_NIR16_DUAL ||
+        info.format.udp_profile_lidar == UDPProfileLidar::PROFILE_FUSA_RNG15_RFL8_NIR8_DUAL)
+        return 2;
+
+    return 1;
 }
 
 size_t get_beams_count(const sensor::sensor_info& info) {

ouster-ros/src/point_cloud_processor.h

@@ -16,6 +16,7 @@
 
 #include "point_cloud_compose.h"
 #include "lidar_packet_handler.h"
+#include "impl/cartesian.h"
 
 namespace ouster_ros {
 
@@ -78,7 +79,8 @@ class PointCloudProcessor {
         for (int i = 0; i < static_cast<int>(pc_msgs.size()); ++i) {
             auto range_channel = static_cast<sensor::ChanField>(sensor::ChanField::RANGE + i);
             auto range = lidar_scan.field<uint32_t>(range_channel);
-            ouster::cartesianT(points, range, lut_direction, lut_offset);
+            ouster::cartesianT(points, range, lut_direction, lut_offset,
+                               std::numeric_limits<float>::quiet_NaN());
 
             scan_to_cloud_fn(cloud, points, scan_ts, lidar_scan,
                                         pixel_shift_by_row, i);

ouster-ros/src/point_cloud_processor_factory.h

@@ -78,6 +78,29 @@ class PointCloudProcessorFactory {
                         pixel_shift_by_row);
                 };
 
+            case UDPProfileLidar::PROFILE_FUSA_RNG15_RFL8_NIR8_DUAL:
+                return [](ouster_ros::Cloud<PointT>& cloud,
+                          const ouster::PointsF& points, uint64_t scan_ts,
+                          const ouster::LidarScan& ls,
+                          const std::vector<int>& pixel_shift_by_row,
+                          int return_index) {
+                    Point_FUSA_RNG15_RFL8_NIR8_DUAL staging_pt;
+                    if (return_index == 0) {
+                        scan_to_cloud_f_destaggered<
+                            Profile_FUSA_RNG15_RFL8_NIR8_DUAL.size(),
+                            Profile_FUSA_RNG15_RFL8_NIR8_DUAL>(
+                            cloud, staging_pt, points, scan_ts, ls,
+                            pixel_shift_by_row);
+                    } else {
+                        scan_to_cloud_f_destaggered<
+                            Profile_FUSA_RNG15_RFL8_NIR8_DUAL_2ND_RETURN
+                                .size(),
+                            Profile_FUSA_RNG15_RFL8_NIR8_DUAL_2ND_RETURN>(
+                            cloud, staging_pt, points, scan_ts, ls,
+                            pixel_shift_by_row);
+                    }
+                };
+
             default:
                 throw std::runtime_error("unsupported udp_profile_lidar");
         }
@@ -100,6 +123,12 @@ class PointCloudProcessorFactory {
                point_type == "original";
     }
 
+    static bool profile_has_intensity(UDPProfileLidar profile) {
+        return profile == UDPProfileLidar::PROFILE_LIDAR_LEGACY ||
+               profile == UDPProfileLidar::PROFILE_RNG19_RFL8_SIG16_NIR16_DUAL ||
+               profile == UDPProfileLidar::PROFILE_RNG19_RFL8_SIG16_NIR16;
+    }
+
     static LidarScanProcessor create_point_cloud_processor(
         const std::string& point_type, const sensor::sensor_info& info,
         const std::string& frame, bool apply_lidar_to_sensor_transform,
@@ -124,6 +153,11 @@ class PointCloudProcessorFactory {
                     return make_point_cloud_processor<Point_RNG15_RFL8_NIR8>(
                         info, frame, apply_lidar_to_sensor_transform,
                         post_processing_fn);
+                case UDPProfileLidar::PROFILE_FUSA_RNG15_RFL8_NIR8_DUAL:
+                    return make_point_cloud_processor<
+                        Point_FUSA_RNG15_RFL8_NIR8_DUAL>(
+                        info, frame, apply_lidar_to_sensor_transform,
+                        post_processing_fn);
                 default:
                     // TODO: implement fallback?
                     throw std::runtime_error("unsupported udp_profile_lidar");