diff --git a/spinnaker_camera_driver/CMakeLists.txt b/spinnaker_camera_driver/CMakeLists.txt index 48cc12b8..59dbef39 100644 --- a/spinnaker_camera_driver/CMakeLists.txt +++ b/spinnaker_camera_driver/CMakeLists.txt @@ -15,7 +15,7 @@ # limitations under the License. cmake_minimum_required(VERSION 3.5) -project(spinnaker_camera_driver) +project(spinnaker_camera_driver VERSION 2.0.0) include(CMakePackageConfigHelpers) include(GNUInstallDirs) @@ -164,7 +164,7 @@ install(TARGETS # generate the ConfigVersion.cmake file that will be included by Config.cmake write_basic_package_version_file( ${PROJECT_NAME}ConfigVersion.cmake - VERSION ${PACKAGE_VERSION} + VERSION ${CMAKE_PROJECT_VERSION} COMPATIBILITY AnyNewerVersion) # generate the *Config.cmake file diff --git a/spinnaker_camera_driver/include/spinnaker_camera_driver/camera.hpp b/spinnaker_camera_driver/include/spinnaker_camera_driver/camera.hpp index 768de1fa..e01c02e3 100644 --- a/spinnaker_camera_driver/include/spinnaker_camera_driver/camera.hpp +++ b/spinnaker_camera_driver/include/spinnaker_camera_driver/camera.hpp @@ -35,10 +35,11 @@ namespace spinnaker_camera_driver { +class ExposureController; // forward decl class Camera { public: - typedef spinnaker_camera_driver::ImageConstPtr ImageConstPtr; + using ImageConstPtr = spinnaker_camera_driver::ImageConstPtr; explicit Camera( rclcpp::Node * node, image_transport::ImageTransport * it, const std::string & prefix, bool useStatus = true); @@ -47,6 +48,12 @@ class Camera bool start(); bool stop(); void setSynchronizer(const std::shared_ptr & s) { synchronizer_ = s; } + void setExposureController(const std::shared_ptr & e) + { + exposureController_ = e; + } + const std::string & getName() const { return (name_); } + const std::string & getPrefix() const { return (prefix_); } private: struct NodeInfo @@ -57,7 +64,7 @@ class Camera NodeType type{INVALID}; rcl_interfaces::msg::ParameterDescriptor descriptor; }; - void publishImage(const ImageConstPtr & image); + void processImage(const ImageConstPtr & image); void readParameters(); void printCameraInfo(); void startCamera(); @@ -127,6 +134,7 @@ class Camera bool autoExposure_; // if auto exposure is on/off bool dumpNodeMap_{false}; bool debug_{false}; + bool quiet_{false}; bool computeBrightness_{false}; double acquisitionTimeout_{3.0}; bool adjustTimeStamp_{false}; @@ -159,6 +167,7 @@ class Camera rclcpp::Time lastStatusTime_; int qosDepth_{4}; std::shared_ptr synchronizer_; + std::shared_ptr exposureController_; bool firstSynchronizedFrame_{true}; }; } // namespace spinnaker_camera_driver diff --git a/spinnaker_camera_driver/include/spinnaker_camera_driver/exposure_controller.hpp b/spinnaker_camera_driver/include/spinnaker_camera_driver/exposure_controller.hpp new file mode 100644 index 00000000..6321537f --- /dev/null +++ b/spinnaker_camera_driver/include/spinnaker_camera_driver/exposure_controller.hpp @@ -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. + +#ifndef SPINNAKER_CAMERA_DRIVER__EXPOSURE_CONTROLLER_HPP_ +#define SPINNAKER_CAMERA_DRIVER__EXPOSURE_CONTROLLER_HPP_ + +#include + +namespace spinnaker_camera_driver +{ +class Image; +class Camera; + +class ExposureController +{ +public: + ExposureController() = default; + virtual ~ExposureController() {} + virtual void update(Camera * cam, const std::shared_ptr & img) = 0; + virtual void addCamera(const std::shared_ptr & cam) = 0; +}; +} // namespace spinnaker_camera_driver +#endif // SPINNAKER_CAMERA_DRIVER__EXPOSURE_CONTROLLER_HPP_ diff --git a/spinnaker_camera_driver/include/spinnaker_camera_driver/synchronizer.hpp b/spinnaker_camera_driver/include/spinnaker_camera_driver/synchronizer.hpp index b5fe5e0e..e05a6640 100644 --- a/spinnaker_camera_driver/include/spinnaker_camera_driver/synchronizer.hpp +++ b/spinnaker_camera_driver/include/spinnaker_camera_driver/synchronizer.hpp @@ -17,7 +17,6 @@ #define SPINNAKER_CAMERA_DRIVER__SYNCHRONIZER_HPP_ #include -#include namespace spinnaker_camera_driver { diff --git a/spinnaker_camera_driver/launch/driver_node.launch.py b/spinnaker_camera_driver/launch/driver_node.launch.py index a08b0a7e..b1445a6b 100644 --- a/spinnaker_camera_driver/launch/driver_node.launch.py +++ b/spinnaker_camera_driver/launch/driver_node.launch.py @@ -122,7 +122,8 @@ def launch_setup(context, *args, **kwargs): output='screen', name=[LaunchConfig('camera_name')], parameters=[example_parameters[camera_type], - {'parameter_file': parameter_file, + {'ffmpeg_image_transport.encoding': 'hevc_nvenc', + 'parameter_file': parameter_file, 'serial_number': [LaunchConfig('serial')]}], remappings=[('~/control', '/exposure_control/control'), ]) diff --git a/spinnaker_camera_driver/src/camera.cpp b/spinnaker_camera_driver/src/camera.cpp index 9fed0e23..d8b3d67e 100644 --- a/spinnaker_camera_driver/src/camera.cpp +++ b/spinnaker_camera_driver/src/camera.cpp @@ -25,6 +25,7 @@ #include #include #include +#include #include #include @@ -192,11 +193,16 @@ void Camera::checkSubscriptions() void Camera::readParameters() { + quiet_ = safe_declare(prefix_ + "quiet", false); serial_ = safe_declare(prefix_ + "serial_number", "missing_serial_number"); - LOG_INFO("reading ros parameters for camera with serial: " << serial_); + if (!quiet_) { + LOG_INFO("reading ros parameters for camera with serial: " << serial_); + } debug_ = safe_declare(prefix_ + "debug", false); adjustTimeStamp_ = safe_declare(prefix_ + "adjust_timestamp", false); - LOG_INFO((adjustTimeStamp_ ? "" : "not ") << "adjusting time stamps!"); + if (!quiet_) { + LOG_INFO((adjustTimeStamp_ ? "" : "not ") << "adjusting time stamps!"); + } cameraInfoURL_ = safe_declare(prefix_ + "camerainfo_url", ""); frameId_ = safe_declare(prefix_ + "frame_id", node_->get_name()); @@ -258,7 +264,9 @@ void Camera::createCameraParameters() bool Camera::setEnum(const std::string & nodeName, const std::string & v) { - LOG_INFO("setting " << nodeName << " to: " << v); + if (!quiet_) { + LOG_INFO("setting " << nodeName << " to: " << v); + } std::string retV; // what actually was set std::string msg = wrapper_->setEnum(nodeName, v, &retV); bool status(true); @@ -275,7 +283,9 @@ bool Camera::setEnum(const std::string & nodeName, const std::string & v) bool Camera::setDouble(const std::string & nodeName, double v) { - LOG_INFO("setting " << nodeName << " to: " << v); + if (!quiet_) { + LOG_INFO("setting " << nodeName << " to: " << v); + } double retV; // what actually was set std::string msg = wrapper_->setDouble(nodeName, v, &retV); bool status(true); @@ -292,7 +302,9 @@ bool Camera::setDouble(const std::string & nodeName, double v) bool Camera::setInt(const std::string & nodeName, int v) { - LOG_INFO("setting " << nodeName << " to: " << v); + if (!quiet_) { + LOG_INFO("setting " << nodeName << " to: " << v); + } int retV; // what actually was set std::string msg = wrapper_->setInt(nodeName, v, &retV); bool status(true); @@ -309,7 +321,9 @@ bool Camera::setInt(const std::string & nodeName, int v) bool Camera::setBool(const std::string & nodeName, bool v) { - LOG_INFO("setting " << nodeName << " to: " << v); + if (!quiet_) { + LOG_INFO("setting " << nodeName << " to: " << v); + } bool retV; // what actually was set std::string msg = wrapper_->setBool(nodeName, v, &retV); bool status(true); @@ -435,7 +449,7 @@ void Camera::controlCallback(const flir_camera_msgs::msg::CameraControl::UniqueP } } -void Camera::publishImage(const ImageConstPtr & im) +void Camera::processImage(const ImageConstPtr & im) { { std::unique_lock lock(mutex_); @@ -468,6 +482,9 @@ void Camera::run() } // -------- end of locked section if (img && keepRunning_ && rclcpp::ok()) { doPublish(img); + if (exposureController_) { + exposureController_->update(this, img); + } } } } @@ -604,7 +621,7 @@ void Camera::startCamera() { if (!cameraRunning_) { spinnaker_camera_driver::SpinnakerWrapper::Callback cb = - std::bind(&Camera::publishImage, this, std::placeholders::_1); + std::bind(&Camera::processImage, this, std::placeholders::_1); cameraRunning_ = wrapper_->startCamera(cb); if (!cameraRunning_) { LOG_ERROR("failed to start camera!"); diff --git a/spinnaker_camera_driver/src/genicam_utils.cpp b/spinnaker_camera_driver/src/genicam_utils.cpp index 7a21f04d..4051a53a 100644 --- a/spinnaker_camera_driver/src/genicam_utils.cpp +++ b/spinnaker_camera_driver/src/genicam_utils.cpp @@ -46,14 +46,14 @@ void get_nodemap_as_string(std::stringstream & ss, Spinnaker::CameraPtr cam) ss << s; } -static CNodePtr find_node(const std::string & path, CNodePtr & node, bool debug) +static std::optional find_node(const std::string & path, CNodePtr & node, bool debug) { // split off first part auto pos = path.find("/"); const std::string token = path.substr(0, pos); // first part of it if (node->GetPrincipalInterfaceType() != intfICategory) { std::cerr << "no category node: " << node->GetName() << " vs " << path << std::endl; - return (NULL); + return (std::nullopt); } CCategoryPtr catNode = static_cast(node); @@ -70,28 +70,31 @@ static CNodePtr find_node(const std::string & path, CNodePtr & node, bool debug) std::cout << "checking child: " << childNode->GetName() << " vs " << token << std::endl; } if (std::string(childNode->GetName().c_str()) == token) { + // no slash in name, this is a leaf node + const bool is_leaf_node = (pos == std::string::npos); if (is_readable(childNode)) { - if (pos == std::string::npos) { // no slash in name, found leaf node + if (is_leaf_node) { return (childNode); } else { const std::string rest = path.substr(pos + 1); return (find_node(rest, childNode, debug)); } + } else { + return (CNodePtr(nullptr)); // found, but not readable } } } if (debug) { std::cerr << "driver: node not found: " << path << std::endl; } - return (CNodePtr(NULL)); + return (std::nullopt); } -CNodePtr find_node(const std::string & path, Spinnaker::CameraPtr cam, bool debug) +std::optional find_node(const std::string & path, Spinnaker::CameraPtr cam, bool debug) { INodeMap & appLayerNodeMap = cam->GetNodeMap(); CNodePtr rootNode = appLayerNodeMap.GetNode("Root"); - CNodePtr retNode = find_node(path, rootNode, debug); - return (retNode); + return (find_node(path, rootNode, debug)); } } // namespace genicam_utils } // namespace spinnaker_camera_driver diff --git a/spinnaker_camera_driver/src/genicam_utils.hpp b/spinnaker_camera_driver/src/genicam_utils.hpp index c43844e1..fdd5d9fc 100644 --- a/spinnaker_camera_driver/src/genicam_utils.hpp +++ b/spinnaker_camera_driver/src/genicam_utils.hpp @@ -19,6 +19,7 @@ #include #include +#include #include #include @@ -27,7 +28,7 @@ namespace spinnaker_camera_driver namespace genicam_utils { void get_nodemap_as_string(std::stringstream & ss, Spinnaker::CameraPtr cam); -Spinnaker::GenApi::CNodePtr find_node( +std::optional find_node( const std::string & path, Spinnaker::CameraPtr cam, bool debug); } // namespace genicam_utils } // namespace spinnaker_camera_driver diff --git a/spinnaker_camera_driver/src/spinnaker_wrapper_impl.cpp b/spinnaker_camera_driver/src/spinnaker_wrapper_impl.cpp index 0b682f26..2cb8be77 100644 --- a/spinnaker_camera_driver/src/spinnaker_wrapper_impl.cpp +++ b/spinnaker_camera_driver/src/spinnaker_wrapper_impl.cpp @@ -51,7 +51,8 @@ static bool common_checks( const GenApi::CNodePtr & np, const std::string & nodeName, std::string * msg) { if (!np.IsValid()) { - *msg = "node " + nodeName + " does not exist!"; + // nullptr means node was found, but is greyed out + *msg = "node " + nodeName + " exists but is not accessible!"; return (false); } if (!is_available(np)) { @@ -143,12 +144,15 @@ std::string SpinnakerWrapperImpl::setEnum( const std::string & nodeName, const std::string & val, std::string * retVal) { *retVal = "UNKNOWN"; - GenApi::CNodePtr np = genicam_utils::find_node(nodeName, camera_, debug_); + const auto np = genicam_utils::find_node(nodeName, camera_, debug_); + if (!np) { + return ("node " + nodeName + " not found!"); + } std::string msg; - if (!common_checks(np, nodeName, &msg)) { + if (!common_checks(*np, nodeName, &msg)) { return (msg); } - GenApi::CEnumerationPtr p = static_cast(np); + GenApi::CEnumerationPtr p = static_cast(*np); if (!is_writable(p)) { return ("node " + nodeName + " not enum???"); } @@ -206,14 +210,17 @@ static std::string set_parameter( const std::string & nodeName, T2 val, T2 * retVal, const Spinnaker::CameraPtr & cam, bool debug) { *retVal = set_invalid(); - GenApi::CNodePtr np = genicam_utils::find_node(nodeName, cam, debug); + const auto np = genicam_utils::find_node(nodeName, cam, debug); + if (!np) { + return ("node " + nodeName + " not found!"); + } std::string msg; - if (!common_checks(np, nodeName, &msg)) { + if (!common_checks(*np, nodeName, &msg)) { return (msg); } - T1 p = static_cast(np); + T1 p = static_cast(*np); p->SetValue(val); - if (!is_readable(np)) { + if (!is_readable(*np)) { return ("node " + nodeName + " current entry not readable!"); } *retVal = p->GetValue(); 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/cmake/FindSPINNAKER.cmake b/spinnaker_synchronized_camera_driver/cmake/FindSPINNAKER.cmake index 13802b00..b4bb82b4 100644 --- a/spinnaker_synchronized_camera_driver/cmake/FindSPINNAKER.cmake +++ b/spinnaker_synchronized_camera_driver/cmake/FindSPINNAKER.cmake @@ -36,6 +36,7 @@ find_library(SPINNAKER_LIBRARIES /opt/spinnaker/lib /usr/lib/ /usr/local/lib + NO_DEFAULT_PATH # else it will find ROS system spinnaker libraries first PATH_SUFFIXES Release Debug ) 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 d31c75fc..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,17 +43,20 @@ class SynchronizedCameraDriver : public rclcpp::Node private: void createCameras(); + void createExposureControllers(); void printStatus(); // ----- variables -- std::shared_ptr imageTransport_; std::map> cameras_; std::vector> timeKeepers_; rclcpp::TimerBase::SharedPtr statusTimer_; - double avgFrameInterval_{0}; + double avgFrameInterval_{-1}; std::mutex mutex_; 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/include/spinnaker_synchronized_camera_driver/time_estimator.hpp b/spinnaker_synchronized_camera_driver/include/spinnaker_synchronized_camera_driver/time_estimator.hpp index 2ada44a2..7dd6cc62 100644 --- a/spinnaker_synchronized_camera_driver/include/spinnaker_synchronized_camera_driver/time_estimator.hpp +++ b/spinnaker_synchronized_camera_driver/include/spinnaker_synchronized_camera_driver/time_estimator.hpp @@ -44,7 +44,7 @@ class TimeEstimator sumFrameTime_ += static_cast(t) * 1e-9; numFrames_++; } - bool isValid() const { return (numFrames_ > 1); } + bool isValid() const { return (numFrames_ > 0); } double getAverageFrameTime() const { return (numFrames_ > 0 ? sumFrameTime_ / static_cast(numFrames_) : 0); 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 0bcd183b..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; } @@ -109,8 +136,9 @@ bool SynchronizedCameraDriver::update( std::unique_lock lock(mutex_); constexpr double NUM_FRAMES_TO_AVG = 20.0; constexpr double alpha = 1.0 / NUM_FRAMES_TO_AVG; + dt = std::max(1e-6, dt); avgFrameInterval_ = - (avgFrameInterval_ == 0) ? dt : (avgFrameInterval_ * (1.0 - alpha) + alpha * dt); + (avgFrameInterval_ < 0) ? dt : (avgFrameInterval_ * (1.0 - alpha) + alpha * dt); if (numUpdatesReceived_ < numUpdatesRequired_) { numUpdatesReceived_++; if (numUpdatesReceived_ >= numUpdatesRequired_) { diff --git a/spinnaker_synchronized_camera_driver/src/time_estimator.cpp b/spinnaker_synchronized_camera_driver/src/time_estimator.cpp index eca75e86..5396f02b 100644 --- a/spinnaker_synchronized_camera_driver/src/time_estimator.cpp +++ b/spinnaker_synchronized_camera_driver/src/time_estimator.cpp @@ -16,6 +16,8 @@ #include #include +// #define DEBUG + static rclcpp::Logger get_logger() { return (rclcpp::get_logger("cam_sync")); } namespace spinnaker_synchronized_camera_driver @@ -112,12 +114,18 @@ bool TimeEstimator::getTimeFromList(uint64_t t_a, uint64_t * T) // this frame is older than the oldest frame time but still within range (*frameTimes_.begin()).addTime(t); *T = static_cast(T_min) + T0_; +#ifdef DEBUG + LOG_INFO("frame old but within range"); +#endif return (true); } // make sure to add new frames if needed for (auto ft = *(frameTimes_.rbegin()); (t - ft.getFrameTime()) * 2 > dT; ft = *(frameTimes_.rbegin())) { + // if a new frame is found, perform a measurement update + // based on the average arrival time of the last frames that + // have arrived before the new frame if (ft.isValid()) { updateKalman(ft.getAverageFrameTime()); } @@ -132,10 +140,13 @@ bool TimeEstimator::getTimeFromList(uint64_t t_a, uint64_t * T) const int64_t T_max = (*frameTimes_.rbegin()).getFrameTime(); const auto dT_max = static_cast(t) - T_max; - if (dT_max > 0 && dT_max * 2 <= dT) { + if (dT_max >= 0 && dT_max * 2 <= dT) { // this frame is younger than the latest frame time but still in range (*frameTimes_.rbegin()).addTime(t); *T = static_cast(T_max) + T0_; +#ifdef DEBUG + LOG_INFO("frame young but within range"); +#endif return (true); } @@ -154,11 +165,17 @@ bool TimeEstimator::getTimeFromList(uint64_t t_a, uint64_t * T) // closer to beginning of interval (*it).addTime(t); *T = static_cast(t1) + T0_; +#ifdef DEBUG + LOG_INFO("frame closer to begin of interval"); +#endif return (true); } else { // closer to end of interval (*itp1).addTime(t); *T = static_cast(t2) + T0_; +#ifdef DEBUG + LOG_INFO("frame closer to end of interval"); +#endif return (true); } } @@ -169,12 +186,18 @@ bool TimeEstimator::getTimeFromList(uint64_t t_a, uint64_t * T) // never be reached. LOG_ERROR("INTERNAL BUG, should never reach this point!!"); *T = static_cast(T_max) + T0_; + LOG_INFO_FMT("newly added time: %8ld", t_a); + for (const auto & tl : frameTimes_) { + LOG_INFO_FMT(" %8ld", tl.getFrameTime()); + } return (true); } bool TimeEstimator::update(size_t idx, uint64_t t_a, uint64_t * frameTime) { (void)idx; - return (getTimeFromList(t_a - T0_, frameTime)); + 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 diff --git a/spinnaker_synchronized_camera_driver/src/time_keeper.cpp b/spinnaker_synchronized_camera_driver/src/time_keeper.cpp index 525ec5d6..7a53d90f 100644 --- a/spinnaker_synchronized_camera_driver/src/time_keeper.cpp +++ b/spinnaker_synchronized_camera_driver/src/time_keeper.cpp @@ -24,32 +24,39 @@ namespace spinnaker_synchronized_camera_driver bool TimeKeeper::getTimeStamp(uint64_t hostTime, uint64_t, uint64_t frameId, uint64_t * frameTime) { + if (lastHostTime_ == 0) { + lastFrameId_ = frameId; + lastHostTime_ = hostTime; + return (false); + } const int64_t gap = frameId - lastFrameId_; const int64_t dt64 = static_cast(hostTime) - static_cast(lastHostTime_); lastFrameId_ = frameId; lastHostTime_ = hostTime; + numFramesDropped_ += std::max(0, gap - 1); if (gap > 0 && gap < 4) { // ignore all but none, 1 or 2 frames dropped if (gap != 1) { LOG_WARN(name_ << " dropped " << gap - 1 << " frame(s)"); } const double dt = dt64 * 1e-9 / static_cast(gap); - bool gotTime = driver_->update(index_, hostTime, dt, frameTime); - const double offset = - (static_cast(hostTime) - static_cast(*frameTime)) * 1e-9; - offsetSum_ += offset; - // running variance is computed following this reference: - // https://www.johndcook.com/blog/standard_deviation/ - if (numOffset_ == 0) { - M_ = offset; - S_ = 0; - } else { - const double M_km1 = M_; - M_ += (offset - M_) / static_cast(numOffset_ + 1); - S_ += (offset - M_km1) * (offset - M_); + const bool gotTime = driver_->update(index_, hostTime, dt, frameTime); + if (gotTime) { + const double offset = + (static_cast(hostTime) - static_cast(*frameTime)) * 1e-9; + offsetSum_ += offset; + // running variance is computed following this reference: + // https://www.johndcook.com/blog/standard_deviation/ + if (numOffset_ == 0) { + M_ = offset; + S_ = 0; + } else { + const double M_km1 = M_; + M_ += (offset - M_) / static_cast(numOffset_ + 1); + S_ += (offset - M_km1) * (offset - M_); + } + numOffset_++; } - numOffset_++; - return (gotTime); } else { if (frameId != 0) {