XnStatus XnSensor::WaitForPrimaryStream(XN_EVENT_HANDLE hNewDataEvent, XnStreamDataSet* pSet)
{
XnStatus nRetVal = XN_STATUS_OK;
if (m_FrameSync.GetValue() == TRUE)
{
// FrameSync is on. check if we have both the image and the depth stream
XnStreamData* apStreamData[XN_DEVICE_BASE_MAX_STREAMS_COUNT];
XnUInt32 nCount = XN_DEVICE_BASE_MAX_STREAMS_COUNT;
nRetVal = XnStreamDataSetCopyToArray(pSet, apStreamData, &nCount);
XN_IS_STATUS_OK(nRetVal);
const XnChar* strImageName = NULL;
const XnChar* strDepthName = NULL;
XnChar strType[XN_DEVICE_MAX_STRING_LENGTH];
for (XnUInt32 i = 0; i < nCount; ++i)
{
nRetVal = GetProperty(apStreamData[i]->StreamName, XN_STREAM_PROPERTY_TYPE, strType);
XN_IS_STATUS_OK(nRetVal);
if (strcmp(strType, XN_STREAM_TYPE_DEPTH) == 0)
{
strDepthName = apStreamData[i]->StreamName;
}
else if (strcmp(strType, XN_STREAM_TYPE_IMAGE) == 0)
{
strImageName = apStreamData[i]->StreamName;
}
// if both are present, wait for frame sync
if (strImageName != NULL && strDepthName != NULL)
{
XnWaitForSycnhedFrameData WaitData;
WaitData.pThis = this;
WaitData.strDepthStream = strDepthName;
WaitData.strImageStream = strImageName;
nRetVal = xnOSWaitForCondition(hNewDataEvent, XN_DEVICE_READ_FRAME_TIMEOUT, &XnSensor::HasSynchedFrameArrived, &WaitData);
if (nRetVal == XN_STATUS_OS_EVENT_TIMEOUT)
{
xnLogError(XN_MASK_DDK, "Not responding - Didn't get any synced frame");
return (XN_STATUS_DEVICE_FRAMES_NOT_SYNCHED);
}
else
{
XN_IS_STATUS_OK(nRetVal);
}
}
} // for loop
}
// if we reached here, either frame sync is off, or one of the streams is not read from.
// either way, we should just wait for the primary stream.
nRetVal = XnDeviceBase::WaitForPrimaryStream(hNewDataEvent, pSet);
XN_IS_STATUS_OK(nRetVal);
return (XN_STATUS_OK);
}
XnBool XnDeviceBase::HasPrimaryStreamAdvanced(XnStreamDataSet* pOutputSet)
{
if (strcmp(m_PrimaryStream.GetValue(), XN_PRIMARY_STREAM_NONE) == 0)
{
// special case of None: condition is always true
return TRUE;
}
const XnChar* astrNames[XN_DEVICE_BASE_MAX_STREAMS_COUNT];
XnUInt32 nArraySize = XN_DEVICE_BASE_MAX_STREAMS_COUNT;
// take a list of streams to check for new data
if (strcmp(m_PrimaryStream.GetValue(), XN_PRIMARY_STREAM_ANY) != 0)
{
// we have a specific stream. Add it to the list
astrNames[0] = m_PrimaryStream.GetValue();
nArraySize = 1;
}
else
{
// special case of ANY. we need to check every one of requested streams
XnStreamData* apStreamOutputs[XN_DEVICE_BASE_MAX_STREAMS_COUNT];
if (XN_STATUS_OK != XnStreamDataSetCopyToArray(pOutputSet, apStreamOutputs, &nArraySize))
{
return FALSE;
}
for (XnUInt32 i = 0; i < nArraySize; ++i)
{
astrNames[i] = apStreamOutputs[i]->StreamName;
}
}
// now check if we have new data
for (XnUInt32 i = 0; i < nArraySize; ++i)
{
XnDeviceStream* pStream = NULL;
if (XN_STATUS_OK == FindStream(astrNames[i], &pStream))
{
if (pStream->IsNewDataAvailable())
return TRUE;
}
}
return FALSE;
}
XnStatus XnDeviceFileWriter::Write(XnStreamDataSet* pStreamOutputSet)
{
XnStatus nRetVal = XN_STATUS_OK;
XN_VALIDATE_INPUT_PTR(pStreamOutputSet);
// get a list of objects in the set
XnStreamData* aOutputs[XN_DEVICE_BASE_MAX_STREAMS_COUNT];
XnUInt32 nCount = XN_DEVICE_BASE_MAX_STREAMS_COUNT;
nRetVal = XnStreamDataSetCopyToArray(pStreamOutputSet, aOutputs, &nCount);
XN_IS_STATUS_OK(nRetVal);
// BC: old applications wrote down all streams every frame, even if no new data. This might
// cause some of the streams to have timestamp 0, while other have a real timestamp.
// In this case, remove these frames. However, we need to check - if all timestamps
// are 0, then we'll fill it up by ourselves.
XnBool bSomeHasTimestamps = FALSE;
for (XnUInt32 i = 0; i < nCount; ++i)
{
if (aOutputs[i]->nTimestamp != 0)
{
bSomeHasTimestamps = TRUE;
break;
}
}
if (bSomeHasTimestamps)
{
// remove all the ones with zero timestamp
for (XnUInt32 i = 0; i < nCount; ++i)
{
if (aOutputs[i]->nTimestamp == 0)
{
aOutputs[i]->bIsNew = FALSE;
}
}
}
// OK. write it down using base
nRetVal = XnStreamWriterDevice::Write(pStreamOutputSet);
XN_IS_STATUS_OK(nRetVal);
return (XN_STATUS_OK);
}
XnStatus XnDeviceBase::Read(XnStreamDataSet* pStreamOutputSet)
{
XnStatus nRetVal = XN_STATUS_OK;
XN_VALIDATE_INPUT_PTR(pStreamOutputSet);
if (m_ReadWriteMode.GetValue() == XN_DEVICE_MODE_WRITE)
{
return XN_STATUS_IO_DEVICE_WRONG_MODE;
}
XnUInt64 nNow;
xnOSGetHighResTimeStamp(&nNow);
xnDumpFileWriteString(m_StreamsDataDump, "%llu,Read Called\n", nNow);
// First thing to do is wait for primary stream to advance. We do this BEFORE checking streams
// because device streams might change during this wait.
nRetVal = WaitForPrimaryStream(m_hNewDataEvent, pStreamOutputSet);
XN_IS_STATUS_OK(nRetVal);
xnOSGetHighResTimeStamp(&nNow);
xnDumpFileWriteString(m_StreamsDataDump, "%llu,Read Condition Met\n", nNow);
// take the list of stream output objects
XnStreamData* apStreamOutputs[XN_DEVICE_BASE_MAX_STREAMS_COUNT];
XnUInt32 nOutputsCount = XN_DEVICE_BASE_MAX_STREAMS_COUNT;
nRetVal = XnStreamDataSetCopyToArray(pStreamOutputSet, apStreamOutputs, &nOutputsCount);
XN_IS_STATUS_OK(nRetVal);
// now read
for (XnUInt32 nIndex = 0; nIndex < nOutputsCount; ++nIndex)
{
// find its corresponding stream
XnDeviceStream* pStream;
nRetVal = FindStream(apStreamOutputs[nIndex]->StreamName, &pStream);
XN_IS_STATUS_OK(nRetVal);
nRetVal = ReadFromStreamImpl(pStream, apStreamOutputs[nIndex]);
XN_IS_STATUS_OK(nRetVal);
}
return (XN_STATUS_OK);
}
XnStatus XnDeviceBase::Write(XnStreamDataSet* pStreamDataSet)
{
XnStatus nRetVal = XN_STATUS_OK;
XN_VALIDATE_INPUT_PTR(pStreamDataSet);
if (m_ReadWriteMode.GetValue() != XN_DEVICE_MODE_WRITE)
{
return XN_STATUS_IO_DEVICE_WRONG_MODE;
}
// take the list of stream output objects
XnStreamData* apStreamOutputs[XN_DEVICE_BASE_MAX_STREAMS_COUNT];
XnUInt32 nOutputsCount = XN_DEVICE_BASE_MAX_STREAMS_COUNT;
nRetVal = XnStreamDataSetCopyToArray(pStreamDataSet, apStreamOutputs, &nOutputsCount);
XN_IS_STATUS_OK(nRetVal);
// find the stream for each one
XnDeviceStream* apStreams[XN_DEVICE_BASE_MAX_STREAMS_COUNT];
for (XnUInt32 nIndex = 0; nIndex < nOutputsCount; ++nIndex)
{
// find its corresponding stream
nRetVal = FindStream(apStreamOutputs[nIndex]->StreamName, &apStreams[nIndex]);
XN_IS_STATUS_OK(nRetVal);
// make sure it is open
if (!apStreams[nIndex]->IsOpen())
{
return XN_STATUS_STREAM_NOT_OPEN;
}
}
// go over them, and write into each one
for (XnUInt32 nIndex = 0; nIndex < nOutputsCount; ++nIndex)
{
nRetVal = apStreams[nIndex]->Write(apStreamOutputs[nIndex]);
XN_IS_STATUS_OK(nRetVal);
}
return (XN_STATUS_OK);
}
