Merge pull request #40 from ouster-lidar/SW-4360-ouster-ros-driver-pe…

…rformance-optimizations

SW-4360: ouster-ros driver performance and stability optimizations

CHANGELOG.rst

@@ -7,14 +7,20 @@ Changelog
 
 ouster_ros
 ----------
+* breaking change: renamed ``ouster_ros/ros.h`` to ``ouster_ros/os_ros.h`` and
+  ``ouster_ros/point.h`` to ``ouster_ros/os_point.h``.
+* breaking change: change the type of the ring field within ``ouster::Point`` from ``uint8_t`` to
+  ``uint16_t``
 * correct LICENSE file installation path.
 * update code files copyrights period.
 * bug fix: ros driver doesn't use correct udp_dest given by user during launch
 * update published TF transforms time with senosr or ros time based on the
   active timestamp mode.
-* breaking change: renamed ``ouster_ros/ros.h`` to ``ouster_ros/os_ros.h`` and
-  ``ouster_ros/point.h`` to ``ouster_ros/os_point.h``.
 * validate lidar and imu port values. warn users when assigning random port numbers.
+* switch to using the cartesianT method when populating pcl point cloud for performance and reduced
+  cpu utilization
+* reduce dynamic memory allocation within the driver for performance and driver stability
+* add ``pcl_ros`` as a dependency to ``package.xml``
 
 ouster_client
 --------------
@@ -31,6 +37,9 @@ ouster_client
 * added a new method ``init_logger()`` to provide control over the logs emitted by ``ouster_client``.
 * add parsing for new FW 3.0 thermal features shot_limiting and thermal_shutdown statuses and countdowns
 * add frame_status to LidarScan
+* introduce a new method ``cartesianT()`` which speeds up the computation of point projecion from range
+  image, the method also can process the cartesian product with single float precision. A new unit test
+  ``cartesian_test`` which shows achieved speed up gains by the number of valid returns in lidar scan.
 
 [20221004]
 ==========

include/ouster_ros/os_point.h

@@ -22,7 +22,7 @@ struct EIGEN_ALIGN16 Point {
     float intensity;
     uint32_t t;
     uint16_t reflectivity;
-    uint8_t ring;
+    uint16_t ring;
     uint16_t ambient;
     uint32_t range;
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
@@ -38,7 +38,7 @@ POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_ros::Point,
     // use std::uint32_t to avoid conflicting with pcl::uint32_t
     (std::uint32_t, t, t)
     (std::uint16_t, reflectivity, reflectivity)
-    (std::uint8_t, ring, ring)
+    (std::uint16_t, ring, ring)
     (std::uint16_t, ambient, ambient)
     (std::uint32_t, range, range)
 )

include/ouster_ros/os_ros.h

@@ -93,6 +93,27 @@ void scan_to_cloud(const ouster::XYZLut& xyz_lut,
                    ouster::LidarScan::ts_t scan_ts, const ouster::LidarScan& ls,
                    ouster_ros::Cloud& cloud, int return_index = 0);
 
+/**
+ * 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] ls 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,
+                ouster::LidarScan::ts_t scan_ts,
+                const ouster::LidarScan& ls,
+                ouster_ros::Cloud& cloud,
+                int return_index);
+
+
 /**
  * Serialize a PCL point cloud to a ROS message
  * @param[in] cloud the PCL point cloud to convert

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>ouster_ros</name>
-  <version>0.7.1</version>
+  <version>0.7.2</version>
   <description>Ouster ROS driver</description>
   <maintainer email="[email protected]">ouster developers</maintainer>
   <license file="LICENSE">BSD</license>
@@ -12,6 +12,8 @@
   <depend>sensor_msgs</depend>
   <depend>geometry_msgs</depend>
   <depend>tf2_ros</depend>
+  <depend>pcl_ros</depend>
+  <depend>pcl_conversions</depend>
 
   <build_depend>boost</build_depend>
   <build_depend>nodelet</build_depend>
@@ -20,7 +22,6 @@
   <build_depend>message_generation</build_depend>
   <build_depend>tf2_eigen</build_depend>
   <build_depend>libpcl-all-dev</build_depend>
-  <build_depend>pcl_conversions</build_depend>
   <build_depend>curl</build_depend>
   <build_depend>spdlog</build_depend>
 

src/os_cloud_nodelet.cpp

@@ -19,6 +19,7 @@
 #include <sensor_msgs/Imu.h>
 #include <sensor_msgs/PointCloud2.h>
 #include <tf2_ros/transform_broadcaster.h>
+#include <pcl_conversions/pcl_conversions.h>
 
 #include <algorithm>
 #include <chrono>
@@ -87,7 +88,14 @@ class OusterCloud : public nodelet::Nodelet {
             lidar_pubs[i] = pub;
         }
 
-        xyz_lut = ouster::make_xyz_lut(info);
+        // The ouster_ros drive currently only uses single precision when it
+        // produces the point cloud. So it isn't of a benefit to compute point
+        //  cloud xyz coordinates using double precision (for the time being).
+        auto xyz_lut = ouster::make_xyz_lut(info);
+        lut_direction = xyz_lut.direction.cast<float>();
+        lut_offset = xyz_lut.offset.cast<float>();
+        points = ouster::PointsF(lut_direction.rows(), lut_offset.cols());
+        pc_ptr = boost::make_shared<sensor_msgs::PointCloud2>();
 
         ls = ouster::LidarScan{W, H, info.format.udp_profile_lidar};
         cloud = ouster_ros::Cloud{W, H};
@@ -104,14 +112,20 @@ class OusterCloud : public nodelet::Nodelet {
             "imu_packets", 100, &OusterCloud::imu_handler, this);
     }
 
+    void pcl_toROSMsg(const ouster_ros::Cloud &pcl_cloud, sensor_msgs::PointCloud2 &cloud) {
+        // TODO: remove the staging step in the future
+        static pcl::PCLPointCloud2 pcl_pc2;
+        pcl::toPCLPointCloud2(pcl_cloud, pcl_pc2);
+        pcl_conversions::moveFromPCL(pcl_pc2, cloud);
+    }
+
     void convert_scan_to_pointcloud_publish(std::chrono::nanoseconds scan_ts,
                                             const ros::Time& msg_ts) {
         for (int i = 0; i < n_returns; ++i) {
-            scan_to_cloud(xyz_lut, scan_ts, ls, cloud, i);
-            sensor_msgs::PointCloud2 pc =
-                ouster_ros::cloud_to_cloud_msg(cloud, msg_ts, sensor_frame);
-            sensor_msgs::PointCloud2Ptr pc_ptr =
-                boost::make_shared<sensor_msgs::PointCloud2>(pc);
+            scan_to_cloud_f(points, lut_direction, lut_offset, scan_ts, ls, cloud, i);
+            pcl_toROSMsg(cloud, *pc_ptr);
+            pc_ptr->header.stamp = msg_ts;
+            pc_ptr->header.frame_id = sensor_frame;
             lidar_pubs[i].publish(pc_ptr);
         }
 
@@ -172,11 +186,15 @@ class OusterCloud : public nodelet::Nodelet {
     std::vector<ros::Publisher> lidar_pubs;
     ros::Subscriber imu_packet_sub;
     ros::Publisher imu_pub;
+    sensor_msgs::PointCloud2::Ptr pc_ptr;
+
 
     sensor::sensor_info info;
     int n_returns = 0;
 
-    ouster::XYZLut xyz_lut;
+    ouster::PointsF lut_direction;
+    ouster::PointsF lut_offset;
+    ouster::PointsF points;
     ouster::LidarScan ls;
     ouster_ros::Cloud cloud;
     std::unique_ptr<ouster::ScanBatcher> scan_batcher;
@@ -189,6 +207,7 @@ class OusterCloud : public nodelet::Nodelet {
 
     bool use_ros_time;
 };
+
 }  // namespace nodelets_os
 
 PLUGINLIB_EXPORT_CLASS(nodelets_os::OusterCloud, nodelet::Nodelet)

src/os_ros.cpp

@@ -1,7 +1,7 @@
 /**
  * Copyright (c) 2018-2022, Ouster, Inc.
  * All rights reserved.
- * 
+ *
  * @file ros.cpp
  * @brief A nodelet that connects to a live ouster sensor
  */
@@ -21,10 +21,11 @@
 #include <string>
 #include <vector>
 
-namespace ouster_ros {
 
 namespace sensor = ouster::sensor;
 
+namespace ouster_ros {
+
 bool read_imu_packet(const sensor::client& cli, PacketMsg& pm,
                      const sensor::packet_format& pf) {
     pm.buf.resize(pf.imu_packet_size + 1);
@@ -114,13 +115,12 @@ sensor::ChanField suitable_return(sensor::ChanField input_field, bool second) {
 template <typename T>
 inline ouster::img_t<T> get_or_fill_zero(sensor::ChanField f,
                                          const ouster::LidarScan& ls) {
-    ouster::img_t<T> result{ls.h, ls.w};
-    if (ls.field_type(f)) {
-        ouster::impl::visit_field(ls, f, read_and_cast(), result);
-    } else {
-        result = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic,
-                               Eigen::RowMajor>::Zero(ls.h, ls.w);
+    if (!ls.field_type(f)) {
+        return ouster::img_t<T>::Zero(ls.h, ls.w);
     }
+
+    ouster::img_t<T> result{ls.h, ls.w};
+    ouster::impl::visit_field(ls, f, read_and_cast(), result);
     return result;
 }
 
@@ -163,6 +163,76 @@ void scan_to_cloud(const ouster::XYZLut& xyz_lut,
     }
 }
 
+template <typename PointT, typename RangeT, typename ReflectivityT,
+          typename NearIrT, typename SignalT>
+void copy_scan_to_cloud(ouster_ros::Cloud& cloud, const ouster::LidarScan& ls,
+                        std::chrono::nanoseconds scan_ts, const PointT& points,
+                        const ouster::img_t<RangeT>& range,
+                        const ouster::img_t<ReflectivityT>& reflectivity,
+                        const ouster::img_t<NearIrT>& near_ir,
+                        const ouster::img_t<SignalT>& signal) {
+    auto timestamp = ls.timestamp();
+
+    const auto rg = range.data();
+    const auto rf = reflectivity.data();
+    const auto nr = near_ir.data();
+    const auto sg = signal.data();
+
+#ifdef __OUSTER_UTILIZE_OPENMP__
+#pragma omp parallel for collapse(2)
+#endif
+    for (auto u = 0; u < ls.h; u++) {
+        for (auto v = 0; v < ls.w; v++) {
+            const auto ts = std::min(
+                std::chrono::nanoseconds(timestamp[v]) - scan_ts, scan_ts);
+            const auto idx = u * ls.w + v;
+            const auto xyz = points.row(idx);
+            cloud.points[idx] = ouster_ros::Point{
+                {static_cast<float>(xyz(0)), static_cast<float>(xyz(1)),
+                 static_cast<float>(xyz(2)), 1.0f},
+                static_cast<float>(sg[idx]),
+                static_cast<uint32_t>(ts.count()),
+                static_cast<uint16_t>(rf[idx]),
+                static_cast<uint16_t>(u),
+                static_cast<uint16_t>(nr[idx]),
+                static_cast<uint32_t>(rg[idx]),
+            };
+        }
+    }
+}
+
+void scan_to_cloud_f(ouster::PointsF& points,
+                     const ouster::PointsF& lut_direction,
+                     const ouster::PointsF& lut_offset,
+                     ouster::LidarScan::ts_t scan_ts,
+                     const ouster::LidarScan& ls, ouster_ros::Cloud& cloud,
+                     int return_index) {
+    bool second = (return_index == 1);
+
+    assert(cloud.width == static_cast<std::uint32_t>(ls.w) &&
+           cloud.height == static_cast<std::uint32_t>(ls.h) &&
+           "point cloud and lidar scan size mismatch");
+
+    // across supported lidar profiles range is always 32-bit
+    auto range_channel_field =
+        second ? sensor::ChanField::RANGE2 : sensor::ChanField::RANGE;
+    ouster::img_t<uint32_t> range = ls.field<uint32_t>(range_channel_field);
+
+    ouster::img_t<uint16_t> reflectivity = get_or_fill_zero<uint16_t>(
+        suitable_return(sensor::ChanField::REFLECTIVITY, second), ls);
+
+    ouster::img_t<uint32_t> signal = get_or_fill_zero<uint32_t>(
+        suitable_return(sensor::ChanField::SIGNAL, second), ls);
+
+    ouster::img_t<uint16_t> near_ir = get_or_fill_zero<uint16_t>(
+        suitable_return(sensor::ChanField::NEAR_IR, second), ls);
+
+    ouster::cartesianT(points, range, lut_direction, lut_offset);
+
+    copy_scan_to_cloud(cloud, ls, scan_ts, points, range, reflectivity, near_ir,
+                       signal);
+}
+
 sensor_msgs::PointCloud2 cloud_to_cloud_msg(const Cloud& cloud,
                                             const ros::Time& timestamp,
                                             const std::string& frame) {