XnStatus XnSensorClient::WaitForReply(XnSensorServerCustomMessages ExpectedMessage)
{
XnStatus nRetVal = XN_STATUS_OK;
// wait for event
nRetVal = xnOSWaitEvent(m_hReplyEvent, XN_SENSOR_REPLY_TIMEOUT);
if (nRetVal != XN_STATUS_OK)
{
XN_LOG_WARNING_RETURN(nRetVal, XN_MASK_SENSOR_SERVER, "Timeout when waiting for reply from sensor server!");
}
// reset it
nRetVal = xnOSResetEvent(m_hReplyEvent);
XN_IS_STATUS_OK(nRetVal);
// check error code
if (m_LastReply.nRetVal != XN_STATUS_OK)
{
XN_LOG_WARNING_RETURN(m_LastReply.nRetVal, XN_MASK_SENSOR_SERVER, "Server returned an error: %s", xnGetStatusString(m_LastReply.nRetVal));
}
if (m_LastReply.Type != ExpectedMessage)
{
XN_LOG_WARNING_RETURN(XN_STATUS_ERROR, XN_MASK_SENSOR_SERVER, "Sensor server protocol error - invalid reply type!");
}
return (XN_STATUS_OK);
}
XnStatus XnServerSensorInvoker::ReadStreams()
{
XnStatus nRetVal = XN_STATUS_OK;
// wait for new data to be available
nRetVal = xnOSWaitEvent(m_hNewDataEvent, XN_NODE_WAIT_FOR_DATA_TIMEOUT);
if (nRetVal == XN_STATUS_OS_EVENT_TIMEOUT)
{
return XN_STATUS_OK;
}
else if (nRetVal != XN_STATUS_OK)
{
xnLogWarning(XN_MASK_SENSOR_SERVER, "Got error waiting for new data event: %s", xnGetStatusString(nRetVal));
// but continue anyway
}
// lock sensor (we iterate over streams list. make sure no stream is added/removed from the list)
{
XnLockedServerStreamsHash lockedHash = m_streams.GetLockedHashForIterating();
for (XnLockedServerStreamsHash::Iterator it = lockedHash.Begin(); it != lockedHash.End(); ++it)
{
SensorInvokerStream& stream = it->Value();
if (stream.bNewData)
{
// ignore audio (it is read by every client)
if (strcmp(stream.strType, XN_STREAM_NAME_AUDIO) != 0)
{
// read this data
nRetVal = m_sensor.ReadStream(stream.pStreamData);
if (nRetVal != XN_STATUS_OK)
{
xnLogWarning(XN_MASK_SENSOR_SERVER, "Failed reading from stream %s (though event was raised): %s", stream.strType, xnGetStatusString(nRetVal));
stream.bNewData = FALSE;
continue;
}
}
stream.bNewData = FALSE;
NewStreamDataEventArgs eventArgs;
eventArgs.strStreamName = stream.strType;
eventArgs.nTimestamp = stream.pStreamData->nTimestamp;
eventArgs.nFrameID = stream.pStreamData->nFrameID;
stream.pNewDataEvent->Raise(eventArgs);
}
} // streams loop
} // lock
return (XN_STATUS_OK);
}
void VideoStream::newFrameThreadMainloop()
{
XnStatus rc = XN_STATUS_OK;
// Wait on frame
while (m_running)
{
rc = xnOSWaitEvent(m_newFrameInternalEvent, XN_WAIT_INFINITE);
if ((rc == XN_STATUS_OK) && m_running)
{
m_newFrameEvent.Raise();
// HACK: To avoid starvation of other threads.
xnOSSleep(1);
}
}
}
XnStatus xnOSWaitForCondition(const XN_EVENT_HANDLE EventHandle, XnUInt32 nMilliseconds, XnConditionFunc pConditionFunc, void* pConditionData)
{
XnStatus nRetVal = XN_STATUS_OK;
// take read start time (for timeout purposes)
XnUInt64 nStarted;
nRetVal = xnOSGetTimeStamp(&nStarted);
XN_IS_STATUS_OK(nRetVal);
XnBool bTimeout = FALSE;
// as long as condition isn't met
while (!pConditionFunc(pConditionData))
{
// check if timeout occurred
XnUInt64 nNow;
nRetVal = xnOSGetTimeStamp(&nNow);
XN_IS_STATUS_OK(nRetVal);
if (nNow - nStarted > nMilliseconds)
{
bTimeout = TRUE;
}
else
{
// not yet. Wait for event to be set
nRetVal = xnOSWaitEvent(EventHandle, (XnUInt32)(nMilliseconds - (nNow - nStarted)));
if (nRetVal == XN_STATUS_OS_EVENT_TIMEOUT)
{
bTimeout = TRUE;
}
else if (nRetVal != XN_STATUS_OK)
{
xnLogWarning(XN_MASK_OS, "Failed waiting on event for condition...");
}
}
if (bTimeout)
{
return (XN_STATUS_OS_EVENT_TIMEOUT);
}
}
// condition was met
return (XN_STATUS_OK);
}
void VideoStream::newFrameThreadMainloop()
{
XnStatus rc = XN_STATUS_OK;
// Wait on frame
while (m_running)
{
rc = xnOSWaitEvent(m_newFrameInternalEvent, XN_WAIT_INFINITE);
if ((rc == XN_STATUS_OK) && m_running)
{
m_newFrameEvent.Raise();
if (m_pContextNewFrameEvent != NULL)
{
m_pContextNewFrameEvent->Set();
}
}
}
}
XnStatus SocketInConnection::Connect()
{
XnStatus nRetVal = XN_STATUS_OK;
Disconnect(); // In case we're already connected
nRetVal = xnOSCreateThread(&ReadThreadProc, this, &m_hReadThread);
XN_IS_STATUS_OK_LOG_ERROR("Create input socket read thread", nRetVal);
xnLogVerbose(XN_MASK_LINK, "Waiting for connection on socket %u...", m_nPort);
nRetVal = xnOSWaitEvent(m_hConnectEvent, CONNECT_TIMEOUT);
XN_IS_STATUS_OK_LOG_ERROR("Wait for input socket to connect", nRetVal);
if (m_nConnectionStatus != XN_STATUS_OK)
{
xnLogError(XN_MASK_LINK, "Failed to connect to socket %u: %s", m_nPort, xnGetStatusString(m_nConnectionStatus));
XN_ASSERT(FALSE);
return m_nConnectionStatus;
}
xnLogVerbose(XN_MASK_LINK, "Socket %u connected.", m_nPort);
nRetVal = xnOSSetThreadPriority(m_hReadThread, XN_PRIORITY_CRITICAL);
XN_IS_STATUS_OK_LOG_ERROR("Set read thread priority", nRetVal);
return XN_STATUS_OK;
}
/* This is the actual scheduler function. It is being run in its own thread. */
XN_THREAD_PROC xnSchedulerThreadFunc(XN_THREAD_PARAM pThreadParam)
{
XnScheduler* pScheduler = (XnScheduler*)pThreadParam;
XnUInt64 nNow;
while (!pScheduler->bStopThread)
{
// check when next task should be executed
XnUInt64 nWait = XN_WAIT_INFINITE;
XnScheduledTask* pTask = NULL;
XnTaskCallbackFuncPtr pCallback = NULL;
void* pCallbackArg = NULL;
// check if something is in the list
if (pScheduler->pFirst != NULL)
{
// enter critical section
xnOSEnterCriticalSection(&pScheduler->hCriticalSection);
pTask = pScheduler->pFirst;
if (pTask != NULL)
{
xnOSGetTimeStamp(&nNow);
if (pTask->nNextTime < nNow)
{
// task should be executed
pCallback = pTask->pCallback;
pCallbackArg = pTask->pCallbackArg;
// remove it from the list
pScheduler->pFirst = pTask->pNextTask;
// calculate next time
pTask->nNextTime += pTask->nInterval;
// add it to the list again
xnSchedulerAddTaskInternal(pScheduler, pTask);
}
else
{
nWait = pTask->nNextTime - nNow;
}
}
// leave critical section
xnOSLeaveCriticalSection(&pScheduler->hCriticalSection);
if (pCallback != NULL)
{
// execute task (outside critical section)
pCallback(pCallbackArg);
// no need to wait (we don't know how much time did callback take)
nWait = 0;
}
}
// wait for a change of the list, or the time of the next task
xnOSWaitEvent(pScheduler->hWakeThreadEvent, (XnUInt32)nWait);
}
XN_THREAD_PROC_RETURN(XN_STATUS_OK);
}
XN_C_API XnBool xnOSIsEventSet(const XN_EVENT_HANDLE EventHandle)
{
return (xnOSWaitEvent(EventHandle, 0) == XN_STATUS_OK);
}
OniStatus Context::waitForStreams(OniStreamHandle* pStreams, int streamCount, int* pStreamIndex, int timeout)
{
static const int MAX_WAITED_STREAMS = 50;
Device* deviceList[MAX_WAITED_STREAMS];
VideoStream* streamsList[MAX_WAITED_STREAMS];
unsigned long long oldestTimestamp = XN_MAX_UINT64;
int oldestIndex = -1;
if (streamCount > MAX_WAITED_STREAMS)
{
m_errorLogger.Append("Cannot wait on more than %d streams", MAX_WAITED_STREAMS);
return ONI_STATUS_NOT_SUPPORTED;
}
int numDevices = 0;
for (int i = 0; i < streamCount; ++i)
{
if (pStreams[i] == NULL)
{
continue;
}
streamsList[i] = ((_OniStream*)pStreams[i])->pStream;
Device* pDevice = &streamsList[i]->getDevice();
// Check if device already exists.
bool found = false;
for (int j = 0; j < numDevices; ++j)
{
if (deviceList[j] == pDevice)
{
found = true;
break;
}
}
// Add new device to list.
if (!found)
{
deviceList[numDevices] = pDevice;
++numDevices;
}
}
XN_EVENT_HANDLE hEvent = getThreadEvent();
XnUInt64 passedTime;
XnOSTimer workTimer;
XnUInt32 timeToWait = timeout;
xnOSStartTimer(&workTimer);
do
{
for (int i = 0; i < streamCount; ++i)
{
if (pStreams[i] == NULL)
continue;
VideoStream* pStream = ((_OniStream*)pStreams[i])->pStream;
pStream->lockFrame();
OniFrame* pFrame = pStream->peekFrame();
if (pFrame != NULL && pFrame->timestamp < oldestTimestamp)
{
oldestTimestamp = pFrame->timestamp;
oldestIndex = i;
}
pStream->unlockFrame();
}
if (oldestIndex != -1)
{
*pStreamIndex = oldestIndex;
break;
}
// 'Poke' the driver to attempt to receive more frames.
for (int j = 0; j < numDevices; ++j)
{
deviceList[j]->tryManualTrigger();
}
if(timeout != ONI_TIMEOUT_FOREVER)
{
xnOSQueryTimer(workTimer, &passedTime);
if((int)passedTime < timeout)
timeToWait = timeout - (int)passedTime;
else
timeToWait = 0;
}
} while (XN_STATUS_OK == xnOSWaitEvent(hEvent, timeToWait));
xnOSStopTimer(&workTimer);
if (oldestIndex != -1)
{
return ONI_STATUS_OK;
}
m_errorLogger.Append("waitForStreams: timeout reached");
return ONI_STATUS_TIME_OUT;
}
XnStatus VideoStream::waitForNewFrameEvent()
{
return xnOSWaitEvent(m_newFrameInternalEventForFrameHolder, XN_WAIT_INFINITE);
}
