Use correct framerate

components/rgbd-sources/include/ftl/rgbd_streamer.hpp

@@ -49,7 +49,7 @@ class Streamer : public ftl::Configurable {
 	void remove(RGBDSource *);
 	void remove(const std::string &);
 
-	void run();
+	void run(bool block=false);
 	void stop();
 
 	RGBDSource *get(const std::string &uri);

components/rgbd-sources/src/rgbd_streamer.cpp

@@ -83,7 +83,7 @@ void Streamer::add(RGBDSource *src) {
 	sources_[src->getURI()] = s;
 }
 
-void Streamer::_addClient(const string &source, int N, int rate, const UUID &peer, const string &dest) {
+void Streamer::_addClient(const string &source, int N, int rate, const ftl::UUID &peer, const string &dest) {
 	shared_lock<shared_mutex> slk(mutex_);
 	if (sources_.find(source) == sources_.end()) return;
 
@@ -113,20 +113,38 @@ void Streamer::stop() {
 	active_ = false;
 }
 
-void Streamer::run() {
+void Streamer::run(bool block) {
 	active_ = true;
 
+	if (block) {
+		while (active_) {
+			double wait = 1.0f / 25.0f;
+			auto start = std::chrono::high_resolution_clock::now();
+			// Create frame jobs at correct FPS interval
+			_schedule();
+
+			std::chrono::duration<double> elapsed =
+				std::chrono::high_resolution_clock::now() - start;
+
+			sleep_for(milliseconds((long long)((wait - elapsed.count()) * 1000.0f)));
+		}
+	} else {
 		// Create thread job for frame ticking
 		pool_.push([this](int id) {
 			while (active_) {
+				double wait = 1.0f / 25.0f;
+				auto start = std::chrono::high_resolution_clock::now();
 				// Create frame jobs at correct FPS interval
 				_schedule();
 
-			// TODO Proper time control
-			sleep_for(milliseconds(40));
+				std::chrono::duration<double> elapsed =
+					std::chrono::high_resolution_clock::now() - start;
+
+				sleep_for(milliseconds((long long)((wait - elapsed.count()) * 1000.0f)));
 			}
 		});
 	}
+}
 
 void Streamer::_swap(StreamSource &src) {
 	if (src.state == (ftl::rgbd::detail::kGrabbed | ftl::rgbd::detail::kTransmitted)) {