Camera recorder

packages/camera/CMakeLists.txt

@@ -2,17 +2,18 @@ cmake_minimum_required(VERSION 3.8)
 project(camera)
 
 add_compile_options(-Wall -Wextra -Werror=unused-variable -Wpedantic)
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread")
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread -O3")
 
 find_package(OpenCV REQUIRED)
 find_package(yaml_cpp_vendor REQUIRED)
 find_package(ament_cmake REQUIRED)
 find_package(Boost REQUIRED)
+find_package(mcap_vendor REQUIRED)
 
 add_library(mvsdk SHARED IMPORTED)
 set_target_properties(mvsdk PROPERTIES IMPORTED_LOCATION "/lib/libMVSDK.so")
 
-add_executable(camera src/camera.cpp src/camera_status.cpp)
+add_executable(camera src/camera_main.cpp src/camera.cpp src/camera_status.cpp )
 add_executable(
   calibration src/calibration_main.cpp src/calibration.cpp src/params.cpp
 )
@@ -32,7 +33,7 @@ target_include_directories(
                        "/usr/include"
 )
 
-ament_target_dependencies(camera yaml_cpp_vendor)
+ament_target_dependencies(camera yaml_cpp_vendor mcap_vendor)
 ament_target_dependencies(calibration yaml_cpp_vendor Boost OpenCV)
 ament_target_dependencies(triangulation OpenCV yaml_cpp_vendor)
 

packages/camera/include/camera.h

@@ -5,46 +5,141 @@
 #include <string>
 #include <vector>
 #include <map>
+#include <thread>
+#include <mutex>
+#include <condition_variable>
+#include <cstdint>
+
+#include <boost/lockfree/queue.hpp>
+#include "mcap_vendor/mcap/writer.hpp"
 
 using namespace std::chrono_literals;
 
 namespace handy::camera {
-class Writer {
+
+class CameraRecorder;
+
+class MappedFileManager final : public mcap::IWritable {
+  public:
+    MappedFileManager();
+    ~MappedFileManager() override{};
+
+    void init(
+        CameraRecorder* recorder_instance, std::string filepath, size_t bytes_per_second_estim);
+    void write(const std::byte* data, uint64_t size);
+    void handleWrite(const std::byte* data, uint64_t size) override;
+    void doubleFileSize();
+    void end() override;
+    uint64_t size() const override;
+
+    uint64_t kMmapLargeConstant = 7 * 1024 * 1024 * 1024;
+
+  private:
+    int file_ = 0;
+    void* mmaped_ptr_ = nullptr;
+    CameraRecorder* recorder_instance_ = nullptr;  // to be able to call stopInstance()
+    uint64_t size_ = 0;
+    uint64_t internal_file_size_ = 0;
+    // uint64_t file_capacity_in_seconds_ = 20; // how long can written to the file
+    size_t counter = 0;
+};
+
+struct Size {
+    size_t area() const { return static_cast<size_t>(width * height); };
+
+    int width = 0;
+    int height = 0;
+
+    bool operator==(const Size& other) { return width == other.width && height == other.height; }
+    bool operator!=(const Size& other) { return !(*this == other); }
+};
+
+struct StampedImageBuffer {
+    uint8_t* raw_buffer = nullptr;
+    tSdkFrameHead frame_info{};
+    int camera_idx = -1;
+    int frame_id = -1;
+    uint32_t timestamp = 0;
+};
+
+struct SynchronizedFrameBuffers {
+    std::vector<uint8_t*> images;
+    std::vector<Size> image_sizes;
+    uint32_t timestamp = 0;  // in 0.1 milliseconds
+};
+
+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 CameraRecorder {
   public:
-    Writer(const char* param_file, const char* output_filename);
+    using CameraSubscriberCallback = std::function<void(std::shared_ptr<SynchronizedFrameBuffers>)>;
+
+    CameraRecorder(const char* param_file, const char* output_filename, bool save_to_file = false);
+    ~CameraRecorder();
+    void registerSubscriberCallback(CameraSubscriberCallback callback);
+    void stopInstance();
+
+    constexpr static int kMaxCameraNum = 2;
+    constexpr static int kQueueCapacity = 80;
 
   private:
     void handleFrame(CameraHandle handle, BYTE* raw_buffer, tSdkFrameHead* frame_info);
+    void triggerCamera();
+    void synchronizeQueues();
     static int getCameraId(int camera_handle);
     void applyParamsToCamera(int handle);
-
-    struct Size {
-        size_t area() const { return static_cast<size_t>(width * height); };
-
-        int width;
-        int height;
-    };
+    void saveSynchronizedBuffers(std::shared_ptr<SynchronizedFrameBuffers> images);
 
     struct Params {
         std::chrono::duration<double> latency{50.0};  // in milliseconds
         std::string param_file;
         std::string output_filename;
-        int fps = 20;
-        int frames_to_take = 1000;
         int master_camera_id = 1;
         bool use_hardware_triger = false;
+        bool save_to_file = false;
     } param_{};
 
     struct State {
-        std::vector<std::atomic<int>> counters;
+        std::array<std::unique_ptr<boost::lockfree::queue<StampedImageBuffer>>, kMaxCameraNum>
+            camera_images;
+        std::array<std::unique_ptr<boost::lockfree::queue<uint8_t*>>, kMaxCameraNum> free_buffers;
+
+        std::atomic<bool> running = true;
+        std::vector<std::thread> threads;  // trigger thread, queue handler thread
+        std::vector<std::atomic<int>> frame_ids;
         std::vector<std::atomic<size_t>> current_buffer_idx;
         int camera_num = 2;
-        std::vector<int> files;
         std::map<int, int> handle_to_idx;
         std::vector<Size> frame_sizes;
-        std::vector<std::mutex> file_mutexes;
         std::vector<int> camera_handles;
-        std::vector<void*> alligned_buffers;
+        std::vector<CameraSubscriberCallback> registered_callbacks;
+        CameraPool buffers;
+        std::vector<mcap::ChannelId> mcap_channels_ids_;
+        mcap::SchemaId bayer_schema_id;
+
+        std::condition_variable synchronizer_condvar;
+        std::mutex synchronizer_mutex;
     } state_{};
+
+    mcap::McapWriter mcap_writer_;
+    MappedFileManager file_manager_;
 };
 }  // namespace handy::camera

packages/camera/package.xml

@@ -11,6 +11,7 @@
 
   <depend>OpenCV</depend>
   <depend>yaml_cpp_vendor</depend>
+  <depend>mcap_vendor</depend>
   <depend>Boost</depend>
 
   <test_depend>ament_lint_auto</test_depend>