From 4aa347ced656c306f9a5847036befeab6c5a0636 Mon Sep 17 00:00:00 2001 From: Bakin Denis <57637880+BakinDF@users.noreply.github.com> Date: Wed, 20 Mar 2024 13:37:29 +0300 Subject: [PATCH] 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 | 7 +- packages/camera/CMakeLists.txt | 2 + packages/camera/include/camera.h | 73 ++++- packages/camera/include/lock_free_queue.h | 89 ++++++ packages/camera/include/params.h | 7 +- packages/camera/launch/camera.yaml | 55 +++- packages/camera/package.xml | 1 + packages/camera/src/camera.cpp | 337 +++++++++++++++------- packages/camera/src/camera_main.cpp | 7 +- packages/camera/src/params.cpp | 25 +- 10 files changed, 466 insertions(+), 137 deletions(-) create mode 100644 packages/camera/include/lock_free_queue.h diff --git a/.gitignore b/.gitignore index ebbc388..06f7000 100644 --- a/.gitignore +++ b/.gitignore @@ -4,6 +4,7 @@ *.db3 #ROS2 builds -packages/build -packages/install -packages/log +**/build +**/install +**/log +**/Camera diff --git a/packages/camera/CMakeLists.txt b/packages/camera/CMakeLists.txt index 2570640..bfd73f7 100644 --- a/packages/camera/CMakeLists.txt +++ b/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) diff --git a/packages/camera/include/camera.h b/packages/camera/include/camera.h index 00b8b7b..a1572be 100644 --- a/packages/camera/include/camera.h +++ b/packages/camera/include/camera.h @@ -6,6 +6,7 @@ #include #include "CameraApi.h" +#include "lock_free_queue.h" #include "params.h" #include @@ -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 raw_ = {}; + std::vector 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 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 camera_handles_ = {}; - std::vector raw_buffer_ptr_ = {}; - std::unique_ptr bgr_buffer_ = nullptr; - std::vector frame_info_ = {}; - std::vector cameras_intrinsics_ = {}; - std::vector frame_sizes_ = {}; + struct State { + std::array>, kMaxCameraNum> camera_images; + std::array>>, kMaxCameraNum> + free_buffers; + std::vector camera_handles = {}; + std::map handle_to_camera_idx = {}; + std::vector cameras_intrinsics = {}; + std::vector frame_sizes = {}; + CameraPool buffers; + } state_{}; struct Signals { std::vector::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 camera_handle_queue_timer; } timer_{}; }; diff --git a/packages/camera/include/lock_free_queue.h b/packages/camera/include/lock_free_queue.h new file mode 100644 index 0000000..a8853b6 --- /dev/null +++ b/packages/camera/include/lock_free_queue.h @@ -0,0 +1,89 @@ +#pragma once + +#include +#include + +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 +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 generation{0}; +}; + +template +class LockFreeQueue { + public: + explicit LockFreeQueue(int size) { + data_ = std::vector>(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> data_; + std::atomic head_ = 0; + std::atomic tail_ = 0; +}; +} // namespace handy diff --git a/packages/camera/include/params.h b/packages/camera/include/params.h index 5895fdc..a378f93 100644 --- a/packages/camera/include/params.h +++ b/packages/camera/include/params.h @@ -9,17 +9,20 @@ struct CameraIntrinsicParameters { CameraIntrinsicParameters() = default; CameraIntrinsicParameters( - cv::Size size, cv::Mat camera_matrix, const cv::Vec& distort_coefs); + cv::Size size, cv::Mat camera_matrix, const cv::Vec& 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& param_camera_matrix, + const std::vector& param_dist_coefs); cv::Size image_size{}; cv::Mat camera_matrix{}; - cv::Vec dist_coefs{}; + cv::Vec dist_coefs{}; struct Cached { std::pair undistort_maps{}; diff --git a/packages/camera/launch/camera.yaml b/packages/camera/launch/camera.yaml index 41e455f..0482348 100644 --- a/packages/camera/launch/camera.yaml +++ b/packages/camera/launch/camera.yaml @@ -23,8 +23,9 @@ 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)"} @@ -32,16 +33,48 @@ launch: - {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]} diff --git a/packages/camera/package.xml b/packages/camera/package.xml index 308c8e6..69eb604 100644 --- a/packages/camera/package.xml +++ b/packages/camera/package.xml @@ -19,6 +19,7 @@ foxglove_msgs geometry_msgs yaml_cpp_vendor + Boost camera_srvs ament_lint_auto diff --git a/packages/camera/src/camera.cpp b/packages/camera/src/camera.cpp index 57a1e4f..900d939 100644 --- a/packages/camera/src/camera.cpp +++ b/packages/camera/src/camera.cpp @@ -3,6 +3,8 @@ #include +#include + namespace handy::camera { using namespace std::chrono_literals; @@ -18,7 +20,7 @@ void CameraNode::abortIfNot(std::string_view msg, int status) { len(status_name), status_name.data(), status); - abort(); + exit(EXIT_FAILURE); } } @@ -34,64 +36,122 @@ void CameraNode::abortIfNot(std::string_view msg, int camera_idx, int status) { len(status_name), status_name.data(), status); - abort(); - } -} - -namespace { -int maxBufSize(const std::vector& frame_sizes) { - int max_size = 0; - for (size_t i = 0; i < frame_sizes.size(); ++i) { - max_size = std::max(max_size, 3 * frame_sizes[i].width * frame_sizes[i].height); + exit(EXIT_FAILURE); } - return max_size; } -} // namespace CameraNode::CameraNode() : Node("camera_node") { abortIfNot("camera init", CameraSdkInit(0)); + tSdkCameraDevInfo cameras_list[kMaxCameraNum]; + abortIfNot("camera listing", CameraEnumerateDevice(cameras_list, ¶m_.camera_num)); - int camera_num = 100; // attach all conntected cameras - tSdkCameraDevInfo cameras_list[100]; - abortIfNot("camera listing", CameraEnumerateDevice(cameras_list, &camera_num)); - - param_.camera_num = this->declare_parameter("camera_num", 1); - if (param_.camera_num != camera_num) { - RCLCPP_ERROR_STREAM( - this->get_logger(), - "expected " << param_.camera_num << " cameras, found " << camera_num); - abort(); - } - - RCLCPP_INFO_STREAM(this->get_logger(), "Camera number: " << param_.camera_num); + RCLCPP_INFO_STREAM(this->get_logger(), "camera number: " << param_.camera_num); param_.preview_frame_size.width = this->declare_parameter("preview/width", 640); param_.preview_frame_size.height = this->declare_parameter("preview/height", 480); RCLCPP_INFO_STREAM(this->get_logger(), "frame preview size=" << param_.preview_frame_size); - camera_handles_ = std::vector(param_.camera_num); - raw_buffer_ptr_ = std::vector(param_.camera_num); - frame_info_ = std::vector(param_.camera_num); - frame_sizes_ = std::vector(param_.camera_num); + param_.hardware_trigger = this->declare_parameter("hardware_trigger", false); + + RCLCPP_INFO(this->get_logger(), "hardware trigger mode: %i", param_.hardware_trigger); + + state_.camera_handles = std::vector(param_.camera_num); + state_.frame_sizes = std::vector(param_.camera_num); + + const int strobe_polarity = this->declare_parameter("strobe_polarity", 1); + RCLCPP_INFO_STREAM( + this->get_logger(), "strobe polarity mode: " << (strobe_polarity == 1 ? "LOW" : "HIGH")); + const int strobe_pulse_width = this->declare_parameter("strobe_pulse_width", 500); + RCLCPP_INFO_STREAM(this->get_logger(), "strobe pulse width = " << strobe_pulse_width); + + const int master_camera_id = [&] { + std::string str_master_camera_id = + this->declare_parameter("master_camera_id", "1"); + + try { + return std::stoi(str_master_camera_id); + } catch (std::exception&) { + RCLCPP_ERROR( + this->get_logger(), "invalid master camera id '%s'!", str_master_camera_id.c_str()); + exit(EXIT_FAILURE); + } + }(); + + RCLCPP_INFO( + this->get_logger(), "master camera id = %s", std::to_string(master_camera_id).c_str()); for (int i = 0; i < param_.camera_num; ++i) { + state_.camera_images[i] = + std::make_unique>(kQueueCapacity); + state_.free_buffers[i] = + std::make_unique>>(kQueueCapacity); + abortIfNot( "camera init " + std::to_string(i), - CameraInit(&cameras_list[i], -1, -1, &camera_handles_[i])); - abortIfNot("set icp", i, CameraSetIspOutFormat(camera_handles_[i], CAMERA_MEDIA_TYPE_BGR8)); - abortIfNot("start", CameraPlay(camera_handles_[i])); + CameraInit(&cameras_list[i], -1, -1, &state_.camera_handles[i])); + + int camera_internal_id = getCameraId(state_.camera_handles[i]); + RCLCPP_INFO( + this->get_logger(), "allocated image queue for camera ID = %d", camera_internal_id); + + state_.handle_to_camera_idx[state_.camera_handles[i]] = i; + + abortIfNot( + "set icp", i, CameraSetIspOutFormat(state_.camera_handles[i], CAMERA_MEDIA_TYPE_BGR8)); + + auto func = [](CameraHandle idx, + BYTE* raw_buffer, + tSdkFrameHead* frame_info, + PVOID camera_node_instance) -> void { + reinterpret_cast(camera_node_instance) + ->handleFrame(idx, raw_buffer, frame_info); + }; + CameraSetCallbackFunction(state_.camera_handles[i], std::move(func), this, nullptr); + + if (!param_.hardware_trigger) { // if node is launch in soft trigger mode + RCLCPP_INFO(this->get_logger(), "soft trigger for camera ID = %d", camera_internal_id); + CameraSetTriggerMode(state_.camera_handles[i], SOFT_TRIGGER); + } else if (camera_internal_id == master_camera_id) { // if hard trigger mode is enabled and + // this is a master camera + RCLCPP_INFO( + this->get_logger(), "treating as master camera ID = %d", camera_internal_id); + // master camera is triggered by the node + CameraSetTriggerMode(state_.camera_handles[i], SOFT_TRIGGER); + // to trigger others by hardware + CameraSetOutPutIOMode(state_.camera_handles[i], 0, IOMODE_STROBE_OUTPUT); + CameraSetStrobeMode(state_.camera_handles[i], STROBE_SYNC_WITH_TRIG_MANUAL); + CameraSetStrobePolarity(state_.camera_handles[i], strobe_polarity); + CameraSetStrobeDelayTime(state_.camera_handles[i], 0); + CameraSetStrobePulseWidth(state_.camera_handles[i], strobe_pulse_width); + + param_.master_camera_idx = i; + RCLCPP_INFO( + this->get_logger(), + "camera ID = %d idx = %d is saved as master camera", + camera_internal_id, + i); + } else { // if hard trigger mode is enabled and this is a slave camera + RCLCPP_INFO(this->get_logger(), "treating as slave camera ID = %d", camera_internal_id); + // slave camera waits for external hardware trigger to occur + CameraSetTriggerMode(state_.camera_handles[i], EXTERNAL_TRIGGER); + CameraSetExtTrigSignalType(state_.camera_handles[i], EXT_TRIG_HIGH_LEVEL); + CameraSetOutPutIOMode(state_.camera_handles[i], 0, IOMODE_TRIG_INPUT); + } + + applyParamsToCamera(state_.camera_handles[i]); + abortIfNot("start", CameraPlay(state_.camera_handles[i])); + RCLCPP_INFO( + this->get_logger(), "inited API and started camera ID = %d", camera_internal_id); + } + if (param_.hardware_trigger && + param_.master_camera_idx == -1) { // provided master camera id was not found + RCLCPP_ERROR(this->get_logger(), "master camera id was not found: %d", master_camera_id); + exit(EXIT_FAILURE); } RCLCPP_INFO_STREAM(this->get_logger(), "all cameras started!"); - param_.calibration_file_path = this->declare_parameter( - "calibration_file_path", "param_save/camera_params.yaml"); - RCLCPP_INFO( - this->get_logger(), - "parameters will be read from: %s", - param_.calibration_file_path.c_str()); - param_.publish_raw = this->declare_parameter("publish_raw", false); RCLCPP_INFO(this->get_logger(), "publish raw: %i", param_.publish_raw); @@ -146,32 +206,76 @@ CameraNode::CameraNode() : Node("camera_node") { const auto fps = this->declare_parameter("fps", 20.0); param_.latency = std::chrono::duration(1 / fps); + const auto queue_latency = + this->declare_parameter("queue_latency", 20) / 1000.; // in millisecond + std::chrono::duration queue_handling_latency(queue_latency); RCLCPP_INFO(this->get_logger(), "latency=%fs", param_.latency.count()); - timer_.camera_capture = this->create_wall_timer(param_.latency, [this] { handleOnTimer(); }); + RCLCPP_INFO(this->get_logger(), "queue_latency=%fs", queue_handling_latency.count()); - applyCameraParameters(); + cv::Size max_frame_size = *std::max_element( + state_.frame_sizes.begin(), + state_.frame_sizes.begin(), + [](cv::Size& first, cv::Size& second) { return first.area() < second.area(); }); - param_.max_buffer_size = maxBufSize(frame_sizes_); - bgr_buffer_ = std::make_unique(param_.camera_num * param_.max_buffer_size); + param_.max_buffer_size = max_frame_size.area() * 3; + state_.buffers = + CameraPool(max_frame_size.height, max_frame_size.width, kQueueCapacity * kMaxCameraNum); + RCLCPP_INFO(this->get_logger(), "%d pools initialised", kQueueCapacity * kMaxCameraNum); - if (param_.publish_rectified_preview) { - initCalibParams(); + // init queues and push pointers to buffers + for (int i = 0; i < param_.camera_num; ++i) { + for (size_t j = 0; j < kQueueCapacity; ++j) { + state_.free_buffers[i]->push(std::make_pair( + state_.buffers.getRawFrame(i * kQueueCapacity + j), + state_.buffers.getBgrFrame(i * kQueueCapacity + j))); + } + } + + call_group_.trigger_timer = + this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + timer_.camera_soft_trigger = this->create_wall_timer( + param_.latency, std::bind(&CameraNode::triggerOnTimer, this), call_group_.trigger_timer); + + call_group_.handling_queue_timer = + this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); + + for (int i = 0; i < param_.camera_num; ++i) { + timer_.camera_handle_queue_timer.push_back(this->create_wall_timer( + queue_handling_latency, + [this, i]() { this->handleQueue(i); }, + call_group_.handling_queue_timer)); + } +} + +CameraNode::~CameraNode() { + for (int i = 0; i < param_.camera_num; ++i) { + abortIfNot("camera " + std::to_string(i) + " stop", CameraStop(state_.camera_handles[i])); + abortIfNot( + "camera " + std::to_string(i) + " uninit", CameraUnInit(state_.camera_handles[i])); } } -void CameraNode::applyCameraParameters() { +void CameraNode::triggerOnTimer() { for (int i = 0; i < param_.camera_num; ++i) { - applyParamsToCamera(i); + if (!param_.hardware_trigger || i == param_.master_camera_idx) { + CameraSoftTrigger(state_.camera_handles[i]); + } } } -void CameraNode::applyParamsToCamera(int camera_idx) { - int handle = camera_handles_[camera_idx]; - std::string prefix = "camera_" + std::to_string(camera_idx) + "."; +int CameraNode::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 CameraNode::applyParamsToCamera(int handle) { + // applying exposure params + int camera_idx = state_.handle_to_camera_idx[handle]; + std::string prefix = std::to_string(getCameraId(handle)) + ".exposure_params."; { tSdkCameraCapbility camera_capability; - abortIfNot( - "get image size", CameraGetCapability(camera_handles_[camera_idx], &camera_capability)); + abortIfNot("get image size", CameraGetCapability(handle, &camera_capability)); tSdkImageResolution* resolution_data = camera_capability.pImageSizeDesc; RCLCPP_INFO( this->get_logger(), @@ -179,7 +283,8 @@ void CameraNode::applyParamsToCamera(int camera_idx) { camera_idx, resolution_data->iWidth, resolution_data->iHeight); - frame_sizes_[camera_idx] = cv::Size(resolution_data->iWidth, resolution_data->iHeight); + state_.frame_sizes[camera_idx] = + cv::Size(resolution_data->iWidth, resolution_data->iHeight); } { @@ -202,7 +307,7 @@ void CameraNode::applyParamsToCamera(int camera_idx) { exposure.count(), camera_idx, param_.latency.count()); - abort(); + exit(EXIT_FAILURE); } abortIfNot("set exposure", camera_idx, CameraSetExposureTime(handle, exposure.count())); @@ -273,6 +378,37 @@ void CameraNode::applyParamsToCamera(int camera_idx) { abortIfNot("set sharpness", CameraSetSharpness(handle, sharpness)); RCLCPP_INFO(this->get_logger(), "camera=%i, sharpness=%i", camera_idx, sharpness); } + + if (!param_.publish_rectified_preview) { + return; + } + + prefix = std::to_string(getCameraId(handle)) + ".intrinsic_params."; + const std::vector param_names = { + "image_size.width", "image_size.height", "camera_matrix", "distorsion_coefs"}; + bool has_all_params = std::all_of( + param_names.begin(), param_names.end(), [&prefix, this](const std::string& param_name) { + return this->has_parameter(prefix + param_name); + }); + if (!has_all_params) { + RCLCPP_ERROR(this->get_logger(), "camera %d failed to read instrinsic params", camera_idx); + exit(EXIT_FAILURE); + } + + // loading intrinsic parameters + const int64_t param_image_width = this->declare_parameter(prefix + "image_size.width"); + const int64_t param_image_height = + this->declare_parameter(prefix + "image_size.height"); + const cv::Size param_image_size(param_image_width, param_image_height); + + const std::vector param_camera_matrix = + this->declare_parameter>(prefix + "camera_matrix"); + const std::vector param_dist_coefs = + this->declare_parameter>(prefix + "distorsion_coefs"); + + state_.cameras_intrinsics.push_back(CameraIntrinsicParameters::loadFromParams( + param_image_size, param_camera_matrix, param_dist_coefs)); + RCLCPP_INFO(this->get_logger(), "camera=%i loaded intrinsics", camera_idx); } namespace { @@ -304,13 +440,15 @@ cv::Mat rescale(const cv::Mat& image, const cv::Size& size) { } // namespace -void CameraNode::publishBGRImage(uint8_t* buffer, const rclcpp::Time& timestamp, int idx) { +void CameraNode::publishBGRImage( + uint8_t* raw_buffer, uint8_t* bgr_buffer, const rclcpp::Time& timestamp, int idx, + tSdkFrameHead& frame_inf) { const auto header = makeHeader(timestamp, idx); - auto* const bgr_buffer = bgr_buffer_.get() + idx * param_.max_buffer_size; abortIfNot( - "get bgr", CameraImageProcess(camera_handles_[idx], buffer, bgr_buffer, &frame_info_[idx])); - cv::Mat image(frame_sizes_[idx], CV_8UC3, bgr_buffer); + "get bgr", + CameraImageProcess(state_.camera_handles[idx], raw_buffer, bgr_buffer, &frame_inf)); + cv::Mat image(state_.frame_sizes[idx], CV_8UC3, bgr_buffer); if (param_.publish_bgr) { cv_bridge::CvImage cv_image(header, "bgr8", image); @@ -323,7 +461,7 @@ void CameraNode::publishBGRImage(uint8_t* buffer, const rclcpp::Time& timestamp, } if (param_.publish_rectified_preview) { - cv::Mat undistorted_image = cameras_intrinsics_[idx].undistortImage(image); + cv::Mat undistorted_image = state_.cameras_intrinsics[idx].undistortImage(image); cv_bridge::CvImage cv_image( header, "bgr8", rescale(undistorted_image, param_.preview_frame_size)); signals_.rectified_preview_img[idx]->publish(toJpegMsg(cv_image)); @@ -332,7 +470,7 @@ void CameraNode::publishBGRImage(uint8_t* buffer, const rclcpp::Time& timestamp, void CameraNode::publishRawImage(uint8_t* buffer, const rclcpp::Time& timestamp, int camera_idx) { const auto header = makeHeader(timestamp, camera_idx); - cv::Mat image(frame_sizes_[camera_idx], CV_8UC1, buffer); + cv::Mat image(state_.frame_sizes[camera_idx], CV_8UC1, buffer); if (param_.publish_raw) { cv_bridge::CvImage cv_image(header, "mono8", image); @@ -345,55 +483,52 @@ void CameraNode::publishRawImage(uint8_t* buffer, const rclcpp::Time& timestamp, } } -void CameraNode::handleOnTimer() { - const auto now = this->get_clock()->now(); - - for (int i = 0; i < param_.camera_num; ++i) { - const auto timeout = std::chrono::duration_cast(param_.latency); - const int status = CameraGetImageBuffer( - camera_handles_[i], &frame_info_[i], &raw_buffer_ptr_[i], timeout.count()); - - if (status != CAMERA_STATUS_SUCCESS) { - const auto status_name = toStatusName(status); - RCLCPP_WARN( - this->get_logger(), - "miss frame camera=%i, %.*s(%i)", - i, - len(status_name), - status_name.data(), - status); - continue; - } - - const cv::Size size{frame_info_[i].iWidth, frame_info_[i].iHeight}; +void CameraNode::handleFrame(CameraHandle handle, BYTE* raw_buffer, tSdkFrameHead* frame_info) { + const cv::Size size{frame_info->iWidth, frame_info->iHeight}; + if (size != state_.frame_sizes[state_.handle_to_camera_idx[handle]]) { + RCLCPP_ERROR_STREAM( + this->get_logger(), + "expected frame size " << state_.frame_sizes[handle] << ", but got " << size); + exit(EXIT_FAILURE); + } + int frame_size_px = frame_info->iWidth * frame_info->iHeight; - if (size != frame_sizes_[i]) { - RCLCPP_ERROR_STREAM( - this->get_logger(), - "expected frame size " << frame_sizes_[i] << ", but got " << size); - abort(); - } + std::pair free_buffers; + while (!state_.free_buffers[state_.handle_to_camera_idx[handle]]->pop(free_buffers)) { } + std::memcpy(free_buffers.first, raw_buffer, frame_size_px); + StampedImageBuffer stamped_buffer_to_add{ + free_buffers.first, // raw buffer + free_buffers.second, // bgr buffer + *frame_info, + this->get_clock()->now()}; + if (!state_.camera_images[state_.handle_to_camera_idx[handle]]->push(stamped_buffer_to_add)) { + RCLCPP_ERROR(this->get_logger(), "unable to fit into queue! exiting"); + exit(EXIT_FAILURE); + } + CameraReleaseImageBuffer(handle, raw_buffer); +} - for (int i = 0; i < param_.camera_num; ++i) { +void CameraNode::handleQueue(int camera_idx) { + StampedImageBuffer stamped_buffer_id; + while (state_.camera_images[camera_idx]->pop(stamped_buffer_id)) { if (param_.publish_raw || param_.publish_raw_preview) { - publishRawImage(raw_buffer_ptr_[i], now, i); + publishRawImage(stamped_buffer_id.raw_buffer, stamped_buffer_id.timestamp, camera_idx); } - if (param_.publish_bgr || param_.publish_bgr_preview) { - publishBGRImage(raw_buffer_ptr_[i], now, i); + publishBGRImage( + stamped_buffer_id.raw_buffer, + stamped_buffer_id.bgr_buffer, + stamped_buffer_id.timestamp, + camera_idx, + stamped_buffer_id.frame_info); + } + if (!state_.free_buffers[camera_idx]->push( + std::make_pair(stamped_buffer_id.raw_buffer, stamped_buffer_id.bgr_buffer))) { + RCLCPP_ERROR(this->get_logger(), "unable to push bgr buffer back after use"); + exit(EXIT_FAILURE); } - - CameraReleaseImageBuffer(camera_handles_[i], raw_buffer_ptr_[i]); } } -void CameraNode::initCalibParams() { - for (int idx = 0; idx < param_.camera_num; ++idx) { - RCLCPP_INFO_STREAM(this->get_logger(), "loading camera " << idx << " parameters"); - std::string calibration_name = "camera_" + std::to_string(idx); - cameras_intrinsics_.push_back( - CameraIntrinsicParameters::loadFromYaml(param_.calibration_file_path)); - } -} } // namespace handy::camera diff --git a/packages/camera/src/camera_main.cpp b/packages/camera/src/camera_main.cpp index e63e3ae..000bdbe 100644 --- a/packages/camera/src/camera_main.cpp +++ b/packages/camera/src/camera_main.cpp @@ -4,7 +4,12 @@ int main(int argc, char *argv[]) { rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); + + rclcpp::Node::SharedPtr node = std::make_shared(); + + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node); + executor.spin(); rclcpp::shutdown(); return 0; diff --git a/packages/camera/src/params.cpp b/packages/camera/src/params.cpp index e034b8c..0cd5cac 100644 --- a/packages/camera/src/params.cpp +++ b/packages/camera/src/params.cpp @@ -10,7 +10,7 @@ namespace handy { CameraIntrinsicParameters::CameraIntrinsicParameters( - cv::Size size, cv::Mat camera_matr, const cv::Vec& distort_coefs) + cv::Size size, cv::Mat camera_matr, const cv::Vec& distort_coefs) : image_size(size), camera_matrix(std::move(camera_matr)), dist_coefs(distort_coefs) {} void CameraIntrinsicParameters::storeYaml(const std::string& yaml_path) const { @@ -23,11 +23,11 @@ void CameraIntrinsicParameters::storeYaml(const std::string& yaml_path) const { output_yaml << YAML::BeginMap; // global yaml map output_yaml << YAML::Key << "image_size"; - output_yaml << YAML::Value << YAML::BeginSeq << image_size.width << image_size.height - << YAML::EndSeq; + output_yaml << YAML::Value << YAML::Flow << YAML::BeginSeq << image_size.width + << image_size.height << YAML::EndSeq; output_yaml << YAML::Key << "camera_matrix"; - output_yaml << YAML::Value << YAML::BeginSeq; + output_yaml << YAML::Value << YAML::Flow << YAML::BeginSeq; for (int i = 0; i < 3; ++i) { for (int j = 0; j < 3; ++j) { output_yaml << camera_matrix.at(i, j); @@ -36,7 +36,7 @@ void CameraIntrinsicParameters::storeYaml(const std::string& yaml_path) const { output_yaml << YAML::EndSeq; output_yaml << YAML::Key << "distorsion_coefs"; - output_yaml << YAML::Value << YAML::BeginSeq; + output_yaml << YAML::Value << YAML::Flow << YAML::BeginSeq; for (int i = 0; i < 5; ++i) { output_yaml << dist_coefs[i]; } @@ -59,7 +59,7 @@ CameraIntrinsicParameters CameraIntrinsicParameters::loadFromYaml(const std::str const auto yaml_camera_matrix = file["camera_matrix"].as>(); result.camera_matrix = cv::Mat(yaml_camera_matrix, true); - const auto coefs = file["distorsion_coefs"].as>(); + const auto coefs = file["distorsion_coefs"].as>(); result.dist_coefs = cv::Mat(coefs, true); result.initUndistortMaps(); @@ -67,6 +67,19 @@ CameraIntrinsicParameters CameraIntrinsicParameters::loadFromYaml(const std::str return result; } +CameraIntrinsicParameters CameraIntrinsicParameters::loadFromParams( + cv::Size param_image_size, const std::vector& param_camera_matrix, + const std::vector& param_dist_coefs) { + CameraIntrinsicParameters result{}; + + result.image_size = param_image_size; + result.camera_matrix = cv::Mat(param_camera_matrix, true); + result.dist_coefs = cv::Mat(param_dist_coefs, true); + result.initUndistortMaps(); + + return result; +} + void CameraIntrinsicParameters::initUndistortMaps() { // note that new camera matrix equals initial camera matrix // because neither scaling nor cropping is used when undistoring