implement individual exposure controller

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)

spinnaker_synchronized_camera_driver/include/spinnaker_synchronized_camera_driver/exposure_controller_factory.hpp

@@ -0,0 +1,38 @@
+// -*-c++-*--------------------------------------------------------------------
+// Copyright 2024 Bernd Pfrommer <bernd.pfrommer@gmail.com>
+//
+// 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 <memory>
+#include <string>
+
+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<spinnaker_camera_driver::ExposureController> 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_

spinnaker_synchronized_camera_driver/include/spinnaker_synchronized_camera_driver/individual_exposure_controller.hpp

@@ -0,0 +1,71 @@
+// -*-c++-*--------------------------------------------------------------------
+// Copyright 2024 Bernd Pfrommer <bernd.pfrommer@gmail.com>
+//
+// 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 <limits>
+#include <rclcpp/rclcpp.hpp>
+#include <spinnaker_camera_driver/exposure_controller.hpp>
+
+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<const spinnaker_camera_driver::Image> & img) final;
+  void addCamera(const std::shared_ptr<spinnaker_camera_driver::Camera> & 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 <class T>
+  T declare_param(const std::string & n, const T & def)
+  {
+    return (node_->declare_parameter<T>(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<float>::lowest()};
+  int numFramesSkip_{0};
+  int maxFramesSkip_{10};
+  bool gainPriority_{false};
+};
+}  // namespace spinnaker_synchronized_camera_driver
+#endif  // SPINNAKER_SYNCHRONIZED_CAMERA_DRIVER__INDIVIDUAL_EXPOSURE_CONTROLLER_HPP_

spinnaker_synchronized_camera_driver/include/spinnaker_synchronized_camera_driver/synchronized_camera_driver.hpp

@@ -24,6 +24,12 @@
 #include <spinnaker_camera_driver/camera.hpp>
 #include <spinnaker_synchronized_camera_driver/time_keeper.hpp>
 #include <thread>
+#include <unordered_map>
+
+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<image_transport::ImageTransport> imageTransport_;
@@ -48,6 +55,8 @@ class SynchronizedCameraDriver : public rclcpp::Node
   size_t numUpdatesRequired_{0};
   size_t numUpdatesReceived_{0};
   std::shared_ptr<TimeEstimator> timeEstimator_;
+  std::unordered_map<std::string, std::shared_ptr<spinnaker_camera_driver::ExposureController>>
+    exposureControllers_;
 };
 }  // namespace spinnaker_synchronized_camera_driver
 #endif  // SPINNAKER_SYNCHRONIZED_CAMERA_DRIVER__SYNCHRONIZED_CAMERA_DRIVER_HPP_

spinnaker_synchronized_camera_driver/src/exposure_controller_factory.cpp

@@ -0,0 +1,35 @@
+// -*-c++-*--------------------------------------------------------------------
+// Copyright 2024 Bernd Pfrommer <bernd.pfrommer@gmail.com>
+//
+// 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 <spinnaker_synchronized_camera_driver/exposure_controller_factory.hpp>
+#include <spinnaker_synchronized_camera_driver/individual_exposure_controller.hpp>
+#include <spinnaker_synchronized_camera_driver/logging.hpp>
+
+namespace spinnaker_synchronized_camera_driver
+{
+namespace exposure_controller_factory
+{
+static rclcpp::Logger get_logger() { return (rclcpp::get_logger("cam_sync")); }
+std::shared_ptr<spinnaker_camera_driver::ExposureController> newInstance(
+  const std::string & type, const std::string & name, rclcpp::Node * node)
+{
+  if (type == "individual") {
+    return (std::make_shared<IndividualExposureController>(name, node));
+  }
+  BOMB_OUT("unknown exposure controller type: " << type);
+  return (nullptr);
+}
+}  // namespace exposure_controller_factory
+}  // namespace spinnaker_synchronized_camera_driver