track incomplete frames

spinnaker_camera_driver/include/spinnaker_camera_driver/image.hpp

@@ -27,7 +27,8 @@ class Image
   Image(
     uint64_t t, int16_t brightness, uint32_t et, uint32_t maxEt, float gain, int64_t imgT,
     size_t imageSize, int status, const void * data, size_t w, size_t h, size_t stride,
-    size_t bitsPerPixel, size_t numChan, uint64_t frameId, pixel_format::PixelFormat pixFmt);
+    size_t bitsPerPixel, size_t numChan, uint64_t frameId, pixel_format::PixelFormat pixFmt,
+    size_t numIncomplete);
 
   // ----- variables --
   uint64_t time_;
@@ -46,6 +47,7 @@ class Image
   size_t numChan_;
   uint64_t frameId_;
   pixel_format::PixelFormat pixelFormat_;
+  size_t numIncomplete_;
 
 private:
 };

spinnaker_camera_driver/include/spinnaker_camera_driver/synchronizer.hpp

@@ -26,7 +26,7 @@ class Synchronizer
   Synchronizer() = default;
   virtual ~Synchronizer() {}
   virtual bool getTimeStamp(
-    uint64_t hostTime, uint64_t imageTime, uint64_t frameId, uint64_t * ft) = 0;
+    uint64_t hostTime, uint64_t imageTime, uint64_t frameId, size_t numIncompl, uint64_t * ft) = 0;
 };
 }  // namespace spinnaker_camera_driver
 #endif  // SPINNAKER_CAMERA_DRIVER__SYNCHRONIZER_HPP_

spinnaker_camera_driver/src/camera.cpp

@@ -557,7 +557,8 @@ void Camera::doPublish(const ImageConstPtr & im)
   rclcpp::Time t;
   if (synchronizer_) {
     uint64_t t_64;
-    bool haveTime = synchronizer_->getTimeStamp(im->time_, im->imageTime_, im->frameId_, &t_64);
+    bool haveTime = synchronizer_->getTimeStamp(
+      im->time_, im->imageTime_, im->frameId_, im->numIncomplete_, &t_64);
     t = rclcpp::Time(t_64, RCL_SYSTEM_TIME);
     if (!haveTime) {
       if (firstSynchronizedFrame_) {

spinnaker_camera_driver/src/image.cpp

@@ -19,7 +19,8 @@ namespace spinnaker_camera_driver
 Image::Image(
   uint64_t t, int16_t brightness, uint32_t et, uint32_t maxEt, float gain, int64_t imgT,
   size_t imageSize, int status, const void * data, size_t w, size_t h, size_t stride,
-  size_t bitsPerPixel, size_t numChan, uint64_t frameId, pixel_format::PixelFormat pixFmt)
+  size_t bitsPerPixel, size_t numChan, uint64_t frameId, pixel_format::PixelFormat pixFmt,
+  size_t ninc)
 : time_(t),
   brightness_(brightness),
   exposureTime_(et),
@@ -35,7 +36,8 @@ Image::Image(
   bitsPerPixel_(bitsPerPixel),
   numChan_(numChan),
   frameId_(frameId),
-  pixelFormat_(pixFmt)
+  pixelFormat_(pixFmt),
+  numIncomplete_(ninc)
 {
 }
 }  // namespace spinnaker_camera_driver

spinnaker_camera_driver/src/spinnaker_wrapper_impl.cpp

@@ -294,9 +294,12 @@ void SpinnakerWrapperImpl::OnImageEvent(Spinnaker::ImagePtr imgPtr)
   }
 
   if (imgPtr->IsIncomplete()) {
+    numIncompleteImages_++;
+#if 0
     // Retrieve and print the image status description
     std::cout << "Image incomplete: "
               << Spinnaker::Image::GetImageStatusDescription(imgPtr->GetImageStatus()) << std::endl;
+#endif
   } else {
     float expTime = 0;
     float gain = 0;
@@ -340,7 +343,8 @@ void SpinnakerWrapperImpl::OnImageEvent(Spinnaker::ImagePtr imgPtr)
       t, brightness, expTime, maxExpTime, gain, stamp, imgPtr->GetImageSize(),
       imgPtr->GetImageStatus(), imgPtr->GetData(), imgPtr->GetWidth(), imgPtr->GetHeight(),
       imgPtr->GetStride(), imgPtr->GetBitsPerPixel(), imgPtr->GetNumChannels(),
-      imgPtr->GetFrameID(), pixelFormat_));
+      imgPtr->GetFrameID(), pixelFormat_, numIncompleteImages_));
+    numIncompleteImages_ = 0;
     callback_(img);
   }
 }

spinnaker_camera_driver/src/spinnaker_wrapper_impl.hpp

@@ -83,6 +83,7 @@ class SpinnakerWrapperImpl : public Spinnaker::ImageEventHandler
   std::shared_ptr<std::thread> thread_;
   std::mutex mutex_;
   uint64_t acquisitionTimeout_{10000000000ULL};
+  size_t numIncompleteImages_{0};
 };
 }  // namespace spinnaker_camera_driver
 

spinnaker_synchronized_camera_driver/include/spinnaker_synchronized_camera_driver/time_keeper.hpp

@@ -33,7 +33,7 @@ class TimeKeeper : public spinnaker_camera_driver::Synchronizer
   const std::string & getName() const { return (name_); }
 
   bool getTimeStamp(
-    uint64_t hostTime, uint64_t imageTime, uint64_t frameId, uint64_t * ft) override;
+    uint64_t hostTime, uint64_t imageTime, uint64_t frameId, size_t ninc, uint64_t * ft) override;
 
   double getOffsetAverage() const
   {
@@ -46,6 +46,7 @@ class TimeKeeper : public spinnaker_camera_driver::Synchronizer
     return (numOffset_ > 1 ? S_ / static_cast<double>(numOffset_ - 1) : 0);
   }
   int64_t getNumFramesDropped() const { return (numFramesDropped_); }
+  size_t getNumFramesIncomplete() const { return (numFramesIncomplete_); }
   void clearStatistics();
 
 private:
@@ -55,6 +56,7 @@ class TimeKeeper : public spinnaker_camera_driver::Synchronizer
   uint64_t lastFrameId_{0};
   uint64_t lastHostTime_{0};
   int64_t numFramesDropped_{0};
+  size_t numFramesIncomplete_{0};
   size_t numOffset_{0};
   double offsetSum_{0};
   double S_{0};

spinnaker_synchronized_camera_driver/src/synchronized_camera_driver.cpp

@@ -58,14 +58,15 @@ void SynchronizedCameraDriver::printStatus()
 
   struct TKInfo
   {
-    explicit TKInfo(const std::string & n, double off, double jit, int64_t d)
-    : name(n), offset(off), jitter(jit), dropped(d)
+    explicit TKInfo(const std::string & n, double off, double jit, int64_t d, size_t i)
+    : name(n), offset(off), jitter(jit), dropped(d), incomplete(i)
     {
     }
     std::string name;
     double offset;
     double jitter;
     int64_t dropped;
+    size_t incomplete;
   };
   std::vector<TKInfo> tki;
   double dt = 0;
@@ -75,15 +76,16 @@ void SynchronizedCameraDriver::printStatus()
     for (auto & tk : timeKeepers_) {
       tki.push_back(TKInfo(
         tk->getName(), tk->getOffsetAverage() / dt, std::sqrt(tk->getOffsetVariance()) / dt,
-        tk->getNumFramesDropped()));
+        tk->getNumFramesDropped(), tk->getNumFramesIncomplete()));
       tk->clearStatistics();
     }
   }
   LOG_INFO_FMT("------ frequency: %10.3f Hz", 1.0 / dt);
-  LOG_INFO_FMT("%-10s %5s %10s %10s", "camera", "drop", "offset", "jitter");
+  LOG_INFO_FMT("%-8s %4s %4s %9s %9s", "camera", "drop", "icmp", "offset", "jitter");
   for (auto & tk : tki) {
     LOG_INFO_FMT(
-      "%-10s %5ld %9.2f%% %9.2f%%", tk.name.c_str(), tk.dropped, tk.offset * 100, tk.jitter * 100);
+      "%-8s %4ld %4zu %8.2f%% %8.2f%%", tk.name.c_str(), tk.dropped, tk.incomplete, tk.offset * 100,
+      tk.jitter * 100);
   }
 }
 

spinnaker_synchronized_camera_driver/src/time_keeper.cpp

@@ -22,7 +22,8 @@ static rclcpp::Logger get_logger() { return (rclcpp::get_logger("cam_sync")); }
 namespace spinnaker_synchronized_camera_driver
 {
 
-bool TimeKeeper::getTimeStamp(uint64_t hostTime, uint64_t, uint64_t frameId, uint64_t * frameTime)
+bool TimeKeeper::getTimeStamp(
+  uint64_t hostTime, uint64_t, uint64_t frameId, size_t ninc, uint64_t * frameTime)
 {
   if (lastHostTime_ == 0) {
     lastFrameId_ = frameId;
@@ -35,6 +36,7 @@ bool TimeKeeper::getTimeStamp(uint64_t hostTime, uint64_t, uint64_t frameId, uin
   lastHostTime_ = hostTime;
 
   numFramesDropped_ += std::max<int64_t>(0, gap - 1);
+  numFramesIncomplete_ += ninc;
   if (gap > 0 && gap < 4) {  // ignore all but none, 1 or 2 frames dropped
     if (gap != 1) {
       LOG_WARN(name_ << " dropped " << gap - 1 << " frame(s)");
@@ -72,6 +74,7 @@ void TimeKeeper::clearStatistics()
   offsetSum_ = 0.0;
   numOffset_ = 0;
   numFramesDropped_ = 0;
+  numFramesIncomplete_ = 0;
   S_ = 0;
   M_ = 0;
 }