From cb35a33e187aae87942ea97e6e0ac73d726ee45e Mon Sep 17 00:00:00 2001
From: Bernd Pfrommer <bernd.pfrommer@gmail.com>
Date: Thu, 29 Feb 2024 07:32:37 -0500
Subject: [PATCH] provision camera driver for exposure control

---
 .../spinnaker_camera_driver/camera.hpp        | 12 +++++--
 .../exposure_controller.hpp                   | 35 +++++++++++++++++++
 .../spinnaker_camera_driver/synchronizer.hpp  |  1 -
 spinnaker_camera_driver/src/camera.cpp        |  8 +++--
 4 files changed, 51 insertions(+), 5 deletions(-)
 create mode 100644 spinnaker_camera_driver/include/spinnaker_camera_driver/exposure_controller.hpp

diff --git a/spinnaker_camera_driver/include/spinnaker_camera_driver/camera.hpp b/spinnaker_camera_driver/include/spinnaker_camera_driver/camera.hpp
index 4a5a4e78..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<Synchronizer> & s) { synchronizer_ = s; }
+  void setExposureController(const std::shared_ptr<ExposureController> & 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();
@@ -160,6 +167,7 @@ class Camera
   rclcpp::Time lastStatusTime_;
   int qosDepth_{4};
   std::shared_ptr<Synchronizer> synchronizer_;
+  std::shared_ptr<ExposureController> 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 <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_CAMERA_DRIVER__EXPOSURE_CONTROLLER_HPP_
+#define SPINNAKER_CAMERA_DRIVER__EXPOSURE_CONTROLLER_HPP_
+
+#include <memory>
+
+namespace spinnaker_camera_driver
+{
+class Image;
+class Camera;
+
+class ExposureController
+{
+public:
+  ExposureController() = default;
+  virtual ~ExposureController() {}
+  virtual void update(Camera * cam, const std::shared_ptr<const Image> & img) = 0;
+  virtual void addCamera(const std::shared_ptr<Camera> & 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 <cstdint>
-#include <rclcpp/rclcpp.hpp>
 
 namespace spinnaker_camera_driver
 {
diff --git a/spinnaker_camera_driver/src/camera.cpp b/spinnaker_camera_driver/src/camera.cpp
index ff58a05a..d8b3d67e 100644
--- a/spinnaker_camera_driver/src/camera.cpp
+++ b/spinnaker_camera_driver/src/camera.cpp
@@ -25,6 +25,7 @@
 #include <sensor_msgs/fill_image.hpp>
 #include <sensor_msgs/image_encodings.hpp>
 #include <spinnaker_camera_driver/camera_driver.hpp>
+#include <spinnaker_camera_driver/exposure_controller.hpp>
 #include <spinnaker_camera_driver/logging.hpp>
 #include <type_traits>
 
@@ -448,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<std::mutex> lock(mutex_);
@@ -481,6 +482,9 @@ void Camera::run()
       }  // -------- end of locked section
       if (img && keepRunning_ && rclcpp::ok()) {
         doPublish(img);
+        if (exposureController_) {
+          exposureController_->update(this, img);
+        }
       }
     }
   }
@@ -617,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!");