added lifecycle node support, handling of acquisition erros

spinnaker_camera_driver/CMakeLists.txt

@@ -26,7 +26,7 @@ set(CMAKE_CXX_EXTENSIONS OFF)
 set(CMAKE_POSITION_INDEPENDENT_CODE ON)
 
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  add_compile_options(-Wall -Wextra -Wpedantic)
+  add_compile_options(-Wall -Wextra -Wpedantic -Werror)
 endif()
 
 if(NOT CMAKE_BUILD_TYPE)
@@ -76,6 +76,11 @@ find_package(sensor_msgs REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(yaml-cpp REQUIRED)
 
+if(${image_transport_VERSION} VERSION_GREATER_EQUAL "6.4.0")
+  add_definitions(-DIMAGE_TRANSPORT_SUPPORTS_NODE_INTERFACES)
+  add_definitions(-DIMAGE_TRANSPORT_SUPPORTS_LIFECYCLE_NODE)
+endif()
+
 include_directories(SYSTEM
   ${SPINNAKER_INCLUDE_DIRS})
 
@@ -103,6 +108,7 @@ add_library(camera_driver SHARED
   src/image.cpp
   src/pixel_format.cpp
   src/genicam_utils.cpp
+  src/utils.cpp
 )
 
 target_link_libraries(camera_driver
@@ -112,6 +118,7 @@ target_link_libraries(camera_driver
   PRIVATE
   Spinnaker::Spinnaker)
 
+
 target_include_directories(camera_driver
   PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include>

spinnaker_camera_driver/doc/index.rst

@@ -144,6 +144,8 @@ spinview program.
 *In addition to the parameters defined in the .yaml
 files*, the driver has the following ROS parameters:
 
+-  ``auto_start``: automatically configure and activate the driver when starting
+   as a lifecycle node. Default: true.
 -  ``adjust_timestamp``: see `About time stamps`_ below for more documentation. Default: false.
 -  ``acquisition_timeout``: timeout for expecting frames (in seconds).
    If no frame is received for this time, the driver restarts. Default
@@ -161,6 +163,17 @@ files*, the driver has the following ROS parameters:
    while no ROS subscribers are present, but adds latency on
    subscription: after a subscription the first image will be published up to 1s later
    than without this option.
+-  ``diagnostic_incompletes_warn``: number of incomplete frames per diagnostic update period
+   before changing status to ``warning``.
+-  ``diagnostic_incompletes_error``: number of incomplete frames per diagnostic update period
+   before changing status to ``error``.
+-  ``diagnostic_drop_warn``: number of SDK-dropped frames per diagnostic update period
+   before changing status to ``warning``.
+-  ``diagnostic_drop_error``: number of SDK-dropped frames per diagnostic update period
+   before changing status to ``error``.
+-  ``diagnostic_min_freq``: minimum incoming message frequency before failing diagnostics.
+-  ``diagnostic_max_freq``: max incoming message frequency before failing diagnostics.
+-  ``diagnostic_window``: number of updates to maintain for diagnostic window.o
 -  ``dump_node_map``: set this to true to get a dump of the node map.
    Default: false.
 -  ``enable_external_control``: set this to true to enable external exposure control.
@@ -179,17 +192,6 @@ files*, the driver has the following ROS parameters:
 -  ``use_ieee_1588``: use PTP (IEEE 1588) to set the ``header.stamp`` time
    stamp instead of system time. Note that you will still need to enable
    IEEE 1588 at the camera level, and enable time stamp "chunks". Default: false.
--  ``diagnostic_incompletes_warn``: number of incomplete frames per diagnostic update period
-   before changing status to ``warning``.
--  ``diagnostic_incompletes_error``: number of incomplete frames per diagnostic update period
-   before changing status to ``error``.
--  ``diagnostic_drop_warn``: number of SDK-dropped frames per diagnostic update period
-   before changing status to ``warning``.
--  ``diagnostic_drop_error``: number of SDK-dropped frames per diagnostic update period
-   before changing status to ``error``.
--  ``diagnostic_min_freq``: minimum incoming message frequency before failing diagnostics.
--  ``diagnostic_max_freq``: max incoming message frequency before failing diagnostics.
--  ``diagnostic_window``: number of updates to maintain for diagnostic window.o
 
 The parameters listed above *cannot* be dynamically updated at runtime
 but require a driver restart to modify.

spinnaker_camera_driver/include/spinnaker_camera_driver/camera.hpp

@@ -31,6 +31,7 @@
 #include <sensor_msgs/msg/image.hpp>
 #include <spinnaker_camera_driver/diagnostic_levels.hpp>
 #include <spinnaker_camera_driver/image.hpp>
+#include <spinnaker_camera_driver/lifecycle_types.hpp>
 #include <spinnaker_camera_driver/spinnaker_wrapper.hpp>
 #include <spinnaker_camera_driver/synchronizer.hpp>
 #include <std_msgs/msg/float64.hpp>
@@ -46,13 +47,21 @@ class Camera
   using CompositeDiagnosticsTask = diagnostic_updater::CompositeDiagnosticTask;
   using FunctionDiagnosticsTask = diagnostic_updater::FunctionDiagnosticTask;
   using DiagnosticStatusWrapper = diagnostic_updater::DiagnosticStatusWrapper;
+  /*
+    The Constructor takes a long list of interfaces to support older distros like Humble
+   */
   explicit Camera(
-    rclcpp::Node * node, image_transport::ImageTransport * it, const std::string & prefix,
-    bool useStatus = true);
+    const std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface> &,
+    const std::shared_ptr<rclcpp::node_interfaces::NodeParametersInterface> &,
+    const std::shared_ptr<rclcpp::node_interfaces::NodeLoggingInterface> &,
+    const std::shared_ptr<rclcpp::node_interfaces::NodeTimersInterface> &,
+    const std::shared_ptr<rclcpp::node_interfaces::NodeClockInterface> &,
+    const std::shared_ptr<rclcpp::node_interfaces::NodeTopicsInterface> &,
+    const std::shared_ptr<rclcpp::node_interfaces::NodeServicesInterface> &,
+    image_transport::ImageTransport * it, camera_info_manager::CameraInfoManager * im,
+    const std::string & logName, const std::string & prefix, bool useStatus = true);
   ~Camera();
 
-  bool start();
-  bool stop();
   void setSynchronizer(const std::shared_ptr<Synchronizer> & s) { synchronizer_ = s; }
   void setExposureController(const std::shared_ptr<ExposureController> & e)
   {
@@ -61,6 +70,11 @@ class Camera
   const std::string & getName() const { return (name_); }
   const std::string & getPrefix() const { return (prefix_); }
 
+  bool configure();    // corresponds to lifecycle configure
+  bool activate();     // corresponds to lifecycle activate
+  bool deactivate();   // corresponds to lifecycle deactivate
+  bool deconfigure();  // corresponds to lifecycle cleanup
+
 private:
   struct NodeInfo
   {
@@ -73,8 +87,8 @@ class Camera
   void processImage(const ImageConstPtr & image);
   void readParameters();
   void printCameraInfo();
-  void startCamera();
-  bool stopCamera();
+  bool startStreaming();
+  bool stopStreaming();
   void createCameraParameters();
   void setParameter(const NodeInfo & ni, const rclcpp::Parameter & p);
   bool setEnum(const std::string & nodeName, const std::string & v = "");
@@ -85,6 +99,14 @@ class Camera
   bool readParameterDefinitionFile();
   void startDiagnostics();
   void stopDiagnostics();
+  void makePublishers();
+  void destroyPublishers();
+  void makeSubscribers();
+  void destroySubscribers();
+  void startTimers();
+  void stopTimers();
+  void openDevice();
+  void closeDevice();
 
   rclcpp::Time getAdjustedTimeStamp(uint64_t t, int64_t sensorTime);
 
@@ -93,37 +115,41 @@ class Camera
   rcl_interfaces::msg::SetParametersResult parameterChanged(
     const std::vector<rclcpp::Parameter> & params);
   void controlCallback(const flir_camera_msgs::msg::CameraControl::UniquePtr msg);
-  void printStatus();
+  void updateStatus();
   void checkSubscriptions();
   void doPublish(const ImageConstPtr & im);
-  rclcpp::Logger get_logger()
-  {
-    return rclcpp::get_logger(
-      !name_.empty() ? name_ : (serial_.empty() ? std::string("camera") : serial_));
-  }
+  rclcpp::Logger get_logger() { return (rclcpp::get_logger(logName_)); }
 
   template <class T>
   T safe_declare(const std::string & name, const T & def)
   {
     try {
-      return (node_->declare_parameter<T>(name, def));
+      return (
+        node_parameters_interface_->declare_parameter(name, rclcpp::ParameterValue(def)).get<T>());
     } catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException & e) {
-      T value(def);
-      (void)node_->get_parameter_or<T>(name, value, def);
-      return (value);
+      const rclcpp::Parameter p = node_parameters_interface_->get_parameter(name);
+      return (p.get_parameter_value().get<T>());
     }
   }
+  template <class T>
+  T safePrefixedDeclare(const std::string & name, const T & def)
+  {
+    return (safe_declare<T>(prefix_ + name, def));
+  }
+
+  auto clock() { return node_clock_interface_->get_clock(); }
+  auto name() { return node_base_interface_->get_name(); }
 
   void safe_declare(
     const std::string & name, const rclcpp::ParameterValue & pv,
     const rcl_interfaces::msg::ParameterDescriptor & desc)
   {
     try {
-      node_->declare_parameter(name, pv, desc, false);
+      node_parameters_interface_->declare_parameter(name, pv, desc, false);
     } catch (rclcpp::exceptions::InvalidParameterTypeException & e) {
       RCLCPP_WARN_STREAM(
         get_logger(), "overwriting bad param with default: " + std::string(e.what()));
-      node_->declare_parameter(name, pv, desc, true);
+      node_parameters_interface_->declare_parameter(name, pv, desc, true);
     } catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException & e) {
       // do nothing
     }
@@ -133,15 +159,24 @@ class Camera
     const DiagnosticLevels<int> & levels);
   void incompleteDiagnostics(DiagnosticStatusWrapper & status);
   void droppedDiagnostics(DiagnosticStatusWrapper & status);
+  void acquisitionDiagnostics(DiagnosticStatusWrapper & status);
 
   // ----- variables --
   std::string prefix_;
   std::string topicPrefix_;
-  rclcpp::Node * node_;
-  image_transport::ImageTransport * imageTransport_;
+  std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface> node_base_interface_;
+  std::shared_ptr<rclcpp::node_interfaces::NodeParametersInterface> node_parameters_interface_;
+  std::shared_ptr<rclcpp::node_interfaces::NodeLoggingInterface> node_logging_interface_;
+  std::shared_ptr<rclcpp::node_interfaces::NodeTimersInterface> node_timers_interface_;
+  std::shared_ptr<rclcpp::node_interfaces::NodeClockInterface> node_clock_interface_;
+  std::shared_ptr<rclcpp::node_interfaces::NodeTopicsInterface> node_topics_interface_;
+  std::shared_ptr<rclcpp::node_interfaces::NodeServicesInterface> node_services_interface_;
+  image_transport::ImageTransport * imageTransport_{nullptr};
+  camera_info_manager::CameraInfoManager * infoManager_{nullptr};
   image_transport::CameraPublisher pub_;
   rclcpp::Publisher<flir_camera_msgs::msg::ImageMetaData>::SharedPtr metaPub_;
   std::string serial_;
+  std::string logName_;
   std::string name_;
   std::string cameraInfoURL_;
   std::string frameId_;
@@ -158,22 +193,21 @@ class Camera
   bool useIEEE1588_{false};
   double minIEEE1588Offset_{0};
   double maxIEEE1588Offset_{0.1};
-  double ptpOffset_{0};                 // in seconds
-  bool connectWhileSubscribed_{false};  // if true, connects to SDK when subscription happens
+  double ptpOffset_{0};                    // in seconds
+  bool streamOnlyWhileSubscribed_{false};  // if true, streams from camera only while subscribed
   bool enableExternalControl_{false};
   uint32_t currentExposureTime_{0};
   double averageTimeDifference_{std::numeric_limits<double>::quiet_NaN()};
   int64_t baseTimeOffset_{0};
   float currentGain_{std::numeric_limits<float>::lowest()};
   std::shared_ptr<spinnaker_camera_driver::SpinnakerWrapper> wrapper_;
-  std::shared_ptr<camera_info_manager::CameraInfoManager> infoManager_;
   sensor_msgs::msg::Image imageMsg_;
   sensor_msgs::msg::CameraInfo cameraInfoMsg_;
   flir_camera_msgs::msg::ImageMetaData metaMsg_;
   rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr callbackHandle_;  // keep alive callbacks
   rclcpp::TimerBase::SharedPtr statusTimer_;
   rclcpp::TimerBase::SharedPtr checkSubscriptionsTimer_;
-  bool cameraRunning_{false};
+  bool cameraStreaming_{false};
   std::mutex mutex_;
   std::condition_variable cv_;
   std::deque<ImageConstPtr> bufferQueue_;
@@ -191,6 +225,7 @@ class Camera
   std::shared_ptr<Synchronizer> synchronizer_;
   std::shared_ptr<ExposureController> exposureController_;
   bool firstSynchronizedFrame_{true};
+  bool runStatusTimer_{false};
   // --------- related to diagnostics
   std::shared_ptr<diagnostic_updater::Updater> updater_;
   std::shared_ptr<diagnostic_updater::TopicDiagnostic> topicDiagnostic_;
@@ -205,6 +240,9 @@ class Camera
   DiagnosticLevels<int> dropLevels_;
   uint64_t lastFrameId_{0};
   uint64_t numDrops_{0};
+  FunctionDiagnosticsTask acquisitionErrorTask_;
+  uint64_t acquisitionTimeouts_{0};
+  bool acquisitionError_{false};
 };
 }  // namespace spinnaker_camera_driver
 #endif  // SPINNAKER_CAMERA_DRIVER__CAMERA_HPP_

spinnaker_camera_driver/include/spinnaker_camera_driver/camera_driver.hpp

@@ -16,22 +16,69 @@
 #ifndef SPINNAKER_CAMERA_DRIVER__CAMERA_DRIVER_HPP_
 #define SPINNAKER_CAMERA_DRIVER__CAMERA_DRIVER_HPP_
 
+#include <functional>
 #include <image_transport/image_transport.hpp>
 #include <memory>
 #include <rclcpp/rclcpp.hpp>
 #include <spinnaker_camera_driver/camera.hpp>
+#include <spinnaker_camera_driver/lifecycle_types.hpp>
 
 namespace spinnaker_camera_driver
 {
-class CameraDriver : public rclcpp::Node
+class CameraDriver : public NodeType
 {
 public:
-  explicit CameraDriver(const rclcpp::NodeOptions & options);
-  ~CameraDriver();
+  using CameraInfoManager = camera_info_manager::CameraInfoManager;
+  using ImageTransport = image_transport::ImageTransport;
+  explicit CameraDriver(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
+  ~CameraDriver() override;
+#ifdef IMAGE_TRANSPORT_SUPPORTS_LIFECYCLE_NODE
+protected:
+  void preShutdown();
+  CbReturn on_configure(const LCState & state) override;
+  CbReturn on_activate(const LCState & state) override;
+  CbReturn on_deactivate(const LCState & state) override;
+  CbReturn on_cleanup(const LCState & state) override;
+  CbReturn on_shutdown(const LCState & state) override;
+  CbReturn on_error(const LCState & state) override;
+  // helper methods
+  using CameraFnPtr = bool (Camera::*)();
+  template <typename F>
+  CbReturn changeState(F && f, const CameraFnPtr & cameraFn, const std::string & trans)
+  {
+    RCLCPP_INFO_STREAM(get_logger(), "requested transition: " << trans);
+    const auto ret = f(*this);
+    if (ret != CbReturn::SUCCESS) {
+      return (ret);
+    }
+    if (!camera_) {
+      return (CbReturn::FAILURE);
+    }
+    const bool v = (camera_.get()->*cameraFn)();
+    return (v ? CbReturn::SUCCESS : CbReturn::FAILURE);
+  }
+#endif
 
 private:
-  std::shared_ptr<image_transport::ImageTransport> imageTransport_;
+  template <class T>
+  T safeDeclare(const std::string & name, const T & def)
+  {
+    try {
+      return (get_node_parameters_interface()
+                ->declare_parameter(name, rclcpp::ParameterValue(def))
+                .get<T>());
+    } catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException & e) {
+      const rclcpp::Parameter p = get_node_parameters_interface()->get_parameter(name);
+      return (p.get_parameter_value().get<T>());
+    }
+  }
+
+  void shutdown();
+  // --------------- variables
+  std::shared_ptr<ImageTransport> imageTransport_;
+  std::shared_ptr<CameraInfoManager> infoManager_;
   std::shared_ptr<Camera> camera_;
+  rclcpp::TimerBase::SharedPtr timer_;
 };
 }  // namespace spinnaker_camera_driver
 #endif  // SPINNAKER_CAMERA_DRIVER__CAMERA_DRIVER_HPP_

spinnaker_camera_driver/include/spinnaker_camera_driver/lifecycle_types.hpp

@@ -0,0 +1,32 @@
+// -*-c++-*--------------------------------------------------------------------
+// Copyright 2025 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__LIFECYCLE_TYPES_HPP_
+#define SPINNAKER_CAMERA_DRIVER__LIFECYCLE_TYPES_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+
+#ifdef IMAGE_TRANSPORT_SUPPORTS_LIFECYCLE_NODE
+#include <rclcpp_lifecycle/lifecycle_node.hpp>
+#include <rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp>
+using NodeType = rclcpp_lifecycle::LifecycleNode;
+using LCState = rclcpp_lifecycle::State;
+using CbReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
+
+#else
+using NodeType = rclcpp::Node;
+#endif
+
+#endif  // SPINNAKER_CAMERA_DRIVER__LIFECYCLE_TYPES_HPP_