diff --git a/spinnaker_camera_driver/include/spinnaker_camera_driver/image.hpp b/spinnaker_camera_driver/include/spinnaker_camera_driver/image.hpp index 21e5fc45..ad24e8ed 100644 --- a/spinnaker_camera_driver/include/spinnaker_camera_driver/image.hpp +++ b/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: }; diff --git a/spinnaker_camera_driver/include/spinnaker_camera_driver/synchronizer.hpp b/spinnaker_camera_driver/include/spinnaker_camera_driver/synchronizer.hpp index e05a6640..58ad2ced 100644 --- a/spinnaker_camera_driver/include/spinnaker_camera_driver/synchronizer.hpp +++ b/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_ diff --git a/spinnaker_camera_driver/src/camera.cpp b/spinnaker_camera_driver/src/camera.cpp index d8b3d67e..58b04ce6 100644 --- a/spinnaker_camera_driver/src/camera.cpp +++ b/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_) { diff --git a/spinnaker_camera_driver/src/image.cpp b/spinnaker_camera_driver/src/image.cpp index 62318f4d..5711f2ba 100644 --- a/spinnaker_camera_driver/src/image.cpp +++ b/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 diff --git a/spinnaker_camera_driver/src/spinnaker_wrapper_impl.cpp b/spinnaker_camera_driver/src/spinnaker_wrapper_impl.cpp index 2cb8be77..f8fc2da2 100644 --- a/spinnaker_camera_driver/src/spinnaker_wrapper_impl.cpp +++ b/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); } } diff --git a/spinnaker_camera_driver/src/spinnaker_wrapper_impl.hpp b/spinnaker_camera_driver/src/spinnaker_wrapper_impl.hpp index 27fbd8ee..76812774 100644 --- a/spinnaker_camera_driver/src/spinnaker_wrapper_impl.hpp +++ b/spinnaker_camera_driver/src/spinnaker_wrapper_impl.hpp @@ -83,6 +83,7 @@ class SpinnakerWrapperImpl : public Spinnaker::ImageEventHandler std::shared_ptr thread_; std::mutex mutex_; uint64_t acquisitionTimeout_{10000000000ULL}; + size_t numIncompleteImages_{0}; }; } // namespace spinnaker_camera_driver diff --git a/spinnaker_synchronized_camera_driver/include/spinnaker_synchronized_camera_driver/time_keeper.hpp b/spinnaker_synchronized_camera_driver/include/spinnaker_synchronized_camera_driver/time_keeper.hpp index 56fdfb37..4d5ed177 100644 --- a/spinnaker_synchronized_camera_driver/include/spinnaker_synchronized_camera_driver/time_keeper.hpp +++ b/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(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}; diff --git a/spinnaker_synchronized_camera_driver/src/synchronized_camera_driver.cpp b/spinnaker_synchronized_camera_driver/src/synchronized_camera_driver.cpp index b2c44afc..5a09c6a5 100644 --- a/spinnaker_synchronized_camera_driver/src/synchronized_camera_driver.cpp +++ b/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 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); } } diff --git a/spinnaker_synchronized_camera_driver/src/time_keeper.cpp b/spinnaker_synchronized_camera_driver/src/time_keeper.cpp index 7a53d90f..596e7c97 100644 --- a/spinnaker_synchronized_camera_driver/src/time_keeper.cpp +++ b/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(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; }