bug fixes and auto exposure for synchronized camera driver

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

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();
@@ -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> synchronizer_;
+  std::shared_ptr<ExposureController> exposureController_;
   bool firstSynchronizedFrame_{true};
 };
 }  // namespace spinnaker_camera_driver

spinnaker_camera_driver/include/spinnaker_camera_driver/exposure_controller.hpp

@@ -0,0 +1,35 @@
+// -*-c++-*--------------------------------------------------------------------
+// Copyright 2024 Bernd Pfrommer <[email protected]>
+//
+// 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_

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
 {

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'), ])
 

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>
 
@@ -192,11 +193,16 @@ void Camera::checkSubscriptions()
 
 void Camera::readParameters()
 {
+  quiet_ = safe_declare<bool>(prefix_ + "quiet", false);
   serial_ = safe_declare<std::string>(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<bool>(prefix_ + "debug", false);
   adjustTimeStamp_ = safe_declare<bool>(prefix_ + "adjust_timestamp", false);
-  LOG_INFO((adjustTimeStamp_ ? "" : "not ") << "adjusting time stamps!");
+  if (!quiet_) {
+    LOG_INFO((adjustTimeStamp_ ? "" : "not ") << "adjusting time stamps!");
+  }
 
   cameraInfoURL_ = safe_declare<std::string>(prefix_ + "camerainfo_url", "");
   frameId_ = safe_declare<std::string>(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<std::mutex> 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!");

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<CNodePtr> 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<CCategoryPtr>(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<CNodePtr> 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

spinnaker_camera_driver/src/genicam_utils.hpp

@@ -19,6 +19,7 @@
 #include <SpinGenApi/SpinnakerGenApi.h>
 #include <Spinnaker.h>
 
+#include <optional>
 #include <sstream>
 #include <string>
 
@@ -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<Spinnaker::GenApi::CNodePtr> find_node(
   const std::string & path, Spinnaker::CameraPtr cam, bool debug);
 }  // namespace genicam_utils
 }  // namespace spinnaker_camera_driver

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<GenApi::CEnumerationPtr>(np);
+  GenApi::CEnumerationPtr p = static_cast<GenApi::CEnumerationPtr>(*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<T2>();
-  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<T1>(np);
+  T1 p = static_cast<T1>(*np);
   p->SetValue(val);
-  if (!is_readable(np)) {
+  if (!is_readable(*np)) {
     return ("node " + nodeName + " current entry not readable!");
   }
   *retVal = p->GetValue();

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/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
 )