From 5f3edfe6c64daf3278ca985dc1979ec913d9d803 Mon Sep 17 00:00:00 2001 From: Bakin Denis Date: Tue, 9 Jul 2024 23:09:28 +0000 Subject: [PATCH] feat: signal handling and main function added fix: improved comments in queue synchronization --- packages/camera/CMakeLists.txt | 2 +- packages/camera/include/camera.h | 9 ++++-- packages/camera/src/camera.cpp | 48 +++++++++++++++++------------ packages/camera/src/camera_main.cpp | 38 +++++++++++++++++++++++ 4 files changed, 75 insertions(+), 22 deletions(-) create mode 100644 packages/camera/src/camera_main.cpp diff --git a/packages/camera/CMakeLists.txt b/packages/camera/CMakeLists.txt index d709b22..656ee29 100644 --- a/packages/camera/CMakeLists.txt +++ b/packages/camera/CMakeLists.txt @@ -13,7 +13,7 @@ find_package(Boost 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 ) diff --git a/packages/camera/include/camera.h b/packages/camera/include/camera.h index 2187844..90ea0ee 100644 --- a/packages/camera/include/camera.h +++ b/packages/camera/include/camera.h @@ -22,7 +22,6 @@ struct Size { int height; bool operator==(const Size& other) { return width == other.width && height == other.height; } - bool operator!=(const Size& other) { return !(*this == other); } }; @@ -68,8 +67,12 @@ struct CameraPool { class CameraRecorder { public: - CameraRecorder(const char* param_file, const char* output_filename); + using CameraSubscriberCallback = std::function)>; + + 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 = 4; constexpr static int kQueueCapacity = 5; @@ -80,6 +83,7 @@ class CameraRecorder { void synchronizeQueues(); static int getCameraId(int camera_handle); void applyParamsToCamera(int handle); + void saveSynchronizedBuffers(std::shared_ptr images); struct Params { std::chrono::duration latency{50.0}; // in milliseconds @@ -107,6 +111,7 @@ class CameraRecorder { std::vector file_mutexes; std::vector camera_handles; std::vector alligned_buffers; + std::vector registered_callbacks; CameraPool buffers; } state_{}; }; diff --git a/packages/camera/src/camera.cpp b/packages/camera/src/camera.cpp index 1ab44d2..f34213a 100644 --- a/packages/camera/src/camera.cpp +++ b/packages/camera/src/camera.cpp @@ -12,7 +12,7 @@ using namespace std::chrono_literals; namespace { template -T getUnsignedDifference(T a, T b) { +T getUnsignedDifference(T& a, T& b) { return (a > b) ? (a - b) : (b - a); } @@ -44,10 +44,19 @@ void abortIfNot(std::string_view msg, int camera_idx, int status) { } } -CameraRecorder::CameraRecorder(const char* param_file, const char* output_filename) { +CameraRecorder::CameraRecorder( + const char* param_file, const char* output_filename, bool save_to_file) { param_.param_file = {param_file}; param_.output_filename = {output_filename}; + if (save_to_file) { + // if needed register saveCallbackLauncher as a callback when images are syncronized + auto saveCallbackLauncher = [this](std::shared_ptr images) { + this->saveSynchronizedBuffers(images); + }; + registerSubscriberCallback(saveCallbackLauncher); + } + abortIfNot("camera init", CameraSdkInit(0)); tSdkCameraDevInfo cameras_list[10]; abortIfNot("camera listing", CameraEnumerateDevice(cameras_list, &state_.camera_num)); @@ -191,10 +200,12 @@ void CameraRecorder::synchronizeQueues() { } int recieved_buffers_idx = new_recieved_buffer.frame_id % num_of_sync_buffers; recieved_buffers[recieved_buffers_idx].push_back(new_recieved_buffer); + // if there are enough frames in the bucket if (recieved_buffers[recieved_buffers_idx].size() == state_.camera_num) { // make single structure as a shared ptr + // deleter is meant to push buffers back to the queue and then free the structure auto custom_deleter = [this](SynchronizedFrameBuffers* sync_buffers_ptr) { - for (int i = 0; i < sync_buffers_ptr->images.size(); ++i) { + for (size_t i = 0; i < sync_buffers_ptr->images.size(); ++i) { while (!this->state_.free_buffers[i]->push(sync_buffers_ptr->images[i])) { } } @@ -212,12 +223,13 @@ void CameraRecorder::synchronizeQueues() { sync_buffers->images[current_recieved_buffer.camera_idx] = BufferPair{ current_recieved_buffer.raw_buffer, current_recieved_buffer.bgr_buffer}; + // logical AND of conditions "timestamp difference is within 2 ms" timestamps_are_close &= (getUnsignedDifference( current_recieved_buffer.timestamp, sync_buffers->timestamp) - < 2); + < 20); } - + // casted to special structure, clear the bucket recieved_buffers[recieved_buffers_idx].clear(); if (!timestamps_are_close) { printf( @@ -225,8 +237,10 @@ void CameraRecorder::synchronizeQueues() { state_.camera_num); continue; } - // send to either to all registered consumers or to save handler - printf("%p\n", sync_buffers.get()); + + for (size_t i = 0; i < state_.registered_callbacks.size(); ++i) { + std::thread(state_.registered_callbacks[i], sync_buffers).detach(); + } } } } @@ -273,12 +287,20 @@ void CameraRecorder::handleFrame(CameraHandle handle, BYTE* raw_buffer, tSdkFram } } +void CameraRecorder::saveSynchronizedBuffers(std::shared_ptr images) {} + +void CameraRecorder::registerSubscriberCallback(CameraSubscriberCallback callback) { + state_.registered_callbacks.push_back(callback); +} + int CameraRecorder::getCameraId(int camera_handle) { uint8_t camera_id; abortIfNot("getting camera id", CameraLoadUserData(camera_handle, 0, &camera_id, 1)); return static_cast(camera_id); } +void CameraRecorder::stopInstance() { state_.running = false; } + void CameraRecorder::applyParamsToCamera(int handle) { const int camera_idx = state_.handle_to_idx[handle]; const std::string camera_id_str = std::to_string(getCameraId(handle)); @@ -404,15 +426,3 @@ void CameraRecorder::applyParamsToCamera(int handle) { } } } // namespace handy::camera - -// ./camera_bin -int main(int argc, char* argv[]) { - if (argc != 3) { - printf("help\n./camera_bin \n"); - return 0; - } - handy::camera::CameraRecorder writer(argv[1], argv[2]); - printf("finished writing to file %s\n", argv[2]); - - return 0; -} diff --git a/packages/camera/src/camera_main.cpp b/packages/camera/src/camera_main.cpp new file mode 100644 index 0000000..5a6a154 --- /dev/null +++ b/packages/camera/src/camera_main.cpp @@ -0,0 +1,38 @@ +#include "camera.h" + +#include +#include +#include +#include + +struct GlobalCameraRecorderInfo { + std::mutex mutex; + std::condition_variable condvar; + handy::camera::CameraRecorder* camera_recorder_ptr = nullptr; + bool ready_to_exit = false; +} global_recorder_info; + +void handleSignal(int signum) { + global_recorder_info.camera_recorder_ptr->stopInstance(); + global_recorder_info.ready_to_exit = true; +} + +// ./camera_bin +int main(int argc, char* argv[]) { + if (argc != 3) { + printf("help\n./camera_bin \n"); + return 0; + } + handy::camera::CameraRecorder writer(argv[1], argv[2], true); + global_recorder_info.camera_recorder_ptr = &writer; + signal(SIGINT, handleSignal); + + std::unique_lock lock(global_recorder_info.mutex); + while (!global_recorder_info.ready_to_exit) { + global_recorder_info.condvar.wait(lock); + } + + printf("finished writing to file %s\n", argv[2]); + + return 0; +}