Remove potential rs bug, and reduce unique locks

components/net/cpp/src/universe.cpp

@@ -122,6 +122,7 @@ int Universe::_setDescriptors() {
 
 	SOCKET n = 0;
 
+	// TODO Shared lock for some of the time...
 	UNIQUE_LOCK(net_mutex_,lk);
 
 	//Set file descriptor for the listening sockets.
@@ -324,6 +325,7 @@ void Universe::_run() {
 		}
 
 		// CHECK Could this mutex be the problem!?
+		{
 			UNIQUE_LOCK(net_mutex_,lk);
 
 			//If connection request is waiting
@@ -344,8 +346,11 @@ void Universe::_run() {
 					}
 				}
 			}
+		}
 
 		// TODO(Nick) Might switch to shared lock here?
+		{
+			SHARED_LOCK(net_mutex_, lk);
 
 			//Also check each clients socket to see if any messages or errors are waiting
 			for (auto s : peers_) {
@@ -360,8 +365,10 @@ void Universe::_run() {
 					}
 				}
 			}
+		}
+
 		// TODO(Nick) Don't always need to call this
-		_cleanupPeers();
+		//_cleanupPeers();
 	}
 }
 

components/rgbd-sources/src/net.cpp

@@ -70,11 +70,12 @@ void NetSource::_recv(const vector<unsigned char> &jpg, const vector<unsigned ch
 	cv::imdecode(jpg, cv::IMREAD_COLOR, &tmp_rgb);
 	cv::imdecode(d, cv::IMREAD_UNCHANGED, &tmp_depth);
 
+	// Lock host to prevent grab
 	UNIQUE_LOCK(host_->mutex(),lk);
 	rgb_ = tmp_rgb;
 	tmp_depth.convertTo(depth_, CV_32FC1, 1.0f/(16.0f*100.0f));
 	N_--;
-	lk.unlock();
+	//lk.unlock();
 }
 
 void NetSource::setPose(const Eigen::Matrix4f &pose) {

components/rgbd-sources/src/realsense_source.cpp

@@ -44,15 +44,11 @@ bool RealsenseSource::grab() {
     rs2::depth_frame depth = frames.get_depth_frame();
     float w = depth.get_width();
     float h = depth.get_height();
-    rs2::frame colour = frames.first(RS2_STREAM_COLOR); //.get_color_frame();
+    rscolour_ = frames.first(RS2_STREAM_COLOR); //.get_color_frame();
 
-    //LOG(INFO) << " RS Frame size = " << w << "x" << h;
-
-    //std::unique_lock<std::mutex> lk(mutex_);
     cv::Mat tmp(cv::Size((int)w, (int)h), CV_16UC1, (void*)depth.get_data(), depth.get_stride_in_bytes());
     tmp.convertTo(depth_, CV_32FC1, scale_);
-    rgb_ = cv::Mat(cv::Size(w, h), CV_8UC4, (void*)colour.get_data(), cv::Mat::AUTO_STEP);
-    //LOG(INFO) << "RS FRAME GRABBED: " << rgb_.cols << "x" << rgb_.rows;
+    rgb_ = cv::Mat(cv::Size(w, h), CV_8UC4, (void*)rscolour_.get_data(), cv::Mat::AUTO_STEP);
     return true;
 }
 

components/rgbd-sources/src/realsense_source.hpp

@@ -25,6 +25,7 @@ class RealsenseSource : public ftl::rgbd::detail::Source {
     float scale_;
     rs2::pipeline pipe_;
     rs2::align align_to_depth_;
+	rs2::frame rscolour_;
 };
 
 }

components/rgbd-sources/src/streamer.cpp

@@ -93,6 +93,9 @@ Streamer::~Streamer() {
 }
 
 void Streamer::add(Source *src) {
+	StreamSource *s = nullptr;
+
+	{
 		UNIQUE_LOCK(mutex_,ulk);
 		if (sources_.find(src->getID()) != sources_.end()) return;
 
@@ -100,6 +103,7 @@ void Streamer::add(Source *src) {
 		s->src = src;
 		s->state = 0;
 		sources_[src->getID()] = s;
+	}
 
 	LOG(INFO) << "Streaming: " << src->getID();
 	net_->broadcast("add_stream", src->getID());
@@ -108,7 +112,7 @@ void Streamer::add(Source *src) {
 void Streamer::_addClient(const string &source, int N, int rate, const ftl::UUID &peer, const string &dest) {
 	StreamSource *s = nullptr;
 
-	{
+	//{
 		UNIQUE_LOCK(mutex_,slk);
 		if (sources_.find(source) == sources_.end()) return;
 
@@ -118,7 +122,7 @@ void Streamer::_addClient(const string &source, int N, int rate, const ftl::UUID
 		DLOG(INFO) << "Adding Stream Peer: " << peer.to_string();
 
 		s = sources_[source];
-	}
+	//}
 
 	if (!s) return;