diff --git a/spinnaker_synchronized_camera_driver/CMakeLists.txt b/spinnaker_synchronized_camera_driver/CMakeLists.txt index 3757a9af..6e03bcd4 100644 --- a/spinnaker_synchronized_camera_driver/CMakeLists.txt +++ b/spinnaker_synchronized_camera_driver/CMakeLists.txt @@ -83,7 +83,9 @@ cmake_print_properties(TARGETS spinnaker_camera_driver::camera_driver PROPERTIES add_library(synchronized_camera_driver SHARED src/synchronized_camera_driver.cpp src/time_estimator.cpp - src/time_keeper.cpp) + src/time_keeper.cpp + src/exposure_controller_factory.cpp + src/individual_exposure_controller.cpp) ament_target_dependencies(synchronized_camera_driver PUBLIC ${ROS_DEPENDENCIES}) target_link_libraries(synchronized_camera_driver PUBLIC spinnaker_camera_driver::camera_driver PRIVATE yaml-cpp) diff --git a/spinnaker_synchronized_camera_driver/include/spinnaker_synchronized_camera_driver/exposure_controller_factory.hpp b/spinnaker_synchronized_camera_driver/include/spinnaker_synchronized_camera_driver/exposure_controller_factory.hpp new file mode 100644 index 00000000..bba1a427 --- /dev/null +++ b/spinnaker_synchronized_camera_driver/include/spinnaker_synchronized_camera_driver/exposure_controller_factory.hpp @@ -0,0 +1,38 @@ +// -*-c++-*-------------------------------------------------------------------- +// Copyright 2024 Bernd Pfrommer +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SPINNAKER_SYNCHRONIZED_CAMERA_DRIVER__EXPOSURE_CONTROLLER_FACTORY_HPP_ +#define SPINNAKER_SYNCHRONIZED_CAMERA_DRIVER__EXPOSURE_CONTROLLER_FACTORY_HPP_ + +#include +#include + +namespace spinnaker_camera_driver +{ +class ExposureController; // forward decl +} +namespace rclcpp +{ +class Node; // forward decl +} +namespace spinnaker_synchronized_camera_driver +{ +namespace exposure_controller_factory +{ +std::shared_ptr newInstance( + const std::string & type, const std::string & name, rclcpp::Node * node); +} // namespace exposure_controller_factory +} // namespace spinnaker_synchronized_camera_driver +#endif // SPINNAKER_SYNCHRONIZED_CAMERA_DRIVER__EXPOSURE_CONTROLLER_FACTORY_HPP_ diff --git a/spinnaker_synchronized_camera_driver/include/spinnaker_synchronized_camera_driver/individual_exposure_controller.hpp b/spinnaker_synchronized_camera_driver/include/spinnaker_synchronized_camera_driver/individual_exposure_controller.hpp new file mode 100644 index 00000000..200a7a3f --- /dev/null +++ b/spinnaker_synchronized_camera_driver/include/spinnaker_synchronized_camera_driver/individual_exposure_controller.hpp @@ -0,0 +1,71 @@ +// -*-c++-*-------------------------------------------------------------------- +// Copyright 2024 Bernd Pfrommer +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SPINNAKER_SYNCHRONIZED_CAMERA_DRIVER__INDIVIDUAL_EXPOSURE_CONTROLLER_HPP_ +#define SPINNAKER_SYNCHRONIZED_CAMERA_DRIVER__INDIVIDUAL_EXPOSURE_CONTROLLER_HPP_ + +#include +#include +#include + +namespace spinnaker_synchronized_camera_driver +{ +class IndividualExposureController : public spinnaker_camera_driver::ExposureController +{ +public: + explicit IndividualExposureController(const std::string & name, rclcpp::Node * n); + void update( + spinnaker_camera_driver::Camera * cam, + const std::shared_ptr & img) final; + void addCamera(const std::shared_ptr & cam) final; + +private: + double calculateGain(double brightRatio) const; + double calculateExposureTime(double brightRatio) const; + bool changeExposure(double brightRatio, double minTime, double maxTime, const char * debugMsg); + bool changeGain(double brightRatio, double minGain, double maxGain, const char * debugMsg); + bool updateExposureWithGainPriority(double brightRatio); + bool updateExposureWithTimePriority(double brightRatio); + bool updateExposure(double b); + rclcpp::Logger get_logger() { return (rclcpp::get_logger(cameraName_)); } + + template + T declare_param(const std::string & n, const T & def) + { + return (node_->declare_parameter(name_ + "." + n, def)); + } + + // ----------------- variables -------------------- + std::string name_; + std::string cameraName_; + rclcpp::Node * node_{0}; + int32_t targetBrightness_{128}; + std::string exposureParameterName_; + std::string gainParameterName_; + + int brightnessTarget_{128}; + int brightnessTolerance_{5}; + double maxExposureTime_{1000}; + double minExposureTime_{0}; + double maxGain_{30}; + int currentBrightness_; + double currentExposureTime_{0}; + double currentGain_{std::numeric_limits::lowest()}; + int numFramesSkip_{0}; + int maxFramesSkip_{10}; + bool gainPriority_{false}; +}; +} // namespace spinnaker_synchronized_camera_driver +#endif // SPINNAKER_SYNCHRONIZED_CAMERA_DRIVER__INDIVIDUAL_EXPOSURE_CONTROLLER_HPP_ diff --git a/spinnaker_synchronized_camera_driver/include/spinnaker_synchronized_camera_driver/synchronized_camera_driver.hpp b/spinnaker_synchronized_camera_driver/include/spinnaker_synchronized_camera_driver/synchronized_camera_driver.hpp index 29fd4917..12206d26 100644 --- a/spinnaker_synchronized_camera_driver/include/spinnaker_synchronized_camera_driver/synchronized_camera_driver.hpp +++ b/spinnaker_synchronized_camera_driver/include/spinnaker_synchronized_camera_driver/synchronized_camera_driver.hpp @@ -24,6 +24,12 @@ #include #include #include +#include + +namespace spinnaker_camera_driver +{ +class ExposureController; +} namespace spinnaker_synchronized_camera_driver { @@ -37,6 +43,7 @@ class SynchronizedCameraDriver : public rclcpp::Node private: void createCameras(); + void createExposureControllers(); void printStatus(); // ----- variables -- std::shared_ptr imageTransport_; @@ -48,6 +55,8 @@ class SynchronizedCameraDriver : public rclcpp::Node size_t numUpdatesRequired_{0}; size_t numUpdatesReceived_{0}; std::shared_ptr timeEstimator_; + std::unordered_map> + exposureControllers_; }; } // namespace spinnaker_synchronized_camera_driver #endif // SPINNAKER_SYNCHRONIZED_CAMERA_DRIVER__SYNCHRONIZED_CAMERA_DRIVER_HPP_ diff --git a/spinnaker_synchronized_camera_driver/src/exposure_controller_factory.cpp b/spinnaker_synchronized_camera_driver/src/exposure_controller_factory.cpp new file mode 100644 index 00000000..2619926c --- /dev/null +++ b/spinnaker_synchronized_camera_driver/src/exposure_controller_factory.cpp @@ -0,0 +1,35 @@ +// -*-c++-*-------------------------------------------------------------------- +// Copyright 2024 Bernd Pfrommer +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include + +namespace spinnaker_synchronized_camera_driver +{ +namespace exposure_controller_factory +{ +static rclcpp::Logger get_logger() { return (rclcpp::get_logger("cam_sync")); } +std::shared_ptr newInstance( + const std::string & type, const std::string & name, rclcpp::Node * node) +{ + if (type == "individual") { + return (std::make_shared(name, node)); + } + BOMB_OUT("unknown exposure controller type: " << type); + return (nullptr); +} +} // namespace exposure_controller_factory +} // namespace spinnaker_synchronized_camera_driver diff --git a/spinnaker_synchronized_camera_driver/src/individual_exposure_controller.cpp b/spinnaker_synchronized_camera_driver/src/individual_exposure_controller.cpp new file mode 100644 index 00000000..458e1d1e --- /dev/null +++ b/spinnaker_synchronized_camera_driver/src/individual_exposure_controller.cpp @@ -0,0 +1,254 @@ +// -*-c++-*-------------------------------------------------------------------- +// Copyright 2024 Bernd Pfrommer +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +// #define DEBUG + +namespace spinnaker_synchronized_camera_driver +{ + +IndividualExposureController::IndividualExposureController( + const std::string & name, rclcpp::Node * node) +: name_(name), node_(node) +{ + exposureParameterName_ = declare_param("exposure_parameter", "exposure_time"); + gainParameterName_ = declare_param("gain_parameter", "gain"); + brightnessTarget_ = std::min(std::max(declare_param("brightness_target", 120), 1), 255); + currentBrightness_ = brightnessTarget_; + brightnessTolerance_ = declare_param("brightness_tolerance", 5); + maxExposureTime_ = std::max(declare_param("max_exposure_time", 1000), 1); + minExposureTime_ = std::max(declare_param("min_exposure_time", 10), 1); + maxGain_ = declare_param("max_gain", 10); + gainPriority_ = declare_param("gain_priority", false); + maxFramesSkip_ = declare_param("min_frames_skip", 10); // number of frames to wait +} + +double IndividualExposureController::calculateGain(double brightRatio) const +{ + // because gain is in db: + // db(G) = 10 * log_10(G) = 10 * ln(G) / ln(10) = 4.34 * ln(G) + const double kp = 4.34; + const double desiredGain = currentGain_ + kp * log(brightRatio); + const double cappedGain = std::max(std::min(desiredGain, maxGain_), 0.0); + // below threshold set to zero because it can no longer be set accurately + // at the camera + return (cappedGain > 0.5 ? cappedGain : 0); +} + +double IndividualExposureController::calculateExposureTime(double brightRatio) const +{ + const double desiredExposureTime = currentExposureTime_ * brightRatio; + const double optTime = std::max(0.0, std::min(desiredExposureTime, maxExposureTime_)); + return (optTime); +} + +bool IndividualExposureController::changeExposure( + double brightRatio, double minTime, double maxTime, const char * debugMsg) +{ + const double optTime = std::min(std::max(calculateExposureTime(brightRatio), minTime), maxTime); + if (currentExposureTime_ != optTime) { + currentExposureTime_ = optTime; +#ifdef DEBUG + LOG_INFO(debugMsg); +#else + (void)debugMsg; +#endif + return (true); + } + return (false); +} + +bool IndividualExposureController::changeGain( + double brightRatio, double minGain, double maxGain, const char * debugMsg) +{ + const double optGain = std::min(std::max(calculateGain(brightRatio), minGain), maxGain); + if (optGain != currentGain_) { + currentGain_ = optGain; +#ifdef DEBUG + LOG_INFO(debugMsg); +#else + (void)debugMsg; +#endif + return (true); + } + return (false); +} + +bool IndividualExposureController::updateExposureWithGainPriority(double brightRatio) +{ + if (brightRatio < 1) { // image is too bright + if (currentGain_ > 0) { + // if gain is nonzero, dial it back before touching exposure times + if (changeGain(brightRatio, 0, maxGain_, "gp: --gain!")) { + return (true); + } + } else { + // gain is already at zero, reduce exposure time + if (changeExposure(brightRatio, 0, maxExposureTime_, "gp: --time!")) { + return (true); + } + } + } else { // image is too dark + if (currentExposureTime_ < maxExposureTime_) { + // first try bumping the exposure time + if (changeExposure(brightRatio, 0, maxExposureTime_, "gp: ++time!")) { + return (true); + } + } else { + // try bumping gain if exposure time is at limit + if (changeGain(brightRatio, 0, maxGain_, "gp: ++gain!")) { + return (true); + } + } + } + return (false); +} + +bool IndividualExposureController::updateExposureWithTimePriority(double brightRatio) +{ + if (brightRatio < 1) { // image is too bright + if (currentExposureTime_ > minExposureTime_) { + // first try to shorten the exposure time + if (changeExposure(brightRatio, minExposureTime_, maxExposureTime_, "tp: cut time!")) { + return (true); + } + } else { + // already have reached minimum exposure, try reducing gain + if (currentGain_ > 0) { + if (changeGain(brightRatio, 0, maxGain_, "tp: cut gain")) { + return (true); + } + } else { + // gain is already at zero, must reduce exposure below min + if (changeExposure(brightRatio, 0, maxExposureTime_, "tp: cut time below min!")) { + return (true); + } + } + } + } else { // image is too dark + // if current exposure time is below minimum, bump it + if (currentExposureTime_ < minExposureTime_) { + if (changeExposure(brightRatio, 0, minExposureTime_, "tp: bump time from min!")) { + return (true); + } + } else { + // next try bumping the gain + if (currentGain_ < maxGain_) { + if (changeGain(brightRatio, 0, maxGain_, "tp: bump gain")) { + return (true); + } + } else { + // already have max gain, must bump exposure time + if (changeExposure(brightRatio, minExposureTime_, maxExposureTime_, "tp: ++time!")) { + return (true); + } + } + } + } + return (false); +} + +bool IndividualExposureController::updateExposure(double b) +{ + const double err_b = (brightnessTarget_ - b); + // the current gain is higher than it should be, let's + // set it to zero + if (currentGain_ > maxGain_) { + currentGain_ = 0; + return (true); + } + + // the current exposure is longer than it should be, let's + // set it to a good value and retry + if (currentExposureTime_ > maxExposureTime_) { + currentExposureTime_ = maxExposureTime_; + return (true); + } + if (fabs(err_b) <= brightnessTolerance_) { + // no need to change anything! + return (false); + } + // ratio between desired and actual brightness + const double brightRatio = std::max(std::min(brightnessTarget_ / b, 10.0), 0.1); + + if (gainPriority_) { + return (updateExposureWithGainPriority(brightRatio)); + } else { + return (updateExposureWithTimePriority(brightRatio)); + } + return (false); +} + +void IndividualExposureController::update( + spinnaker_camera_driver::Camera * cam, + const std::shared_ptr & img) +{ + const int b = std::min(std::max(1, static_cast(img->brightness_)), 255); + // if the exposure parameters are not set yet, set them now + if (currentExposureTime_ == 0) { + currentExposureTime_ = static_cast(img->exposureTime_); + } + if (currentGain_ == std::numeric_limits::lowest()) { + currentGain_ = img->gain_; + } +#if 0 + LOG_INFO( + "img: " << img->exposureTime_ << "/" << currentExposureTime_ << " gain: " << img->gain_ << "/" + << currentGain_ << " brightness: " << b); +#endif + + // check if the reported exposure and brightness settings + // match ours. That means the changes have taken effect + // on the driver side, and the brightness measurement can be + // used right away. + if ( + fabs(currentGain_ - img->gain_) <= 0.05 * (currentGain_ + img->gain_) && + fabs(currentExposureTime_ - static_cast(img->exposureTime_)) <= + 0.05 * (currentExposureTime_ + static_cast(img->exposureTime_)) && + numFramesSkip_ < maxFramesSkip_) { + numFramesSkip_ = 0; // no skipping anymore! + } + + if (numFramesSkip_ > 0) { + // Changes in gain or shutter take a few + // frames to arrive at the camera, so we skip those. + numFramesSkip_--; + } else { + const double oldExposureTime = currentExposureTime_; + const double oldGain = currentGain_; + if (updateExposure(b)) { + LOG_INFO( + "bright " << b << " at time/gain: [" << oldExposureTime << " " << oldGain << "] new: [" + << currentExposureTime_ << " " << currentGain_ << "]"); + numFramesSkip_ = maxFramesSkip_; // restart frame skipping + const auto expName = cam->getPrefix() + exposureParameterName_; + node_->set_parameter(rclcpp::Parameter(expName, currentExposureTime_)); + const auto gainName = cam->getPrefix() + gainParameterName_; + node_->set_parameter(rclcpp::Parameter(gainName, currentGain_)); + } + } +} + +void IndividualExposureController::addCamera( + const std::shared_ptr & cam) +{ + cameraName_ = cam->getName(); +}; + +} // namespace spinnaker_synchronized_camera_driver diff --git a/spinnaker_synchronized_camera_driver/src/synchronized_camera_driver.cpp b/spinnaker_synchronized_camera_driver/src/synchronized_camera_driver.cpp index dc1a4265..b2c44afc 100644 --- a/spinnaker_synchronized_camera_driver/src/synchronized_camera_driver.cpp +++ b/spinnaker_synchronized_camera_driver/src/synchronized_camera_driver.cpp @@ -1,5 +1,5 @@ // -*-c++-*-------------------------------------------------------------------- -// Copyright 2023 Bernd Pfrommer +// Copyright 2024 Bernd Pfrommer // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -16,6 +16,8 @@ #include #include #include +#include +#include #include #include #include @@ -28,6 +30,7 @@ SynchronizedCameraDriver::SynchronizedCameraDriver(const rclcpp::NodeOptions & o { imageTransport_ = std::make_shared( std::shared_ptr(this, [](auto *) {})); + createExposureControllers(); // before cams so they can refer to it createCameras(); // start cameras only when all synchronizer state has been set up! for (auto & c : cameras_) { @@ -84,6 +87,21 @@ void SynchronizedCameraDriver::printStatus() } } +void SynchronizedCameraDriver::createExposureControllers() +{ + using svec = std::vector; + const svec controllers = this->declare_parameter("exposure_controllers", svec()); + for (const auto & c : controllers) { + const std::string type = this->declare_parameter(c + ".type", ""); + if (!type.empty()) { + exposureControllers_.insert({c, exposure_controller_factory::newInstance(type, c, this)}); + LOG_INFO("created exposure controller: " << c); + } else { + BOMB_OUT("no controller type specified for controller " << c); + } + } +} + void SynchronizedCameraDriver::createCameras() { using svec = std::vector; @@ -91,7 +109,6 @@ void SynchronizedCameraDriver::createCameras() if (cameras.empty()) { BOMB_OUT("no cameras configured for synchronized driver!"); } - for (size_t i = 0; i < cameras.size(); i++) { const auto & c = cameras[i]; auto cam = @@ -99,6 +116,16 @@ void SynchronizedCameraDriver::createCameras() cameras_.insert({c, cam}); timeKeepers_.push_back(std::make_shared(i, c, this)); cam->setSynchronizer(timeKeepers_.back()); + // set exposure controller if configured + const auto ctrlName = this->declare_parameter(c + ".exposure_controller_name", ""); + if (!ctrlName.empty()) { + auto it = exposureControllers_.find(ctrlName); + if (it == exposureControllers_.end()) { + BOMB_OUT("unknown exposure controller: " << ctrlName); + } + it->second->addCamera(cam); + cam->setExposureController(it->second); + } } numUpdatesRequired_ = cameras.size() * 3; } diff --git a/spinnaker_synchronized_camera_driver/src/time_estimator.cpp b/spinnaker_synchronized_camera_driver/src/time_estimator.cpp index b6af2a6e..5396f02b 100644 --- a/spinnaker_synchronized_camera_driver/src/time_estimator.cpp +++ b/spinnaker_synchronized_camera_driver/src/time_estimator.cpp @@ -196,16 +196,8 @@ bool TimeEstimator::getTimeFromList(uint64_t t_a, uint64_t * T) bool TimeEstimator::update(size_t idx, uint64_t t_a, uint64_t * frameTime) { (void)idx; - const bool gotValidTime = getTimeFromList(t_a - T0_, frameTime); - if ( - gotValidTime && - std::abs(static_cast(*frameTime) - static_cast(t_a)) > 1000000000LL) { - std::cout << "bad time: " << t_a << " vs ft: " << *frameTime << std::endl; - LOG_INFO_FMT("time list looks like this"); - for (const auto & tl : frameTimes_) { - LOG_INFO_FMT(" %8ld", tl.getFrameTime()); - } - } + const uint64_t t_a_0 = (t_a >= T0_) ? (t_a - T0_) : 0; + const bool gotValidTime = getTimeFromList(t_a_0, frameTime); return (gotValidTime); } } // namespace spinnaker_synchronized_camera_driver