implemented support for diagnostics

spinnaker_camera_driver/CMakeLists.txt

@@ -67,6 +67,7 @@ find_package(SPINNAKER REQUIRED)
 find_package(ament_cmake REQUIRED)
 find_package(ament_cmake_ros REQUIRED)
 find_package(camera_info_manager REQUIRED)
+find_package(diagnostic_updater REQUIRED)
 find_package(flir_camera_msgs REQUIRED)
 find_package(image_transport REQUIRED)
 find_package(rclcpp REQUIRED)
@@ -82,6 +83,7 @@ include_directories(SYSTEM
 
 set(ROS_DEPENDENCIES
   camera_info_manager::camera_info_manager
+  diagnostic_updater::diagnostic_updater
   ${flir_camera_msgs_TARGETS}
   image_transport::image_transport
   rclcpp::rclcpp

spinnaker_camera_driver/cmake/spinnaker_camera_driverConfig.cmake.in

@@ -1,12 +1,13 @@
 include(CMakeFindDependencyMacro)
 
 # find_dependency(SPINNAKER)
-find_dependency(rclcpp)
-find_dependency(rclcpp_components)
 find_dependency(camera_info_manager)
-find_dependency(image_transport)
+find_dependency(diagnostic_updater)
 find_dependency(flir_camera_msgs)
+find_dependency(image_transport)
 find_dependency(sensor_msgs)
+find_dependency(rclcpp)
+find_dependency(rclcpp_components)
 find_dependency(std_msgs)
 
 # Add the targets file

spinnaker_camera_driver/doc/index.rst

@@ -179,6 +179,17 @@ 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.
@@ -203,9 +214,8 @@ Using multiple cameras at the same time
 
 The FLIR Spinnaker does not support two programs accessing the Spinnaker SDK at the same time,
 even if two different cameras are accessed. Strange things happen, in particular with USB3 cameras.
-You can however run multiple cameras in the same
-address space using ROS2's Composable Node concept, see the example
-launch file ``multiple_cameras.launch.py``.
+You can however run multiple cameras in the same program (address space) using ROS2's Composable Node
+concept, see the example launch file ``multiple_cameras.launch.py``.
 
 If you are using hardware synchronized cameras please use the ``spinnaker_synchronized_camera_driver``,
 which will associate the images triggered by the same sync pulse with each other
@@ -296,6 +306,8 @@ Note that this will *not* work if you ping the camera: you will find
 a MTU size of 1500, even if both camera and host NIC have correct MTU setting.
 Beware that some network cards have `bugs that mess up the MTU when
 running ptp4l <https://www.reddit.com/r/networking/comments/1ebvj5y/linux_ptp_and_jumbo_frames_for_gige_vision/>`__.
+At least for the Intel igb cards, installing the 5.19.12 drivers `from here <https://github.com/intel/ethernet-linux-igb/>`__
+fixes these issues.
 
 For more tips on GigE setup look at FLIR’s support pages
 `here <https://www.flir.com/support-center/iis/machine-vision/knowledge-base/lost-ethernet-data-packets-on-linux-systems/>`__
@@ -315,14 +327,14 @@ Only FLIR GigE cameras are PTP capable, but not all of them. For example the Bla
 Each camera has a built-in PTP hardware clock (PHC). To use it, you need to:
 
 - configure the camera to use PTP by enabling it either in SpinView or via the ROS parameter `gev_ieee_1588`` (set to true/false)
-- run the camera in ``Auto`` or ``SlaveOnly`` mode by setting the corresponding parameter in SpinView or using the ROS parameter ``gev_ieee_1588_mode``.
+- run the camera in ``Auto`` or ``SlaveOnly`` mode by setting the corresponding parameter in SpinView or using the ROS parameter ``gev_ieee_1588_mode``. Running in ``Auto`` rarely makes sense though because one of the cameras may become a master clock without really having an absolute time reference (like ntp) to the ROS host clock.
 - enable time stamps being sent via chunks by setting ``chunk_mode_active``
   to ``True``, ``chunk_selector_timestamp`` to ``Timestamp`` followed by ``chunk_enable_timestamp`` to True.
 - tell the driver to actually use the time stamps instead of the system clock by setting ``use_ieee_1588`` to True. If this flag is set, the driver will populate the ROS image message header stamp with the PTP time stamp.
 
-The launch file section for the Blackfly S has some commented out example settings.
+For an example launch file, see ``multiple_cameras_ptp.launch.py``.
 
-When the camera boots up it will start its PHC time at zero rather than at the correct time (TAI). The driver log will have warning messages and indicate a huge offset.
+When the camera boots up it will start its PHC time at zero rather than at the correct time. The driver log will have warning messages and indicate a huge offset.
 
 ::
 

spinnaker_camera_driver/include/spinnaker_camera_driver/camera.hpp

@@ -18,6 +18,8 @@
 
 #include <camera_info_manager/camera_info_manager.hpp>
 #include <deque>
+#include <diagnostic_updater/diagnostic_updater.hpp>
+#include <diagnostic_updater/publisher.hpp>
 #include <flir_camera_msgs/msg/camera_control.hpp>
 #include <flir_camera_msgs/msg/image_meta_data.hpp>
 #include <image_transport/image_transport.hpp>
@@ -27,6 +29,7 @@
 #include <rclcpp/rclcpp.hpp>
 #include <sensor_msgs/msg/camera_info.hpp>
 #include <sensor_msgs/msg/image.hpp>
+#include <spinnaker_camera_driver/diagnostic_levels.hpp>
 #include <spinnaker_camera_driver/image.hpp>
 #include <spinnaker_camera_driver/spinnaker_wrapper.hpp>
 #include <spinnaker_camera_driver/synchronizer.hpp>
@@ -40,6 +43,9 @@ class Camera
 {
 public:
   using ImageConstPtr = spinnaker_camera_driver::ImageConstPtr;
+  using CompositeDiagnosticsTask = diagnostic_updater::CompositeDiagnosticTask;
+  using FunctionDiagnosticsTask = diagnostic_updater::FunctionDiagnosticTask;
+  using DiagnosticStatusWrapper = diagnostic_updater::DiagnosticStatusWrapper;
   explicit Camera(
     rclcpp::Node * node, image_transport::ImageTransport * it, const std::string & prefix,
     bool useStatus = true);
@@ -77,6 +83,8 @@ class Camera
   bool setBool(const std::string & nodeName, bool v);
   bool execute(const std::string & nodeName);
   bool readParameterDefinitionFile();
+  void startDiagnostics();
+  void stopDiagnostics();
 
   rclcpp::Time getAdjustedTimeStamp(uint64_t t, int64_t sensorTime);
 
@@ -120,6 +128,11 @@ class Camera
       // do nothing
     }
   }
+  void updateDiagnosticsStatus(
+    DiagnosticStatusWrapper & status, const std::string & label, const int val,
+    const DiagnosticLevels<int> & levels);
+  void incompleteDiagnostics(DiagnosticStatusWrapper & status);
+  void droppedDiagnostics(DiagnosticStatusWrapper & status);
 
   // ----- variables --
   std::string prefix_;
@@ -178,6 +191,20 @@ class Camera
   std::shared_ptr<Synchronizer> synchronizer_;
   std::shared_ptr<ExposureController> exposureController_;
   bool firstSynchronizedFrame_{true};
+  // --------- related to diagnostics
+  std::shared_ptr<diagnostic_updater::Updater> updater_;
+  std::shared_ptr<diagnostic_updater::TopicDiagnostic> topicDiagnostic_;
+  std::shared_ptr<diagnostic_updater::FrequencyStatus> imageArrivalDiagnostic_;
+  double maxFreqDiag_{0};
+  double minFreqDiag_{0};
+  CompositeDiagnosticsTask imageTask_;
+  FunctionDiagnosticsTask incompleteFrameTask_;
+  uint64_t numIncompletes_{0};
+  DiagnosticLevels<int> incompleteLevels_;
+  FunctionDiagnosticsTask dropFrameTask_;
+  DiagnosticLevels<int> dropLevels_;
+  uint64_t lastFrameId_{0};
+  uint64_t numDrops_{0};
 };
 }  // namespace spinnaker_camera_driver
 #endif  // SPINNAKER_CAMERA_DRIVER__CAMERA_HPP_

spinnaker_camera_driver/include/spinnaker_camera_driver/diagnostic_levels.hpp

@@ -0,0 +1,30 @@
+// -*-c++-*--------------------------------------------------------------------
+// Copyright 2023 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__DIAGNOSTIC_LEVELS_HPP_
+#define SPINNAKER_CAMERA_DRIVER__DIAGNOSTIC_LEVELS_HPP_
+
+namespace spinnaker_camera_driver
+{
+template <class T>
+struct DiagnosticLevels
+{
+public:
+  explicit DiagnosticLevels(T w = 0, T e = 0) : warning(w), error(e) {}
+  T warning{0};
+  T error{0};
+};
+}  // namespace spinnaker_camera_driver
+#endif  // SPINNAKER_CAMERA_DRIVER__DIAGNOSTIC_LEVELS_HPP_

spinnaker_camera_driver/launch/driver_node.launch.py

@@ -67,6 +67,9 @@
         'chunk_enable_gain': True,
         'chunk_selector_timestamp': 'Timestamp',
         'chunk_enable_timestamp': True,
+        'diagnostic_period': 1.0,
+        'diagnostic_min_freq': 39.0,
+        'diagnostic_max_freq': 41.0
     },
     'blackfly': {
         'debug': False,