Apply && to accepted method of ThreadSafeRingBuffer +

nits and code formatting

ouster-ros/include/ouster_ros/os_processing_node_base.h

@@ -18,7 +18,7 @@ namespace ouster_ros {
 class OusterProcessingNodeBase : public rclcpp::Node {
    protected:
     OusterProcessingNodeBase(const std::string& name,
-                                      const rclcpp::NodeOptions& options)
+                             const rclcpp::NodeOptions& options)
         : rclcpp::Node(name, options) {}
 
     void create_metadata_subscriber(

ouster-ros/src/laser_scan_processor.h

@@ -18,47 +18,50 @@ using ouster_ros::get_n_returns;
 using ouster_ros::lidar_scan_to_laser_scan_msg;
 
 class LaserScanProcessor {
-    public:
-        using OutputType = std::vector<std::shared_ptr<sensor_msgs::msg::LaserScan>>;
-        using PostProcessingFunc = std::function<void(OutputType)>;
+   public:
+    using OutputType =
+        std::vector<std::shared_ptr<sensor_msgs::msg::LaserScan>>;
+    using PostProcessingFn = std::function<void(OutputType)>;
 
-    public:
+   public:
     LaserScanProcessor(const ouster::sensor::sensor_info& info,
-                       const std::string& frame_id, uint16_t ring, PostProcessingFunc func) :
-        frame(frame_id), ld_mode(info.mode), ring_(ring),
-        scan_msgs(get_n_returns(info), std::make_shared<sensor_msgs::msg::LaserScan>()),
-        func_(func) {
-    }
-
-    private:
-        void process(const ouster::LidarScan& lidar_scan,
-                     uint64_t scan_ts, const rclcpp::Time& msg_ts) {
-            for (int i = 0; i < static_cast<int>(scan_msgs.size()); ++i) {
-                *scan_msgs[i] = lidar_scan_to_laser_scan_msg(
-                    lidar_scan, msg_ts, frame, ld_mode, ring_, i);
-            }
+                       const std::string& frame_id, uint16_t ring,
+                       PostProcessingFn func)
+        : frame(frame_id),
+          ld_mode(info.mode),
+          ring_(ring),
+          scan_msgs(get_n_returns(info),
+                    std::make_shared<sensor_msgs::msg::LaserScan>()),
+          post_processing_fn(func) {}
 
-            if (func_)
-                func_(scan_msgs);
+   private:
+    void process(const ouster::LidarScan& lidar_scan, uint64_t scan_ts,
+                 const rclcpp::Time& msg_ts) {
+        for (int i = 0; i < static_cast<int>(scan_msgs.size()); ++i) {
+            *scan_msgs[i] = lidar_scan_to_laser_scan_msg(
+                lidar_scan, msg_ts, frame, ld_mode, ring_, i);
         }
 
+        if (post_processing_fn) post_processing_fn(scan_msgs);
+    }
+
    public:
     static LidarScanProcessor create(const ouster::sensor::sensor_info& info,
-                              const std::string& frame, uint16_t ring, PostProcessingFunc func) {
-
+                                     const std::string& frame, uint16_t ring,
+                                     PostProcessingFn func) {
         auto handler =
             std::make_shared<LaserScanProcessor>(info, frame, ring, func);
 
-        return [handler](const ouster::LidarScan& lidar_scan,
-                         uint64_t scan_ts, const rclcpp::Time& msg_ts) {
+        return [handler](const ouster::LidarScan& lidar_scan, uint64_t scan_ts,
+                         const rclcpp::Time& msg_ts) {
             handler->process(lidar_scan, scan_ts, msg_ts);
         };
     }
 
-    private:
-        std::string frame;
-        sensor::lidar_mode ld_mode;
-        uint16_t ring_;
-        OutputType scan_msgs;
-        PostProcessingFunc func_;
+   private:
+    std::string frame;
+    sensor::lidar_mode ld_mode;
+    uint16_t ring_;
+    OutputType scan_msgs;
+    PostProcessingFn post_processing_fn;
 };

ouster-ros/src/lidar_packet_handler.h

@@ -122,9 +122,7 @@ class LidarPacketHandler {
         lidar_scan_handlers.push_back(handler);
     }
 
-    void clear_registered_lidar_scan_handlers() {
-        lidar_scan_handlers.clear();
-    }
+    void clear_registered_lidar_scan_handlers() { lidar_scan_handlers.clear(); }
 
    public:
     static HandlerType create_handler(

ouster-ros/src/os_sensor_node.cpp

@@ -794,10 +794,11 @@ void OusterSensor::start_packet_processing_threads() {
     imu_packets_skip = false;
     imu_packets_processing_thread_active = true;
     imu_packets_processing_thread = std::make_unique<std::thread>([this]() {
+        auto read_packet = [this](const uint8_t* buffer) {
+            if (!imu_packets_skip) on_imu_packet_msg(buffer);
+        };
         while (imu_packets_processing_thread_active) {
-            imu_packets->read([this](const uint8_t* buffer) {
-                if (!imu_packets_skip) on_imu_packet_msg(buffer);
-            });
+            imu_packets->read(read_packet);
         }
         RCLCPP_DEBUG(get_logger(), "imu_packets_processing_thread done.");
     });

ouster-ros/src/point_cloud_processor.h

@@ -19,17 +19,21 @@
 using ouster_ros::get_n_returns;
 
 class PointCloudProcessor {
-    public:
-    using OutputType = std::vector<std::shared_ptr<sensor_msgs::msg::PointCloud2>>;
-    using PostProcessingFunc = std::function<void(OutputType)>;
+   public:
+    using OutputType =
+        std::vector<std::shared_ptr<sensor_msgs::msg::PointCloud2>>;
+    using PostProcessingFn = std::function<void(OutputType)>;
 
-    public:
+   public:
     PointCloudProcessor(const ouster::sensor::sensor_info& info,
-                       const std::string& frame_id, bool apply_lidar_to_sensor_transform, PostProcessingFunc func)
-            : frame(frame_id), cloud{info.format.columns_per_frame, info.format.pixels_per_column},
-             pc_msgs(get_n_returns(info), std::make_shared<sensor_msgs::msg::PointCloud2>()),
-             func_(func) {
-
+                        const std::string& frame_id,
+                        bool apply_lidar_to_sensor_transform,
+                        PostProcessingFn func)
+        : frame(frame_id),
+          cloud{info.format.columns_per_frame, info.format.pixels_per_column},
+          pc_msgs(get_n_returns(info),
+                  std::make_shared<sensor_msgs::msg::PointCloud2>()),
+          post_processing_fn(func) {
         ouster::mat4d additional_transform =
             apply_lidar_to_sensor_transform ? info.lidar_to_sensor_transform
                                             : ouster::mat4d::Identity();
@@ -46,56 +50,54 @@ class PointCloudProcessor {
         points = ouster::PointsF(lut_direction.rows(), lut_offset.cols());
     }
 
-    private:
-        void pcl_toROSMsg(const ouster_ros::Cloud& pcl_cloud,
+   private:
+    void pcl_toROSMsg(const ouster_ros::Cloud& pcl_cloud,
                       sensor_msgs::msg::PointCloud2& cloud) {
-            // TODO: remove the staging step in the future
-            pcl::toPCLPointCloud2(pcl_cloud, staging_pcl_pc2);
-            pcl_conversions::moveFromPCL(staging_pcl_pc2, cloud);
-        }
+        // TODO: remove the staging step in the future
+        pcl::toPCLPointCloud2(pcl_cloud, staging_pcl_pc2);
+        pcl_conversions::moveFromPCL(staging_pcl_pc2, cloud);
+    }
 
-        void process(const ouster::LidarScan& lidar_scan, uint64_t scan_ts, const rclcpp::Time& msg_ts) {
-            for (int i = 0; i < static_cast<int>(pc_msgs.size()); ++i) {
-                ouster_ros::scan_to_cloud_f(
-                    points,
-                    lut_direction,
-                    lut_offset, scan_ts,
-                    lidar_scan, cloud, i);
-                pcl_toROSMsg(cloud, *pc_msgs[i]);
-                pc_msgs[i]->header.stamp = msg_ts;
-                pc_msgs[i]->header.frame_id = frame;
-            }
-
-            if (func_)
-                func_(pc_msgs);
+    void process(const ouster::LidarScan& lidar_scan, uint64_t scan_ts,
+                 const rclcpp::Time& msg_ts) {
+        for (int i = 0; i < static_cast<int>(pc_msgs.size()); ++i) {
+            ouster_ros::scan_to_cloud_f(points, lut_direction, lut_offset,
+                                        scan_ts, lidar_scan, cloud, i);
+            pcl_toROSMsg(cloud, *pc_msgs[i]);
+            pc_msgs[i]->header.stamp = msg_ts;
+            pc_msgs[i]->header.frame_id = frame;
         }
 
+        if (post_processing_fn) post_processing_fn(pc_msgs);
+    }
+
    public:
     static LidarScanProcessor create(const ouster::sensor::sensor_info& info,
-                                     const std::string& frame, bool apply_lidar_to_sensor_transform,
-                                     PostProcessingFunc func) {
-
+                                     const std::string& frame,
+                                     bool apply_lidar_to_sensor_transform,
+                                     PostProcessingFn func) {
         auto handler = std::make_shared<PointCloudProcessor>(
             info, frame, apply_lidar_to_sensor_transform, func);
 
-        return [handler](const ouster::LidarScan& lidar_scan, uint64_t scan_ts, const rclcpp::Time& msg_ts) {
+        return [handler](const ouster::LidarScan& lidar_scan, uint64_t scan_ts,
+                         const rclcpp::Time& msg_ts) {
             handler->process(lidar_scan, scan_ts, msg_ts);
         };
     }
 
-    private:
-        // a buffer used for staging during the conversion
-        // from a PCL point cloud to a ros point cloud message
-        pcl::PCLPointCloud2 staging_pcl_pc2;
+   private:
+    // a buffer used for staging during the conversion
+    // from a PCL point cloud to a ros point cloud message
+    pcl::PCLPointCloud2 staging_pcl_pc2;
 
-        std::string frame;
+    std::string frame;
 
-        ouster::PointsF lut_direction;
-        ouster::PointsF lut_offset;
-        ouster::PointsF points;
-        ouster_ros::Cloud cloud;
+    ouster::PointsF lut_direction;
+    ouster::PointsF lut_offset;
+    ouster::PointsF points;
+    ouster_ros::Cloud cloud;
 
-        OutputType pc_msgs;
+    OutputType pc_msgs;
 
-        PostProcessingFunc func_;
+    PostProcessingFn post_processing_fn;
 };

ouster-ros/src/thread_safe_ring_buffer.h

@@ -57,8 +57,8 @@ class ThreadSafeRingBuffer {
      * Writes to the buffer safely, the method will keep blocking until the there
      * is a space available within the buffer.
      */
-    template <class BufferWriteT>
-    void write(BufferWriteT buffer_write) {
+    template <class BufferWriteFn>
+    void write(BufferWriteFn&& buffer_write) {
         std::unique_lock<std::mutex> lock(mutex);
         fullCondition.wait(lock, [this] { return active_items_count < capacity(); });
         buffer_write(&buffer[write_idx * item_size]);
@@ -72,8 +72,8 @@ class ThreadSafeRingBuffer {
      * Writes to the buffer safely, if there is not space left then this method
      * will overite the last item.
      */
-    template <class BufferWriteT>
-    void write_overwrite(BufferWriteT buffer_write) {
+    template <class BufferWriteFn>
+    void write_overwrite(BufferWriteFn&& buffer_write) {
         std::unique_lock<std::mutex> lock(mutex);
         buffer_write(&buffer[write_idx * item_size]);
         write_idx = (write_idx + 1) % capacity();
@@ -89,8 +89,8 @@ class ThreadSafeRingBuffer {
      * Gives access to read the buffer through a callback, the method will block
      * until there is something to read is available.
      */
-    template <typename BufferReadT>
-    void read(BufferReadT&& buffer_read) {
+    template <typename BufferReadFn>
+    void read(BufferReadFn&& buffer_read) {
         std::unique_lock<std::mutex> lock(mutex);
         emptyCondition.wait(lock, [this] { return active_items_count > 0; });
         buffer_read(&buffer[read_idx * item_size]);
@@ -103,8 +103,8 @@ class ThreadSafeRingBuffer {
      * Gives access to read the buffer through a callback, if buffer is
      * inaccessible the method will timeout and buffer_read gets a nullptr.
      */
-    template <typename BufferReadT>
-    void read_timeout(BufferReadT buffer_read, std::chrono::seconds timeout) {
+    template <typename BufferReadFn>
+    void read_timeout(BufferReadFn&& buffer_read, std::chrono::seconds timeout) {
         std::unique_lock<std::mutex> lock(mutex);
         if (emptyCondition.wait_for(lock, timeout, [this] { return active_items_count > 0; })) {
             buffer_read(&buffer[read_idx * item_size]);