[Jderobot-admin] jderobot-r1227 - trunk/src/stable/components/openniServer

[rocapal en jderobot.org]

Author: rocapal
Date: 2014-05-05 12:27:51 +0200 (Mon, 05 May 2014)
New Revision: 1227

Modified:
   trunk/src/stable/components/openniServer/openniServer.cpp
Log:
#222 fixed task about device pre-heating


Modified: trunk/src/stable/components/openniServer/openniServer.cpp
===================================================================
--- trunk/src/stable/components/openniServer/openniServer.cpp	2014-04-29 15:18:11 UTC (rev 1226)
+++ trunk/src/stable/components/openniServer/openniServer.cpp	2014-05-05 10:27:51 UTC (rev 1227)
@@ -58,6 +58,8 @@
 #define SAMPLE_READ_WAIT_TIMEOUT 2000
 #define RETRY_MAX_TIMES 5
 
+#define MAX_TIMES_PREHEATING 10
+
 #define CHECK_RC(rc, what)                                      \
 if (rc != openni::STATUS_OK)                                         \
 {                                                               \
@@ -325,6 +327,21 @@
 	m_streams[0] = &depth;
 	m_streams[1] = &color;
 
+	jderobot::Logger::getInstance()->info("Starting device pre-heating .... ");
+	// Repeat at least MAX_TIMES_PREHEATING to wait Xtion is not cold
+	for (int i=0; i<MAX_TIMES_PREHEATING; i++)
+	{
+		int changedIndex;
+		openni::OpenNI::waitForAnyStream(m_streams, 2, &changedIndex,SAMPLE_READ_WAIT_TIMEOUT);
+
+		if (changedIndex != 0)
+			continue;
+
+		openni::VideoFrameRef mAuxFrame;
+		depth.readFrame( &mAuxFrame );
+	}
+	jderobot::Logger::getInstance()->info("End device pre-heating");
+
 	sem.broadcast();
 
 
@@ -334,6 +351,8 @@
 	bool first=true;
 
 
+
+
 	while(componentAlive){
 
 
@@ -824,7 +843,7 @@
 		    reply->timeStamp.seconds = (long)t.toSeconds();
 		    reply->timeStamp.useconds = (long)t.toMicroSeconds() - reply->timeStamp.seconds*1000000;
 			if (!m_depthFrame.isValid()){
-				pthread_mutex_unlock(&mutex);			
+				pthread_mutex_unlock(&mutex);
 				continue;
 			}