better warnings on incomplete images

ros-drivers · Jul 21, 2024 · e561501 · e561501
1 parent 63df12e
commit e561501
Show file tree

Hide file tree

Showing 6 changed files with 36 additions and 14 deletions.
diff --git a/spinnaker_camera_driver/include/spinnaker_camera_driver/logging.hpp b/spinnaker_camera_driver/include/spinnaker_camera_driver/logging.hpp
@@ -39,6 +39,10 @@
   {                                                \
     RCLCPP_WARN_STREAM(get_logger(), __VA_ARGS__); \
   }
+#define LOG_WARN_FMT(...)                   \
+  {                                         \
+    RCLCPP_WARN(get_logger(), __VA_ARGS__); \
+  }
 #define LOG_ERROR(...)                              \
   {                                                 \
     RCLCPP_ERROR_STREAM(get_logger(), __VA_ARGS__); \

diff --git a/spinnaker_camera_driver/include/spinnaker_camera_driver/spinnaker_wrapper.hpp b/spinnaker_camera_driver/include/spinnaker_camera_driver/spinnaker_wrapper.hpp
@@ -54,6 +54,7 @@ class SpinnakerWrapper
 
   std::string getPixelFormat() const;
   double getReceiveFrameRate() const;
+  double getIncompleteRate();
   std::string getNodeMapAsString();
   std::string setEnum(const std::string & nodeName, const std::string & val, std::string * retVal);
   std::string setDouble(const std::string & nodeName, double val, double * retVal);

diff --git a/spinnaker_camera_driver/src/camera.cpp b/spinnaker_camera_driver/src/camera.cpp
@@ -165,11 +165,18 @@ void Camera::printStatus()
                          : 0;
     const rclcpp::Time t = node_->now();
     const rclcpp::Duration dt = t - lastStatusTime_;
-    double dtns = std::max(dt.nanoseconds(), (int64_t)1);
-    double outRate = publishedCount_ * 1e9 / dtns;
-    LOG_INFO_FMT(
-      "rate [Hz] in %6.2f out %6.2f drop %3.0f%%", wrapper_->getReceiveFrameRate(), outRate,
-      dropRate * 100);
+    const double dtns = std::max(dt.nanoseconds(), (int64_t)1);
+    const double outRate = publishedCount_ * 1e9 / dtns;
+    const double incompleteRate = wrapper_->getIncompleteRate();
+    if (incompleteRate != 0) {
+      LOG_WARN_FMT(
+        "rate [Hz] in %6.2f out %6.2f drop %3.0f%% INCOMPLETE %3.0f%%",
+        wrapper_->getReceiveFrameRate(), outRate, dropRate * 100, incompleteRate * 100);
+    } else {
+      LOG_INFO_FMT(
+        "rate [Hz] in %6.2f out %6.2f drop %3.0f%%", wrapper_->getReceiveFrameRate(), outRate,
+        dropRate * 100);
+    }
     lastStatusTime_ = t;
     droppedCount_ = 0;
     publishedCount_ = 0;

diff --git a/spinnaker_camera_driver/src/spinnaker_wrapper.cpp b/spinnaker_camera_driver/src/spinnaker_wrapper.cpp
@@ -52,6 +52,7 @@ double SpinnakerWrapper::getReceiveFrameRate() const
 {
   return (wrapperImpl_->getReceiveFrameRate());
 }
+double SpinnakerWrapper::getIncompleteRate() { return (wrapperImpl_->getIncompleteRate()); }
 
 std::string SpinnakerWrapper::getNodeMapAsString() { return (wrapperImpl_->getNodeMapAsString()); }
 

diff --git a/spinnaker_camera_driver/src/spinnaker_wrapper_impl.cpp b/spinnaker_camera_driver/src/spinnaker_wrapper_impl.cpp
@@ -151,9 +151,9 @@ void SpinnakerWrapperImpl::refreshCameraList()
         }  // end for camList
       } else {
         LOG_ERROR("Unknown Interface (Display name not readable)");
-      }  // end if-else ptrInterfaceDisplayName
-    }    // end if ptrInterfaceType
-  }      // end for interfaceList
+      }
+    }
+  }
 
   interfaceList.Clear();
 #endif
@@ -316,6 +316,15 @@ double SpinnakerWrapperImpl::getReceiveFrameRate() const
   return (avgTimeInterval_ > 0 ? (1.0 / avgTimeInterval_) : 0);
 }
 
+double SpinnakerWrapperImpl::getIncompleteRate()
+{
+  const double r =
+    numImagesTotal_ == 0 ? 0 : (numIncompleteImagesTotal_ / static_cast<double>(numImagesTotal_));
+  numImagesTotal_ = 0;
+  numIncompleteImagesTotal_ = 0;
+  return (r);
+}
+
 static int int_ceil(size_t x, int y)
 {
   // compute the integer ceil(x / y)
@@ -358,14 +367,10 @@ void SpinnakerWrapperImpl::OnImageEvent(Spinnaker::ImagePtr imgPtr)
     std::unique_lock<std::mutex> lock(mutex_);
     lastTime_ = t;
   }
-
+  numImagesTotal_++;
   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
+    numIncompleteImagesTotal_++;
   } else {
     float expTime = 0;
     float gain = 0;

diff --git a/spinnaker_camera_driver/src/spinnaker_wrapper_impl.hpp b/spinnaker_camera_driver/src/spinnaker_wrapper_impl.hpp
@@ -52,6 +52,8 @@ class SpinnakerWrapperImpl : public Spinnaker::ImageEventHandler
   bool stopCamera();
 
   double getReceiveFrameRate() const;
+  double getIncompleteRate();
+
   std::string getNodeMapAsString();
   // methods for setting camera params
   std::string setEnum(const std::string & nodeName, const std::string & val, std::string * retVal);
@@ -88,6 +90,8 @@ class SpinnakerWrapperImpl : public Spinnaker::ImageEventHandler
   std::mutex mutex_;
   uint64_t acquisitionTimeout_{10000000000ULL};
   size_t numIncompleteImages_{0};
+  size_t numImagesTotal_{0};
+  size_t numIncompleteImagesTotal_{0};
 };
 }  // namespace spinnaker_camera_driver
-Original file line number
+Diff line change
@@ Expand Up / @@ -52,6 +52,7 @@ double SpinnakerWrapper::getReceiveFrameRate() const @@
     {
       return (wrapperImpl_->getReceiveFrameRate());
     }
+    double SpinnakerWrapper::getIncompleteRate() { return (wrapperImpl_->getIncompleteRate()); }
     std::string SpinnakerWrapper::getNodeMapAsString() { return (wrapperImpl_->getNodeMapAsString()); }
@@ Expand Down @@