Support FUSA dual returns udp profile [NOETIC] (#334)

* Add support for FUSA profile in the driver
* Set xyz values of individual points in the PointCloud to NaNs when range is zero
* Mark the change for xyz changed to NaNs as breaking
* mention the timestamp unit

CHANGELOG.rst

@@ -12,7 +12,9 @@ Changelog
 * [BUGFIX]: LaserScan is not properly aligned with generated point cloud
   * address an issue where LaserScan appeared different on FW prior to 2.4
 * [BUGFIX]: LaserScan does not work when using dual mode
-* [BUGFIX]: Implement lock free ring buffer with throttling
+* [BUGFIX]: Implement lock free ring buffer with throttling to avoid generating 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.10.0

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

launch/driver.launch

@@ -7,9 +7,10 @@
   <arg name="imu_port" default="0" doc="port to which the sensor should send imu data"/>
   <arg name="udp_profile_lidar" default=" " doc="lidar packet profile; possible values: {
     LEGACY,
-    RNG19_RFL8_SIG16_NIR16_DUAL,
     RNG19_RFL8_SIG16_NIR16,
     RNG15_RFL8_NIR8
+    RNG19_RFL8_SIG16_NIR16_DUAL,
+    FUSA_RNG15_RFL8_NIR8_DUAL
     }"/>
   <arg name="lidar_mode" default=" " doc="resolution and rate; possible values: {
     512x10,

launch/record.launch

@@ -7,9 +7,10 @@
   <arg name="imu_port" default="0" doc="port to which the sensor should send imu data"/>
   <arg name="udp_profile_lidar" default=" " doc="lidar packet profile; possible values: {
     LEGACY,
-    RNG19_RFL8_SIG16_NIR16_DUAL,
     RNG19_RFL8_SIG16_NIR16,
     RNG15_RFL8_NIR8
+    RNG19_RFL8_SIG16_NIR16_DUAL,
+    FUSA_RNG15_RFL8_NIR8_DUAL
     }"/>
   <arg name="lidar_mode" default=" " doc="resolution and rate; possible values: {
     512x10,

launch/sensor.launch

@@ -7,9 +7,10 @@
   <arg name="imu_port" default="0" doc="port to which the sensor should send imu data"/>
   <arg name="udp_profile_lidar" default=" " doc="lidar packet profile; possible values: {
     LEGACY,
-    RNG19_RFL8_SIG16_NIR16_DUAL,
     RNG19_RFL8_SIG16_NIR16,
     RNG15_RFL8_NIR8
+    RNG19_RFL8_SIG16_NIR16_DUAL,
+    FUSA_RNG15_RFL8_NIR8_DUAL
     }"/>
   <arg name="lidar_mode" default=" " doc="resolution and rate; possible values: {
     512x10,

launch/sensor_mtp.launch

@@ -11,9 +11,10 @@
   <arg name="imu_port" default="0" doc="port to which the sensor should send imu data"/>
   <arg name="udp_profile_lidar" default=" " doc="lidar packet profile; possible values: {
     LEGACY,
-    RNG19_RFL8_SIG16_NIR16_DUAL,
     RNG19_RFL8_SIG16_NIR16,
     RNG15_RFL8_NIR8
+    RNG19_RFL8_SIG16_NIR16_DUAL,
+    FUSA_RNG15_RFL8_NIR8_DUAL
     }"/>
   <arg name="lidar_mode" default=" " doc="resolution and rate; possible values: {
     512x10,

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>ouster_ros</name>
-  <version>0.12.2</version>
+  <version>0.12.3</version>
   <description>Ouster ROS driver</description>
   <maintainer email="[email protected]">ouster developers</maintainer>
   <license file="LICENSE">BSD</license>

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

src/os_cloud_nodelet.cpp

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

src/os_driver_nodelet.cpp

@@ -89,9 +89,11 @@ 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) {
+                !PointCloudProcessorFactory::profile_has_intensity(info.format.udp_profile_lidar)) {
                 NODELET_WARN_STREAM(
-                    "selected point type '" << point_type << "' is not compatible with the current udp profile: RNG15_RFL8_NIR8");
+                    "selected point type '" << point_type
+                    << "' is not compatible with the udp profile: "
+                    << to_string(info.format.udp_profile_lidar));
             }
         }
 

src/os_ros.cpp

@@ -35,10 +35,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) {

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);