Camera synchronization (#15)

* main multithread done

* mutexes optimised, typo fixed, callback group for handling queue added

* feat: reworked multi-thread logic

Lock-free queue from boost library is used to store images in newly allocated memory

StampedImagePtr is changed to be trivially contructable, destructable and copyable

* fix: PR requested changes

Minor changes implemented

* fix: memory allocation

Checked with address sanitizer, fixed allocations

Added logs in assigning master camera idx

fixed bug with assigning master camera idx when flag hardware_trigger is false

* debug, fix: added debug logging

added and commented performance data: CPU time and logs related to lock-free queue

deleted redundunt static param from CameraNode class

* feat: lock-free queue redone

implemented PR requested changes

deleted debug code

* minor codestyle fix in lock-free queue

* fix: minor codestyle in lock-free queue.h

fix: queue latency changed

in order to provide publishing up to 30 FPS

* fix: abort() replaced with exit()

fix: exit code

* feat: reworked acync approach to implement reqeusted changes

* fix: added separate queues

fix: buffer allocation fixed, tested up to 40 fps

* fix: minor fixes after rebase

* fix: codestyle with pre-commit

* feat: PR requested changes implemented

* fix: lock-free queue bug fixed

* feat: storing raw pointers in queues instead of undexes

* fix: lvalue, rvalue fixed in loadFromParams static method

* fix: override for destructor added

.gitignore

@@ -4,6 +4,7 @@
 *.db3
 
 #ROS2 builds
-packages/build
-packages/install
-packages/log
+**/build
+**/install
+**/log
+**/Camera

packages/camera/CMakeLists.txt

@@ -13,6 +13,7 @@ find_package(foxglove_msgs REQUIRED)
 find_package(visualization_msgs REQUIRED)
 find_package(yaml_cpp_vendor REQUIRED)
 find_package(geometry_msgs REQUIRED)
+find_package(Boost REQUIRED)
 find_package(camera_srvs REQUIRED)
 
 add_library(mvsdk SHARED IMPORTED)
@@ -48,6 +49,7 @@ ament_target_dependencies(
     visualization_msgs
     geometry_msgs
     std_msgs
+    Boost
     camera_srvs)
 
 target_link_libraries(camera mvsdk)

packages/camera/include/camera.h

@@ -6,6 +6,7 @@
 #include <opencv2/core/core.hpp>
 
 #include "CameraApi.h"
+#include "lock_free_queue.h"
 #include "params.h"
 
 #include <chrono>
@@ -16,19 +17,53 @@
 
 namespace handy::camera {
 
+struct StampedImageBuffer {
+    uint8_t* raw_buffer = nullptr;
+    uint8_t* bgr_buffer = nullptr;
+    tSdkFrameHead frame_info{};
+    rclcpp::Time timestamp{};
+};
+
+struct CameraPool {
+  public:
+    CameraPool() = default;
+    CameraPool(size_t height, size_t width, size_t frame_n)
+        : frame_n_(frame_n), raw_frame_size_(height * width), bgr_frame_size_(height * width * 3) {
+        raw_.resize(raw_frame_size_ * frame_n_);
+        bgr_.resize(bgr_frame_size_ * frame_n_);
+    }
+
+    uint8_t* getRawFrame(size_t frame_idx) { return raw_.data() + frame_idx * raw_frame_size_; }
+    uint8_t* getBgrFrame(size_t frame_idx) { return bgr_.data() + frame_idx * bgr_frame_size_; }
+
+  private:
+    size_t frame_n_ = 0;
+    size_t raw_frame_size_ = 0;
+    size_t bgr_frame_size_ = 0;
+    std::vector<uint8_t> raw_ = {};
+    std::vector<uint8_t> bgr_ = {};
+};
+
 class CameraNode : public rclcpp::Node {
   public:
     CameraNode();
+    ~CameraNode() override;
+
+    constexpr static int kMaxCameraNum = 4;
+    constexpr static int kQueueCapacity = 5;
 
   private:
-    void applyCameraParameters();
-    void applyParamsToCamera(int camera_idx);
-    void initCalibParams();
+    void applyParamsToCamera(int handle);
+    int getCameraId(int camera_handle);
 
-    void handleOnTimer();
+    void triggerOnTimer();
+    void handleFrame(CameraHandle handle, BYTE* raw_buffer, tSdkFrameHead* frame_info);
+    void handleQueue(int camera_idx);
 
     void publishRawImage(uint8_t* buffer, const rclcpp::Time& timestamp, int camera_idx);
-    void publishBGRImage(uint8_t* buffer, const rclcpp::Time& timestamp, int camera_idx);
+    void publishBGRImage(
+        uint8_t* buffer, uint8_t* bgr_buffer, const rclcpp::Time& timestamp, int camera_idx,
+        tSdkFrameHead& frame_inf);
 
     void abortIfNot(std::string_view msg, int status);
     void abortIfNot(std::string_view msg, int camera_idx, int status);
@@ -37,21 +72,27 @@ class CameraNode : public rclcpp::Node {
         cv::Size preview_frame_size = cv::Size(640, 480);
         std::chrono::duration<double> latency{50.0};
         std::string calibration_file_path = "";
-        int camera_num = 0;
+        int camera_num = kMaxCameraNum;
         bool publish_bgr = false;
         bool publish_bgr_preview = false;
         bool publish_raw = false;
         bool publish_raw_preview = false;
         bool publish_rectified_preview = false;
+        bool hardware_trigger = false;
         int max_buffer_size = 0;
+        int master_camera_idx = -1;
     } param_{};
 
-    std::vector<int> camera_handles_ = {};
-    std::vector<uint8_t*> raw_buffer_ptr_ = {};
-    std::unique_ptr<uint8_t[]> bgr_buffer_ = nullptr;
-    std::vector<tSdkFrameHead> frame_info_ = {};
-    std::vector<CameraIntrinsicParameters> cameras_intrinsics_ = {};
-    std::vector<cv::Size> frame_sizes_ = {};
+    struct State {
+        std::array<std::unique_ptr<LockFreeQueue<StampedImageBuffer>>, kMaxCameraNum> camera_images;
+        std::array<std::unique_ptr<LockFreeQueue<std::pair<uint8_t*, uint8_t*>>>, kMaxCameraNum>
+            free_buffers;
+        std::vector<int> camera_handles = {};
+        std::map<int, int> handle_to_camera_idx = {};
+        std::vector<CameraIntrinsicParameters> cameras_intrinsics = {};
+        std::vector<cv::Size> frame_sizes = {};
+        CameraPool buffers;
+    } state_{};
 
     struct Signals {
         std::vector<rclcpp::Publisher<sensor_msgs::msg::CompressedImage>::SharedPtr> raw_img;
@@ -64,8 +105,14 @@ class CameraNode : public rclcpp::Node {
         // clang-format on
     } signals_{};
 
+    struct CallbackGroups {
+        rclcpp::CallbackGroup::SharedPtr trigger_timer = nullptr;
+        rclcpp::CallbackGroup::SharedPtr handling_queue_timer = nullptr;
+    } call_group_{};
+
     struct Timers {
-        rclcpp::TimerBase::SharedPtr camera_capture = nullptr;
+        rclcpp::TimerBase::SharedPtr camera_soft_trigger = nullptr;
+        std::vector<rclcpp::TimerBase::SharedPtr> camera_handle_queue_timer;
     } timer_{};
 };
 

packages/camera/include/lock_free_queue.h

@@ -0,0 +1,89 @@
+#pragma once
+
+#include <atomic>
+#include <vector>
+
+namespace handy {
+
+/*
+    Node consists of the value itself and index of the vector where it was added
+    this index is required to solve ABA problem
+*/
+template<typename T>
+struct Node {
+    T value;
+    // generation is changed on each assignment and then used to determine
+    // whether this Node was changed by different thread or not
+    std::atomic<int> generation{0};
+};
+
+template<typename T>
+class LockFreeQueue {
+  public:
+    explicit LockFreeQueue(int size) {
+        data_ = std::vector<Node<T>>(size);
+        for (int i = 0; i < size; ++i) {
+            data_[i].generation.store(i);
+        }
+    }
+
+    LockFreeQueue(LockFreeQueue&& other) = delete;
+    LockFreeQueue(const LockFreeQueue& other) = delete;
+
+    bool push(const T& value) {
+        // while could not proccess
+        while (true) {
+            int snap = tail_.load();
+            if (snap - head_.load() == data_.size()) {
+                // buffer is full and can't be updated
+                // in fact, slot can be freed during verification, but we do not double-check
+                return false;
+            }
+
+            if (data_[snap % data_.size()].generation < snap ||
+                !tail_.compare_exchange_weak(snap, snap + 1)) {
+                // desired cell in buffer was already used by another thread
+                // let's try again
+                continue;
+            }
+
+            data_[snap % data_.size()].value = value;
+            // next possible push will be at (current_tail + 1) minimum
+            // so we add +1
+            data_[snap % data_.size()].generation += 1;
+            return true;
+        }
+    }
+
+    bool pop(T& data) {
+        while (true) {
+            int snap = head_.load();
+            if (tail_.load() - snap == 0) {
+                // buffer is empty and can't be updated
+                // in fact, slot can be freed during verification, but we do not double-check
+                return false;
+            }
+
+            if (data_[snap % data_.size()].generation <= snap) {
+                return false;
+            }
+            if (!head_.compare_exchange_weak(snap, snap + 1)) {
+                // desired cell in buffer was already used by another thread
+                // let's try again
+                continue;
+            }
+
+            data = data_[snap % data_.size()].value;
+            // store a value that is for sure larger that any tail, so, + size of buffer
+            data_[snap % data_.size()].generation.store(snap + data_.size());
+            return true;
+        }
+    }
+
+  private:
+    // head ------- tail
+    std::vector<Node<T>> data_;
+    std::atomic<int> head_ = 0;
+    std::atomic<int> tail_ = 0;
+};
+}  // namespace handy

packages/camera/include/params.h

@@ -9,17 +9,20 @@ struct CameraIntrinsicParameters {
     CameraIntrinsicParameters() = default;
 
     CameraIntrinsicParameters(
-        cv::Size size, cv::Mat camera_matrix, const cv::Vec<float, 5>& distort_coefs);
+        cv::Size size, cv::Mat camera_matrix, const cv::Vec<double, 5>& distort_coefs);
 
     void initUndistortMaps();
     cv::Mat undistortImage(cv::Mat& src);
 
     void storeYaml(const std::string& yaml_path) const;
     static CameraIntrinsicParameters loadFromYaml(const std::string& yaml_path);
+    static CameraIntrinsicParameters loadFromParams(
+        cv::Size param_image_size, const std::vector<double>& param_camera_matrix,
+        const std::vector<double>& param_dist_coefs);
 
     cv::Size image_size{};
     cv::Mat camera_matrix{};
-    cv::Vec<float, 5> dist_coefs{};
+    cv::Vec<double, 5> dist_coefs{};
 
     struct Cached {
         std::pair<cv::Mat, cv::Mat> undistort_maps{};

packages/camera/launch/camera.yaml

@@ -23,25 +23,58 @@ launch:
     exec: "camera"
     name: "camera"
     param:
-    - {name: "camera_num", value: 1}
     - {name: "fps", value: 10.0}
+    # interval between calling queue handlers
+    - {name: "queue_latency", value: 15} # in millisecond
     - {name: "preview/width", value: 1280}
     - {name: "preview/height", value: 1024}
     - {name: "publish_raw_preview", value: "$(var publish_raw_preview)"}
     - {name: "publish_bgr_preview", value: "$(var publish_bgr_preview)"}
     - {name: "publish_bgr", value: "$(var publish_bgr)"}
     - {name: "publish_raw", value: "$(var publish_raw)"}
     - {name: "publish_rectified_preview", value: "$(var publish_rectified_preview)"}
-    - {name: "calibration_file_path", value: "$(var calibration_file_path)"}
 
+    # external trigger mode and parameters
+    - { name: "hardware_triger", value: False }  # if True only master is sent trigger signal
+    - { name: "strobe_pulse_width", value: 500 } # in microseconds
+    - { name: "strobe_polarity", value: 1 }      # 0 is valid at low level, 1 is valid at high level
+    - { name: "master_camera_id", value: "1" }
 
-    - name: "camera_0"
+    # name of a camera is its ID (see camera identifier script)
+    - name: "1"
       param:
-        - {name: "exposure_time", value: 6000}        # range: ...
-        - {name: "contrast", value: 100}              # range: 0-100
-        - {name: "gain_rgb", value: [120, 120, 120]}  # range: 0-400, auto default if analog_gain != -1
-        - {name: "analog_gain", value: 9}
-        - {name: "gamma", value: 80}                  # range: 0-250
-        - {name: "saturation", value: 100}            # range: 0-200
-        - {name: "sharpness", value: 0}               # range: 0-100
-        - {name: "auto_exposure", value: False}
+        - name: "exposure_params"
+          param:
+            - { name: "exposure_time", value: 6000 }       # range: ...
+            - { name: "contrast", value: 100 }             # range: 0-100
+            - { name: "gain_rgb", value: [120, 120, 120] } # range: 0-400, auto default if analog_gain != -1
+            - { name: "analog_gain", value: 50 }
+            - { name: "gamma", value: 80 }                 # range: 0-250
+            - { name: "saturation", value: 100 }           # range: 0-200
+            - { name: "sharpness", value: 0 }              # range: 0-100
+            - { name: "auto_exposure", value: False }
+        - name: "intrinsic_params"
+          param:
+            - name: "image_size"
+              param:
+                - { name: "width", value: 1920 }
+                - { name: "height", value: 1200 }
+            - { name: "camera_matrix", value: [2909.92107945441, 0., 582.1724561319506, 0., 3134.621273608562, 532.0677320807733, 0., 0., 1.] }
+            - { name: "distorsion_coefs", value: [-0.7151398, 10.59934, 1.975451e-05, 0.07159046, 1.047479]}
+    - name: "2"
+      param:
+        - name: "exposure_params"
+          param:
+            - { name: "exposure_time", value: 6000 }       # range: ...
+            - { name: "contrast", value: 100 }             # range: 0-100
+            - { name: "gain_rgb", value: [120, 120, 120] } # range: 0-400, auto default if analog_gain != -1
+            - { name: "analog_gain", value: 50 }
+            - { name: "gamma", value: 80 }                 # range: 0-250
+            - { name: "saturation", value: 100 }           # range: 0-200
+            - { name: "sharpness", value: 0 }              # range: 0-100
+            - { name: "auto_exposure", value: False }
+        - name: "intrinsic_params"
+          param:
+            - { name: "image_size", value: [1920, 1200] }
+            - { name: "camera_matrix", value: [2909.92107945441, 0., 582.1724561319506, 0., 3134.621273608562, 532.0677320807733, 0., 0., 1.] }
+            - { name: "distorsion_coefs", value: [-0.7151398, 10.59934, 1.975451e-05, 0.07159046, 1.047479]}

packages/camera/package.xml

@@ -19,6 +19,7 @@
   <depend>foxglove_msgs</depend>
   <depend>geometry_msgs</depend>
   <depend>yaml_cpp_vendor</depend>
+  <depend>Boost</depend>
   <depend>camera_srvs</depend>
 
   <test_depend>ament_lint_auto</test_depend>