Fixes for pylon latency

components/common/cpp/src/timer.cpp

@@ -31,7 +31,7 @@ static bool clock_slave = true;
 struct TimerJob {
 	int id=0;
 	ftl::SingletonHandler<int64_t> job;
-	volatile bool active=false;
+	std::atomic_bool active=false;
 	// TODO: (Nick) Implement richer forms of timer
 	//bool paused;
 	//int multiplier;
@@ -165,6 +165,11 @@ static void trigger_jobs() {
 	UNIQUE_LOCK(mtx, lk);
 	const int64_t ts = last_frame*mspf;
 
+	if (active_jobs > 1) {
+		LOG(WARNING) << "Previous timer incomplete, skipping " << ts;
+		return;
+	}
+
 	// First do non-blocking high precision callbacks
 	const int64_t before = get_time();
 	for (auto &j : jobs[kTimerHighPrecision]) {
@@ -181,7 +186,7 @@ static void trigger_jobs() {
 	// Now use thread jobs to do more intensive callbacks
 	for (auto &j : jobs[kTimerMain]) {
 		if (j.active) {
-			//LOG(WARNING) << "Timer job too slow ... skipped for " << ts;
+			LOG(WARNING) << "Timer job too slow ... skipped for " << ts;
 			continue;
 		}
 		j.active = true;

components/rgbd-sources/src/sources/stereovideo/pylon.cpp

@@ -202,6 +202,7 @@ bool PylonDevice::get(cv::cuda::GpuMat &l_out, cv::cuda::GpuMat &r_out, cv::cuda
 			future_b = std::move(ftl::pool.push([this,&rfull,&r,&l,c,&r_out,&h_r,&stream](int id) {
 				Pylon::CGrabResultPtr result_right;
 				int rcount = 0;
+				if (rcam_->NumReadyBuffers.GetValue() > 1) LOG(WARNING) << "Frames lost in retrieve (right) = " << rcam_->NumReadyBuffers.GetValue(); 
 				if (rcam_ && rcam_->RetrieveResult(0, result_right, Pylon::TimeoutHandling_Return)) ++rcount;
 
 				if (rcount == 0 || !result_right->GrabSucceeded()) {
@@ -215,9 +216,13 @@ bool PylonDevice::get(cv::cuda::GpuMat &l_out, cv::cuda::GpuMat &r_out, cv::cuda
 				CV_8UC1,
 				(uint8_t*)result_right->GetBuffer());
 
+				{
+					FTL_Profile("Bayer Colour (R)", 0.005);
 					cv::cvtColor(wrap_right, rfull, cv::COLOR_BayerRG2BGRA);
+				}
 
 				if (isStereo()) {
+					FTL_Profile("Rectify and Resize (R)", 0.005);
 					c->rectify(rfull, Channel::Right);
 
 					if (hasHigherRes()) {
@@ -235,6 +240,7 @@ bool PylonDevice::get(cv::cuda::GpuMat &l_out, cv::cuda::GpuMat &r_out, cv::cuda
 		}
 
 		Pylon::CGrabResultPtr result_left;
+		if (lcam_->NumReadyBuffers.GetValue() > 1) LOG(WARNING) << "Frames lost in retrieve (left) = " << lcam_->NumReadyBuffers.GetValue();
 		int lcount = 0;
 		{
 			if (lcam_->RetrieveResult(0, result_left, Pylon::TimeoutHandling_Return)) ++lcount;
@@ -251,8 +257,13 @@ bool PylonDevice::get(cv::cuda::GpuMat &l_out, cv::cuda::GpuMat &r_out, cv::cuda
 			CV_8UC1,
 			(uint8_t*)result_left->GetBuffer());
 
+		{
+			FTL_Profile("Bayer Colour (L)", 0.005);
 			cv::cvtColor(wrap_left, lfull, cv::COLOR_BayerRG2BGRA);
+		}
 
+		{
+			FTL_Profile("Rectify and Resize (L)", 0.005);
 			if (isStereo()) {
 				c->rectify(lfull, Channel::Left);
 			}
@@ -263,6 +274,7 @@ bool PylonDevice::get(cv::cuda::GpuMat &l_out, cv::cuda::GpuMat &r_out, cv::cuda
 			} else {
 				h_l = cv::cuda::GpuMat();
 			}
+		}
 
 		l_out.upload(l, stream);