XnStatus XnDeviceFileWriter::DestroyStream(const XnChar* StreamName)
{
XnStatus nRetVal = XN_STATUS_OK;
// before closing this stream, we want to write down how many frames were written
XnFileWriterStream* pStream;
nRetVal = FindStream(StreamName, &pStream);
XN_IS_STATUS_OK(nRetVal);
XnUInt64 nCurPos;
nRetVal = GetIOStream()->Tell(&nCurPos);
XN_IS_STATUS_OK(nRetVal);
nRetVal = GetIOStream()->Seek(pStream->m_nNumFramesPos);
XN_IS_STATUS_OK(nRetVal);
nRetVal = GetDataPacker()->WriteProperty(StreamName, XN_STREAM_PROPERTY_NUMBER_OF_FRAMES, pStream->GetNumberOfFrames());
XN_IS_STATUS_OK(nRetVal);
// now seek back
nRetVal = GetIOStream()->Seek(nCurPos);
XN_IS_STATUS_OK(nRetVal);
// and destroy it
nRetVal = XnStreamWriterDevice::DestroyStream(StreamName);
XN_IS_STATUS_OK(nRetVal);
return (XN_STATUS_OK);
}
XnStatus XnDeviceFileReader::HandleStreamRemoved(const XnChar* strName)
{
XnStatus nRetVal = XN_STATUS_OK;
// check for specific case: all streams are removed and then end-of-file is reached.
// in this case, we don't really want to destroy streams, just wrap around.
XnStringsSet StreamsToRemove;
nRetVal = StreamsToRemove.Set(strName);
XN_IS_STATUS_OK(nRetVal);
XnPackedDataType nType = XN_PACKED_STREAM_REMOVED;
XnUInt64 nPositionBefore;
for (;;)
{
nRetVal = GetIOStream()->Tell(&nPositionBefore);
XN_IS_STATUS_OK(nRetVal);
nRetVal = GetDataPacker()->ReadNextObject(&nType);
XN_IS_STATUS_OK(nRetVal);
if (nType == XN_PACKED_STREAM_REMOVED)
{
XnChar strTempName[XN_DEVICE_MAX_STRING_LENGTH];
nRetVal = GetDataPacker()->ReadStreamRemoved(strTempName);
XN_IS_STATUS_OK(nRetVal);
nRetVal = StreamsToRemove.Set(strTempName);
XN_IS_STATUS_OK(nRetVal);
}
else
{
break;
}
}
if (nType != XN_PACKED_END)
{
// Not the case we were looking for. Remove those streams.
for (XnStringsSet::Iterator it = StreamsToRemove.Begin(); it != StreamsToRemove.End(); ++it)
{
nRetVal = XnStreamReaderDevice::HandleStreamRemoved(it->Key());
XN_IS_STATUS_OK(nRetVal);
}
}
// in any case, the last object we read wasn't handled yet (end-of-stream or another event), so
// seek back, so it will be handled.
nRetVal = GetIOStream()->Seek(nPositionBefore);
XN_IS_STATUS_OK(nRetVal);
return (XN_STATUS_OK);
}
XnStatus XnSensorClient::Listen()
{
XnStatus nRetVal = XN_STATUS_OK;
while (m_bShouldRun)
{
nRetVal = ReadNextEventFromStream();
if (nRetVal == XN_STATUS_OS_NETWORK_TIMEOUT)
{
continue;
}
else if ((nRetVal == XN_STATUS_OS_NETWORK_CONNECTION_CLOSED) && !m_bShouldRun)
{
xnLogInfo(XN_MASK_SENSOR_CLIENT, "Client connection was closed gracefully");
}
else if (nRetVal != XN_STATUS_OK)
{
XnIONetworkStream* pStream = (XnIONetworkStream*)GetIOStream();
if (!pStream->IsConnected())
{
m_bConnected = FALSE;
xnLogError(XN_MASK_SENSOR_CLIENT, "Server has disconnected!");
break;
}
else
{
xnLogWarning(XN_MASK_SENSOR_CLIENT, "Sensor client failed to handle event: %s", xnGetStatusString(nRetVal));
}
}
}
return (XN_STATUS_OK);
}
XnStatus XnDeviceFileReader::HandleStreamData(XnStreamData* pDataProps, XnCompressionFormats nCompression, XnUInt32 nCompressedSize)
{
XnStatus nRetVal = XN_STATUS_OK;
XnUInt64 nPosition;
nRetVal = GetIOStream()->Tell(&nPosition);
XN_IS_STATUS_OK(nRetVal);
XnUIntHash::Iterator it = m_PositionsToIgnore.End();
if (XN_STATUS_OK == m_PositionsToIgnore.Find(nPosition, it))
{
// ignore this one. Just update the frame ID
XnStreamDeviceStreamHolder* pHolder;
nRetVal = FindStream(pDataProps->StreamName, &pHolder);
XN_IS_STATUS_OK(nRetVal);
XnStreamReaderStream* pStream = (XnStreamReaderStream*)pHolder->GetStream();
pStream->NewDataAvailable(pDataProps->nTimestamp, pDataProps->nFrameID);
// and remove it from list
nRetVal = m_PositionsToIgnore.Remove(it);
XN_IS_STATUS_OK(nRetVal);
}
else
{
// normal case. handle it
nRetVal = XnStreamReaderDevice::HandleStreamData(pDataProps, nCompression, nCompressedSize);
XN_IS_STATUS_OK(nRetVal);
}
return (XN_STATUS_OK);
}
XnStatus XnDeviceFileReader::ReadFileVersion()
{
XnStatus nRetVal = XN_STATUS_OK;
// read magic from file
XnChar csFileMagic[XN_DEVICE_FILE_MAGIC_LEN];
nRetVal = GetIOStream()->ReadData((XnUChar*)csFileMagic, XN_DEVICE_FILE_MAGIC_LEN);
XN_IS_STATUS_OK(nRetVal);
if (strncmp(csFileMagic, XN_DEVICE_FILE_MAGIC_V4, XN_DEVICE_FILE_MAGIC_LEN) == 0)
{
m_nFileVersion = 4;
}
else if (strncmp(csFileMagic, XN_DEVICE_FILE_MAGIC_V3, XN_DEVICE_FILE_MAGIC_LEN) == 0)
{
m_nFileVersion = 3;
}
else if (strncmp(csFileMagic, XN_DEVICE_FILE_MAGIC_V2, XN_DEVICE_FILE_MAGIC_LEN) == 0)
{
m_nFileVersion = 2;
}
else if (strncmp(csFileMagic, XN_DEVICE_FILE_MAGIC_V1, XN_DEVICE_FILE_MAGIC_LEN) == 0)
{
m_nFileVersion = 1;
}
else
{
XN_LOG_ERROR_RETURN(XN_STATUS_DEVICE_FILE_CORRUPTED, XN_MASK_FILE, "Invalid file magic!");
}
return (XN_STATUS_OK);
}
XnStatus XnDeviceFileWriter::CreateStream(const XnChar* StreamType, const XnChar* StreamName /* = NULL */, const XnPropertySet* pInitialValues /* = NULL */)
{
XnStatus nRetVal = XN_STATUS_OK;
// create stream using base (this will also write it down to the file)
nRetVal = XnStreamWriterDevice::CreateStream(StreamType, StreamName, pInitialValues);
XN_IS_STATUS_OK(nRetVal);
// now, we leave a place for a property change - the number of frames. We will update this
// property once the stream is closed.
XnFileWriterStream* pStream;
nRetVal = FindStream(StreamName, &pStream);
XN_IS_STATUS_OK(nRetVal);
nRetVal = GetIOStream()->Tell(&pStream->m_nNumFramesPos);
XN_IS_STATUS_OK(nRetVal);
nRetVal = GetDataPacker()->WriteProperty(StreamName, XN_STREAM_PROPERTY_NUMBER_OF_FRAMES, 0ULL);
XN_IS_STATUS_OK(nRetVal);
return (XN_STATUS_OK);
}
XnStatus XnDeviceFileReader::SeekTo(XnUInt64 nMinTimestamp, XnUInt32 nMinFrameID)
{
XnStatus nRetVal = XN_STATUS_OK;
// first check if we need to seek forward or backwards (even if we're in the correct location,
// we need to rewind, so that next read will return this frame again).
if ((nMinTimestamp != 0 && nMinTimestamp <= GetLastTimestamp()) ||
(nMinFrameID != 0 && nMinFrameID <= GetLastFrameID()))
{
nRetVal = Rewind();
XN_IS_STATUS_OK(nRetVal);
}
XnBool bFoundNewData = FALSE;
// Keep current position.
XnUInt64 nStartingPosition;
nRetVal = GetIOStream()->Tell(&nStartingPosition);
XN_IS_STATUS_OK(nRetVal);
// Take primary stream (it determines frame ID and timestamp)
XnPackedDataType nType = (XnPackedDataType)-1;
const XnChar* strPrimaryStream = GetPrimaryStream();
if (strcmp(strPrimaryStream, XN_PRIMARY_STREAM_ANY) == 0 ||
strcmp(strPrimaryStream, XN_PRIMARY_STREAM_NONE) == 0)
{
strPrimaryStream = NULL;
}
// start seeking forward until point is reached.
XnUInt64 nFoundPosition;
XnLastStreamDataHash StreamsHash;
for (;;)
{
XnUInt64 nPositionBefore;
nRetVal = GetIOStream()->Tell(&nPositionBefore);
XN_IS_STATUS_OK(nRetVal);
nRetVal = GetDataPacker()->ReadNextObject(&nType);
XN_IS_STATUS_OK(nRetVal);
XnUInt64 nPositionAfter;
nRetVal = GetIOStream()->Tell(&nPositionAfter);
XN_IS_STATUS_OK(nRetVal);
if (nType == XN_PACKED_STREAM_DATA)
{
bFoundNewData = TRUE;
XnStreamData props;
XnCompressionFormats nCompression;
XnUInt32 nCompressedSize;
nRetVal = GetDataPacker()->ReadStreamDataProps(&props, &nCompression, &nCompressedSize);
XN_IS_STATUS_OK(nRetVal);
XnLastStreamData data;
if (XN_STATUS_OK != StreamsHash.Get(props.StreamName, data))
{
XnStreamDeviceStreamHolder* pHolder;
nRetVal = FindStream(props.StreamName, &pHolder);
XN_IS_STATUS_OK(nRetVal);
data.nFrameID = pHolder->GetStream()->GetLastFrameID() + 1;
}
else
{
// if we had previous data from this stream, ignore it
m_PositionsToIgnore.Set(data.nPosition, 0);
++data.nFrameID;
}
data.nPosition = nPositionAfter;
data.nTimestamp = props.nTimestamp;
nRetVal = StreamsHash.Set(props.StreamName, data);
XN_IS_STATUS_OK(nRetVal);
// now check if condition is met
if (strPrimaryStream == NULL ||
strcmp(strPrimaryStream, props.StreamName) == 0)
{
if (data.nFrameID >= nMinFrameID && data.nTimestamp >= nMinTimestamp)
{
// we have everything we need
// keep this position (we'll read up till here).
nFoundPosition = nPositionAfter;
break;
}
}
}
else if (nType == XN_PACKED_END)
{
// we'll read up to the last data of each stream
nFoundPosition = nPositionBefore;
break;
}
}
// now seek back
nRetVal = GetIOStream()->Seek(nStartingPosition);
XN_IS_STATUS_OK(nRetVal);
if (bFoundNewData)
{
// read everything up to position
XnUInt64 nPositionAfter = nStartingPosition;
while (nPositionAfter < nFoundPosition)
{
nRetVal = ReadNextEventFromStream(&nType);
XN_IS_STATUS_OK(nRetVal);
nRetVal = GetIOStream()->Tell(&nPositionAfter);
XN_IS_STATUS_OK(nRetVal);
}
}
else
{
// just remark the data as new (this is last frame, return it again to user)
XnDeviceModuleHolderList streams;
nRetVal = GetStreamsList(streams);
XN_IS_STATUS_OK(nRetVal);
for (XnDeviceModuleHolderList::Iterator it = streams.Begin(); it != streams.End(); ++it)
{
XnStreamReaderStream* pStream = (XnStreamReaderStream*)(*it)->GetModule();
pStream->ReMarkDataAsNew();
}
}
return (XN_STATUS_OK);
}
XnStatus XnDeviceFileReader::Rewind()
{
XnStatus nRetVal = XN_STATUS_OK;
// go back to start of stream
nRetVal = GetIOStream()->Seek(XN_DEVICE_FILE_MAGIC_LEN);
XN_IS_STATUS_OK(nRetVal);
// read initial state
XN_PROPERTY_SET_CREATE_ON_STACK(state);
nRetVal = ReadInitialState(&state);
XN_IS_STATUS_OK(nRetVal);
// first handle current streams. remove or reset them
XnDeviceModuleHolderList streams;
nRetVal = GetStreamsList(streams);
XN_IS_STATUS_OK(nRetVal);
for (XnDeviceModuleHolderList::Iterator it = streams.Begin(); it != streams.End(); ++it)
{
XnDeviceModuleHolder* pHolder = *it;
if (m_bStreamsCollectionChanged)
{
// we need to destroy all streams, and recreate them later
nRetVal = DestroyStream(pHolder->GetModule()->GetName());
XN_IS_STATUS_OK(nRetVal);
}
else
{
// just reset frame ID
XnStreamReaderStream* pStream = (XnStreamReaderStream*)pHolder->GetModule();
pStream->Reset();
}
}
// if we need, recreate streams
if (m_bStreamsCollectionChanged)
{
nRetVal = CreateStreams(&state);
XN_IS_STATUS_OK(nRetVal);
}
// now set state.
for (XnPropertySetData::Iterator it = state.pData->Begin(); it != state.pData->End(); ++it)
{
const XnChar* strName = it->Key();
XnActualPropertiesHash* pHash = it->Value();
// fix it first
if (strcmp(strName, XN_MODULE_NAME_DEVICE) == 0)
{
pHash->Remove(XN_MODULE_PROPERTY_READ_WRITE_MODE);
pHash->Remove(XN_MODULE_PROPERTY_PRIMARY_STREAM);
}
XnDeviceModule* pModule;
nRetVal = FindModule(strName, &pModule);
XN_IS_STATUS_OK(nRetVal);
nRetVal = pModule->UnsafeBatchConfig(*pHash);
XN_IS_STATUS_OK(nRetVal);
}
ResetLastTimestampAndFrame();
m_nReferenceTimestamp = 0;
m_nReferenceTime = 0;
m_bStreamsCollectionChanged = FALSE;
return (XN_STATUS_OK);
}
XnStatus XnDeviceFileReader::ReadInitialState(XnPropertySet *pSet)
{
XnStatus nRetVal = XN_STATUS_OK;
if (m_nFileVersion < 4)
{
if (m_pBCData == NULL)
{
nRetVal = BCInit();
XN_IS_STATUS_OK(nRetVal);
}
return BCReadInitialState(pSet);
}
// first read first object - modules properties - using base
nRetVal = XnStreamReaderDevice::ReadInitialState(pSet);
XN_IS_STATUS_OK(nRetVal);
// now continue reading until we get to first data
XnPackedDataType nType;
XnBool bStateEnd = FALSE;
XnUInt64 nPositionBefore;
while (!bStateEnd)
{
nRetVal = GetIOStream()->Tell(&nPositionBefore);
XN_IS_STATUS_OK(nRetVal);
nRetVal = GetDataPacker()->ReadNextObject(&nType);
XN_IS_STATUS_OK(nRetVal);
switch (nType)
{
case XN_PACKED_NEW_STREAM:
{
XnChar strType[XN_DEVICE_MAX_STRING_LENGTH];
XnChar strName[XN_DEVICE_MAX_STRING_LENGTH];
XN_PROPERTY_SET_CREATE_ON_STACK(props);
nRetVal = GetDataPacker()->ReadNewStream(strType, strName, &props);
XN_IS_STATUS_OK(nRetVal);
XnActualPropertiesHash* pStreamProps;
nRetVal = XnPropertySetDataDetachModule(props.pData, strName, &pStreamProps);
XN_IS_STATUS_OK(nRetVal);
nRetVal = XnPropertySetDataAttachModule(pSet->pData, strName, pStreamProps);
XN_IS_STATUS_OK(nRetVal);
break;
}
case XN_PACKED_INT_PROPERTY:
{
XnChar strModule[XN_DEVICE_MAX_STRING_LENGTH];
XnChar strProp[XN_DEVICE_MAX_STRING_LENGTH];
XnUInt64 nValue;
nRetVal = GetDataPacker()->ReadProperty(strModule, strProp, &nValue);
XN_IS_STATUS_OK(nRetVal);
XnActualPropertiesHash* pModule = NULL;
nRetVal = pSet->pData->Get(strModule, pModule);
XN_IS_STATUS_OK(nRetVal);
XnProperty* pProp = NULL;
nRetVal = pModule->Get(strProp, pProp);
XN_IS_STATUS_OK(nRetVal);
XnActualIntProperty* pIntProp = (XnActualIntProperty*)pProp;
nRetVal = pIntProp->UnsafeUpdateValue(nValue);
XN_IS_STATUS_OK(nRetVal);
break;
}
case XN_PACKED_REAL_PROPERTY:
{
XnChar strModule[XN_DEVICE_MAX_STRING_LENGTH];
XnChar strProp[XN_DEVICE_MAX_STRING_LENGTH];
XnDouble dValue;
nRetVal = GetDataPacker()->ReadProperty(strModule, strProp, &dValue);
XN_IS_STATUS_OK(nRetVal);
XnActualPropertiesHash* pModule;
nRetVal = pSet->pData->Get(strModule, pModule);
XN_IS_STATUS_OK(nRetVal);
XnProperty* pProp;
nRetVal = pModule->Get(strProp, pProp);
XN_IS_STATUS_OK(nRetVal);
XnActualRealProperty* pRealProp = (XnActualRealProperty*)pProp;
nRetVal = pRealProp->UnsafeUpdateValue(dValue);
XN_IS_STATUS_OK(nRetVal);
break;
}
case XN_PACKED_STRING_PROPERTY:
{
XnChar strModule[XN_DEVICE_MAX_STRING_LENGTH];
XnChar strProp[XN_DEVICE_MAX_STRING_LENGTH];
XnChar strValue[XN_DEVICE_MAX_STRING_LENGTH];
nRetVal = GetDataPacker()->ReadProperty(strModule, strProp, strValue);
XN_IS_STATUS_OK(nRetVal);
XnActualPropertiesHash* pModule;
nRetVal = pSet->pData->Get(strModule, pModule);
XN_IS_STATUS_OK(nRetVal);
XnProperty* pProp;
nRetVal = pModule->Get(strProp, pProp);
XN_IS_STATUS_OK(nRetVal);
XnActualStringProperty* pStringProp = (XnActualStringProperty*)pProp;
nRetVal = pStringProp->UnsafeUpdateValue(strValue);
XN_IS_STATUS_OK(nRetVal);
break;
}
case XN_PACKED_GENERAL_PROPERTY:
{
XnChar strModule[XN_DEVICE_MAX_STRING_LENGTH];
XnChar strProp[XN_DEVICE_MAX_STRING_LENGTH];
XnGeneralBuffer gbValue;
nRetVal = GetDataPacker()->ReadProperty(strModule, strProp, &gbValue);
XN_IS_STATUS_OK(nRetVal);
XnActualPropertiesHash* pModule;
nRetVal = pSet->pData->Get(strModule, pModule);
XN_IS_STATUS_OK(nRetVal);
XnProperty* pProp;
nRetVal = pModule->Get(strProp, pProp);
XN_IS_STATUS_OK(nRetVal);
XnActualGeneralProperty* pIntProp = (XnActualGeneralProperty*)pProp;
nRetVal = pIntProp->UnsafeUpdateValue(gbValue);
XN_IS_STATUS_OK(nRetVal);
break;
}
default:
// reached end of initial state. go back to beginning of this object
nRetVal = GetIOStream()->Seek(nPositionBefore);
XN_IS_STATUS_OK(nRetVal);
// stop reading
bStateEnd = TRUE;
}
} // objects loop
return (XN_STATUS_OK);
}