SW-5396: publish point cloud in destaggered form

CHANGELOG.rst

@@ -4,6 +4,13 @@ Changelog
 
 [unreleased]
 ============
+ouster_ros(1)
+-------------
+* breaking: publish PCL point clouds destaggered.
+
+
+ouster_ros v0.10.0
+==================
 
 ouster_ros(1)
 -------------

include/ouster_ros/os_ros.h

@@ -124,6 +124,27 @@ void scan_to_cloud_f(ouster::PointsF& points,
                      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

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>ouster_ros</name>
-  <version>0.9.0</version>
+  <version>0.9.1</version>
   <description>Ouster ROS driver</description>
   <maintainer email="oss@ouster.io">ouster developers</maintainer>
   <license file="LICENSE">BSD</license>

src/imu_packet_handler.h

@@ -40,4 +40,4 @@ class ImuPacketHandler {
     }
 };
 
-}  // namespace ouster_ros
+}   // namespace ouster_ros

src/lidar_packet_handler.h

@@ -75,12 +75,13 @@ inline bool check_token(const std::set<std::string>& tokens,
 
 }  // namespace
 
+namespace ouster_ros {
+
 namespace sensor = ouster::sensor;
 
 using LidarScanProcessor =
     std::function<void(const ouster::LidarScan&, uint64_t, const ros::Time&)>;
 
-namespace ouster_ros {
 
 class LidarPacketHandler {
     using LidarPacketAccumlator = std::function<bool(const uint8_t*)>;
@@ -288,4 +289,4 @@ class LidarPacketHandler {
     LidarPacketAccumlator lidar_packet_accumlator;
 };
 
-}  // namespace ouster_ros
+}   // namespace ouster_ros

src/os_image_nodelet.cpp

@@ -134,13 +134,10 @@ class OusterImage : public nodelet::Nodelet {
         auto nearir_image_map = Eigen::Map<ouster::img_t<pixel_type>>(
             (pixel_type*)nearir_image->data.data(), H, W);
 
-        const auto& px_offset = info.format.pixel_shift_by_row;
-
         // copy data out of Cloud message, with destaggering
         for (size_t u = 0; u < H; u++) {
             for (size_t v = 0; v < W; v++) {
-                const size_t vv = (v + W - px_offset[u]) % W;
-                const auto& pt = cloud[u * W + vv];
+                const auto& pt = cloud[u * W + v];
 
                 // 16 bit img: use 4mm resolution and throw out returns >
                 // 260m

src/os_ros.cpp

@@ -196,6 +196,46 @@ void copy_scan_to_cloud(ouster_ros::Cloud& cloud, const ouster::LidarScan& ls,
     }
 }
 
+template <typename PointT, typename RangeT, typename ReflectivityT,
+          typename NearIrT, typename SignalT>
+void copy_scan_to_cloud_destaggered(
+    ouster_ros::Cloud& cloud, const ouster::LidarScan& ls, uint64_t 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,
+    const std::vector<int>& pixel_shift_by_row) {
+    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 col_ts = timestamp[v];
+            const auto ts = col_ts > scan_ts ? col_ts - scan_ts : 0UL;
+            const auto src_idx =
+                u * ls.w + (v + ls.w - pixel_shift_by_row[u]) % ls.w;
+            const auto tgt_idx = u * ls.w + v;
+            const auto xyz = points.row(src_idx);
+            cloud.points[tgt_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[src_idx]),
+                static_cast<uint32_t>(ts),
+                static_cast<uint16_t>(rf[src_idx]),
+                static_cast<uint16_t>(u),
+                static_cast<uint16_t>(nr[src_idx]),
+                static_cast<uint32_t>(rg[src_idx]),
+            };
+        }
+    }
+}
+
 void scan_to_cloud_f(ouster::PointsF& points,
                      const ouster::PointsF& lut_direction,
                      const ouster::PointsF& lut_offset,
@@ -259,6 +299,40 @@ void scan_to_cloud_f(ouster::PointsF& points,
                        signal);
 }
 
+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) {
+    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 = impl::get_or_fill_zero<uint16_t>(
+        impl::suitable_return(sensor::ChanField::REFLECTIVITY, second), ls);
+
+    ouster::img_t<uint32_t> signal = impl::get_or_fill_zero<uint32_t>(
+        impl::suitable_return(sensor::ChanField::SIGNAL, second), ls);
+
+    ouster::img_t<uint16_t> near_ir = impl::get_or_fill_zero<uint16_t>(
+        impl::suitable_return(sensor::ChanField::NEAR_IR, second), ls);
+
+    ouster::cartesianT(points, range, lut_direction, lut_offset);
+
+    copy_scan_to_cloud_destaggered(cloud, ls, scan_ts, points, range,
+                                   reflectivity, near_ir, signal,
+                                   pixel_shift_by_row);
+}
+
 sensor_msgs::PointCloud2 cloud_to_cloud_msg(const Cloud& cloud,
                                             const ros::Time& timestamp,
                                             const std::string& frame) {

src/point_cloud_processor.h

@@ -29,6 +29,7 @@ class PointCloudProcessor {
                         bool apply_lidar_to_sensor_transform,
                         PostProcessingFn func)
         : frame(frame_id),
+          pixel_shift_by_row(info.format.pixel_shift_by_row),
           cloud{info.format.columns_per_frame, info.format.pixels_per_column},
           pc_msgs(get_n_returns(info)),
           post_processing_fn(func) {
@@ -61,8 +62,9 @@ class PointCloudProcessor {
     void process(const ouster::LidarScan& lidar_scan, uint64_t scan_ts,
                  const ros::Time& msg_ts) {
         for (int i = 0; i < static_cast<int>(pc_msgs.size()); ++i) {
-            scan_to_cloud_f(points, lut_direction, lut_offset, scan_ts,
-                            lidar_scan, cloud, i);
+            scan_to_cloud_f_destaggered(cloud,
+                points, lut_direction, lut_offset,
+                scan_ts, lidar_scan, pixel_shift_by_row, i);
             pcl_toROSMsg(cloud, *pc_msgs[i]);
             pc_msgs[i]->header.stamp = msg_ts;
             pc_msgs[i]->header.frame_id = frame;
@@ -95,11 +97,12 @@ class PointCloudProcessor {
     ouster::PointsF lut_direction;
     ouster::PointsF lut_offset;
     ouster::PointsF points;
+    std::vector<int> pixel_shift_by_row;
     ouster_ros::Cloud cloud;
 
     OutputType pc_msgs;
 
     PostProcessingFn post_processing_fn;
 };
 
-}  // namespace ouster_ros
+}   // namespace ouster_ros