XnStatus XnDeviceBase::CloseAllStreams()
{
XnStatus nRetVal = XN_STATUS_OK;
xnLogVerbose(XN_MASK_DDK, "Closing all streams...");
// go over modules list, and look for closed streams
for (XnStringsHash::Iterator it = m_Modules.begin(); it != m_Modules.end(); ++it)
{
XnDeviceModuleHolder* pModuleHolder = (XnDeviceModuleHolder*)it.Value();
if (IsStream(pModuleHolder->GetModule()))
{
XnDeviceStream* pStream = (XnDeviceStream*)pModuleHolder->GetModule();
if (pStream->IsOpen())
{
nRetVal = pStream->Close();
XN_IS_STATUS_OK(nRetVal);
}
}
}
xnLogInfo(XN_MASK_DDK, "All streams are closed.");
return XN_STATUS_OK;
}
XnStatus XnDeviceBase::OpenAllStreams()
{
XnStatus nRetVal = XN_STATUS_OK;
xnLogVerbose(XN_MASK_DDK, "Opening all streams...");
// go over modules list, and look for closed streams
for (ModuleHoldersHash::Iterator it = m_Modules.Begin(); it != m_Modules.End(); ++it)
{
XnDeviceModuleHolder* pModuleHolder = it->Value();
if (IsStream(pModuleHolder->GetModule()))
{
XnDeviceStream* pStream = (XnDeviceStream*)pModuleHolder->GetModule();
if (!pStream->IsOpen())
{
nRetVal = pStream->Open();
XN_IS_STATUS_OK(nRetVal);
}
}
}
xnLogInfo(XN_MASK_DDK, "All streams are open.");
return XN_STATUS_OK;
}
XnStatus XnDeviceBase::WriteStream(XnStreamData* pStreamData)
{
XnStatus nRetVal = XN_STATUS_OK;
XN_VALIDATE_INPUT_PTR(pStreamData);
if (m_ReadWriteMode.GetValue() != XN_DEVICE_MODE_WRITE)
{
return XN_STATUS_IO_DEVICE_WRONG_MODE;
}
// take the stream to write
XnDeviceStream* pStream;
nRetVal = FindStream(pStreamData->StreamName, &pStream);
XN_IS_STATUS_OK(nRetVal);
// make sure it is open
if (!pStream->IsOpen())
{
return XN_STATUS_STREAM_NOT_OPEN;
}
// and write
nRetVal = pStream->Write(pStreamData);
XN_IS_STATUS_OK(nRetVal);
return (XN_STATUS_OK);
}
XnStatus XnDeviceBase::ReadStream(XnStreamData* pStreamOutput)
{
XnStatus nRetVal = XN_STATUS_OK;
XN_VALIDATE_INPUT_PTR(pStreamOutput);
if (m_ReadWriteMode.GetValue() == XN_DEVICE_MODE_WRITE)
{
return XN_STATUS_IO_DEVICE_WRONG_MODE;
}
// take the stream to read
XnDeviceStream* pStream;
nRetVal = FindStream(pStreamOutput->StreamName, &pStream);
XN_IS_STATUS_OK(nRetVal);
// make sure it is open
if (!pStream->IsNewDataAvailable() && !pStream->IsOpen())
{
return XN_STATUS_STREAM_NOT_OPEN;
}
// wait for it to advance
nRetVal = WaitForStream(m_hNewDataEvent, pStream);
XN_IS_STATUS_OK(nRetVal);
// and read
nRetVal = ReadFromStreamImpl(pStream, pStreamOutput);
XN_IS_STATUS_OK(nRetVal);
return (XN_STATUS_OK);
}
XnStatus XnSensor::GetBufferPool(const XnChar* strStream, XnBufferPool** ppBufferPool)
{
XnStatus nRetVal = XN_STATUS_OK;
XnDeviceModuleHolder* pHolder;
nRetVal = FindStream(strStream, &pHolder);
XN_IS_STATUS_OK(nRetVal);
XnSensorStreamHolder* pSensorStreamHolder = (XnSensorStreamHolder*)(pHolder);
XnDeviceStream* pStream = pSensorStreamHolder->GetStream();
XnUInt64 nFrameBased;
nRetVal = pStream->GetProperty(XN_STREAM_PROPERTY_IS_FRAME_BASED, &nFrameBased);
XN_IS_STATUS_OK(nRetVal);
if (nFrameBased == 0)
{
return XN_STATUS_BAD_TYPE;
}
XnFrameStream* pFrameStream = (XnFrameStream*)pStream;
*ppBufferPool = pFrameStream->GetBufferPool();
return (XN_STATUS_OK);
}
XnStatus XN_CALLBACK_TYPE XnDeviceStream::SetIsOpenCallback(XnActualIntProperty* /*pSender*/, XnUInt64 nValue, void* pCookie)
{
XnDeviceStream* pStream = (XnDeviceStream*)pCookie;
if (nValue == TRUE)
{
return pStream->Open();
}
else
{
return pStream->Close();
}
}
XnStatus XnDeviceBase::GetStreamRequiredDataSize(const XnChar* StreamName, XnUInt32* pnRequiredSize)
{
XnStatus nRetVal = XN_STATUS_OK;
// find stream
XnDeviceStream* pStream;
nRetVal = FindStream(StreamName, &pStream);
XN_IS_STATUS_OK(nRetVal);
*pnRequiredSize = pStream->GetRequiredDataSize();
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 XnDeviceBase::CreateStreamData(const XnChar* StreamName, XnStreamData** ppStreamData)
{
XnStatus nRetVal = XN_STATUS_OK;
XN_VALIDATE_INPUT_PTR(StreamName);
XN_VALIDATE_OUTPUT_PTR(ppStreamData);
// find stream
XnDeviceStream* pStream;
nRetVal = FindStream(StreamName, &pStream);
XN_IS_STATUS_OK(nRetVal);
// and create stream output
nRetVal = pStream->CreateStreamData(ppStreamData);
XN_IS_STATUS_OK(nRetVal);
return (XN_STATUS_OK);
}
OniStatus XnOniDevice::setProperty(int propertyId, const void* data, int dataSize)
{
switch (propertyId)
{
case ONI_DEVICE_PROPERTY_IMAGE_REGISTRATION:
{
if (dataSize == sizeof(OniImageRegistrationMode))
{
OniImageRegistrationMode* mode = (OniImageRegistrationMode*)data;
// Find the depth stream in the sensor.
XnDeviceStream* pDepth = NULL;
XnStatus xnrc = m_sensor.GetStream(XN_STREAM_NAME_DEPTH, &pDepth);
if (xnrc != XN_STATUS_OK)
{
return ONI_STATUS_BAD_PARAMETER;
}
// Set the mode in the depth stream.
XnUInt64 val = (*mode == ONI_IMAGE_REGISTRATION_DEPTH_TO_COLOR) ? 1 : 0;
xnrc = pDepth->SetProperty(XN_STREAM_PROPERTY_REGISTRATION, val);
if (xnrc != XN_STATUS_OK)
{
return ONI_STATUS_ERROR;
}
}
else
{
m_driverServices.errorLoggerAppend("Unexpected size: %d != %d\n", dataSize, sizeof(OniImageRegistrationMode));
return ONI_STATUS_ERROR;
}
}
break;
default:
XnStatus nRetVal = m_sensor.DeviceModule()->SetProperty(propertyId, data, dataSize);
if (nRetVal != XN_STATUS_OK)
{
return ONI_STATUS_BAD_PARAMETER;
}
}
return ONI_STATUS_OK;
}
XnStatus XnDeviceBase::CloseStream(const XnChar* StreamName)
{
XnStatus nRetVal = XN_STATUS_OK;
XN_VALIDATE_INPUT_PTR(StreamName);
xnLogVerbose(XN_MASK_DDK, "Closing stream %s...", StreamName);
// find this stream
XnDeviceStream* pStream;
nRetVal = FindStream(StreamName, &pStream);
XN_IS_STATUS_OK(nRetVal);
// close it
nRetVal = pStream->Close();
XN_IS_STATUS_OK(nRetVal);
xnLogInfo(XN_MASK_DDK, "Stream %s is closed.", StreamName);
return (XN_STATUS_OK);
}
XnBool XnSensor::HasSynchedFrameArrived(const XnChar* strDepthStream, const XnChar* strImageStream)
{
// find both streams
XnDeviceStream* pDepth;
XnDeviceStream* pImage;
if (XN_STATUS_OK != FindStream(strDepthStream, &pDepth))
return FALSE;
if (XN_STATUS_OK != FindStream(strImageStream, &pImage))
return FALSE;
XnUInt32 nThreshold = XN_SENSOR_FRAME_SYNC_MAX_DIFF;
if (IsHighResTimestamps())
nThreshold *= 1000;
// wait for both to advance, and time difference to be less than threshold
XnInt32 nTimestampDiff = XnInt32(pDepth->GetLastTimestamp() - pImage->GetLastTimestamp());
XnBool bConditionMet = (
pDepth->IsNewDataAvailable() &&
pImage->IsNewDataAvailable() &&
(XnUInt32)abs(nTimestampDiff) <= nThreshold
);
if (xnLogIsDumpMaskEnabled(XN_DUMP_FRAME_SYNC))
{
XnUInt64 nNow;
xnOSGetHighResTimeStamp(&nNow);
xnDumpWriteString(m_FrameSyncDump, "%llu,%u,%llu,%u,%llu,%s\n",
nNow,
pDepth->IsNewDataAvailable(),
pDepth->GetLastTimestamp(),
pImage->IsNewDataAvailable(),
pImage->GetLastTimestamp(),
bConditionMet ? "OK" : "Waiting");
}
return bConditionMet;
}
XnStatus XnDeviceBase::DestroyStream(const XnChar* StreamName)
{
XnStatus nRetVal = XN_STATUS_OK;
xnLogInfo(XN_MASK_DDK, "Destroying stream '%s'...", StreamName);
// keep the stream name (we now delete the module, so the name will be lost)
XnChar strStreamName[XN_DEVICE_MAX_STRING_LENGTH];
strncpy(strStreamName, StreamName, XN_DEVICE_MAX_STRING_LENGTH);
xnl::AutoCSLocker lock(m_hLock);
// Find the stream
XnDeviceModuleHolder* pStreamHolder;
nRetVal = FindStream(strStreamName, &pStreamHolder);
XN_IS_STATUS_OK(nRetVal);
XnDeviceStream* pStream = (XnDeviceStream*)pStreamHolder->GetModule();
XnUInt32 nRefCount = pStream->DecRef();
if (0 == nRefCount)
{
// remove it from map
nRetVal = RemoveModule(strStreamName);
XN_IS_STATUS_OK(nRetVal);
// and free it's memory
DestroyStreamModule(pStreamHolder);
// free memory of registered properties to this stream
FreeModuleRegisteredProperties(StreamName);
xnLogVerbose(XN_MASK_DDK, "'%s' stream destroyed.", strStreamName);
}
else
{
xnLogVerbose(XN_MASK_DDK, "'%s' stream now has %d references.", strStreamName, nRefCount);
}
return XN_STATUS_OK;
}
XnStatus XnDeviceBase::SetMirror(XnBool bMirror)
{
XnStatus nRetVal = XN_STATUS_OK;
// change all streams
for (XnStringsHash::Iterator it = m_Modules.begin(); it != m_Modules.end(); ++it)
{
XnDeviceModuleHolder* pModuleHolder = (XnDeviceModuleHolder*)it.Value();
if (IsStream(pModuleHolder->GetModule()))
{
XnDeviceStream* pStream = (XnDeviceStream*)pModuleHolder->GetModule();
nRetVal = pStream->SetMirror(bMirror);
XN_IS_STATUS_OK(nRetVal);
}
}
// and set property
nRetVal = m_DeviceMirror.UnsafeUpdateValue(bMirror);
XN_IS_STATUS_OK(nRetVal);
return (XN_STATUS_OK);
}
XnStatus XnDeviceBase::IsNewDataAvailable(const XnChar* StreamName, XnBool* pbNewDataAvailable, XnUInt64* pnTimestamp)
{
XnStatus nRetVal = XN_STATUS_OK;
XN_VALIDATE_INPUT_PTR(StreamName);
XN_VALIDATE_OUTPUT_PTR(pbNewDataAvailable);
*pbNewDataAvailable = FALSE;
if (strcmp(StreamName, XN_PRIMARY_STREAM_ANY) == 0)
{
const XnChar* aStreamNames[100];
XnUInt32 nCount = 100;
nRetVal = GetStreamNames(aStreamNames, &nCount);
XN_IS_STATUS_OK(nRetVal);
for (XnUInt32 i = 0; i < nCount; ++i)
{
// find stream
XnDeviceStream* pStream = NULL;
nRetVal = FindStream(StreamName, &pStream);
XN_IS_STATUS_OK(nRetVal);
if (pStream->IsNewDataAvailable())
{
*pbNewDataAvailable = TRUE;
*pnTimestamp = pStream->GetLastTimestamp();
break;
}
}
}
else
{
// find stream
XnDeviceStream* pStream = NULL;
nRetVal = FindStream(StreamName, &pStream);
XN_IS_STATUS_OK(nRetVal);
if (pStream->IsNewDataAvailable())
{
*pbNewDataAvailable = TRUE;
*pnTimestamp = pStream->GetLastTimestamp();
}
}
return (XN_STATUS_OK);
}
XnStatus XnServerSensorInvoker::OnStreamAdded(const XnChar* StreamName)
{
XnStatus nRetVal = XN_STATUS_OK;
// get all props
XN_PROPERTY_SET_CREATE_ON_STACK(props);
nRetVal = m_sensor.GetAllProperties(&props, FALSE, StreamName);
XN_IS_STATUS_OK(nRetVal);
// register to all props
nRetVal = RegisterToProps(&props);
XN_IS_STATUS_OK(nRetVal);
XnActualPropertiesHash* pStreamProps = props.pData->Begin()->Value();
// create stream data
SensorInvokerStream serverStream;
xnOSMemSet(&serverStream, 0, sizeof(serverStream));
strcpy(serverStream.strType, StreamName);
XN_VALIDATE_NEW(serverStream.pNewDataEvent, NewStreamDataEvent);
// check if this is a frame stream
XnProperty* pIsFrameBased;
nRetVal = pStreamProps->Get(XN_STREAM_PROPERTY_IS_FRAME_BASED, pIsFrameBased);
if (nRetVal == XN_STATUS_OK)
{
XnActualIntProperty* pIntProp = (XnActualIntProperty*)pIsFrameBased;
serverStream.bFrameStream = (pIntProp->GetValue() == TRUE);
}
if (serverStream.bFrameStream)
{
// create the "shared memory name" property
XN_VALIDATE_NEW(serverStream.pSharedMemoryName, XnActualStringProperty, XN_STREAM_PROPERTY_SHARED_BUFFER_NAME);
// and add it to the stream
XnDeviceStream* pStream;
nRetVal = m_sensor.GetStream(StreamName, &pStream);
if (nRetVal != XN_STATUS_OK)
{
XN_DELETE(serverStream.pNewDataEvent);
XN_DELETE(serverStream.pSharedMemoryName);
return nRetVal;
}
nRetVal = pStream->AddProperty(serverStream.pSharedMemoryName);
if (nRetVal != XN_STATUS_OK)
{
XN_DELETE(serverStream.pNewDataEvent);
XN_DELETE(serverStream.pSharedMemoryName);
return nRetVal;
}
// create a shared memory buffer pool for it
nRetVal = SetStreamSharedMemory(&serverStream);
if (nRetVal != XN_STATUS_OK)
{
XN_DELETE(serverStream.pNewDataEvent);
XN_DELETE(serverStream.pSharedMemoryName);
return nRetVal;
}
}
// create a stream data object for the stream
nRetVal = m_sensor.CreateStreamData(StreamName, &serverStream.pStreamData);
if (nRetVal != XN_STATUS_OK)
{
XN_DELETE(serverStream.pNewDataEvent);
XN_DELETE(serverStream.pSharedMemoryName);
return (nRetVal);
}
// and add it to our list of streams
nRetVal = m_streams.Set(StreamName, serverStream);
if (nRetVal != XN_STATUS_OK)
{
XN_DELETE(serverStream.pNewDataEvent);
XN_DELETE(serverStream.pSharedMemoryName);
return (nRetVal);
}
return (XN_STATUS_OK);
}
OniStatus XnOniDevice::getProperty(int propertyId, void* data, int* pDataSize)
{
switch (propertyId)
{
case ONI_DEVICE_PROPERTY_DRIVER_VERSION:
{
if (*pDataSize == sizeof(OniVersion))
{
OniVersion* version = (OniVersion*)data;
version->major = XN_PS_MAJOR_VERSION;
version->minor = XN_PS_MINOR_VERSION;
version->maintenance = XN_PS_MAINTENANCE_VERSION;
version->build = XN_PS_BUILD_VERSION;
}
else
{
m_driverServices.errorLoggerAppend("Unexpected size: %d != %d\n", *pDataSize, sizeof(OniVersion));
return ONI_STATUS_ERROR;
}
}
break;
case ONI_DEVICE_PROPERTY_IMAGE_REGISTRATION:
{
if (*pDataSize == sizeof(OniImageRegistrationMode))
{
OniImageRegistrationMode* mode = (OniImageRegistrationMode*)data;
// Find the depth stream in the sensor.
XnDeviceStream* pDepth = NULL;
XnStatus xnrc = m_sensor.GetStream(XN_STREAM_NAME_DEPTH, &pDepth);
if (xnrc != XN_STATUS_OK)
{
return ONI_STATUS_BAD_PARAMETER;
}
// Set the mode in the depth stream.
XnUInt64 val;
xnrc = pDepth->GetProperty(XN_STREAM_PROPERTY_REGISTRATION, &val);
if (xnrc != XN_STATUS_OK)
{
return ONI_STATUS_ERROR;
}
// Update the return value.
*mode = (val == 1) ? ONI_IMAGE_REGISTRATION_DEPTH_TO_COLOR : ONI_IMAGE_REGISTRATION_OFF;
}
else
{
m_driverServices.errorLoggerAppend("Unexpected size: %d != %d\n", *pDataSize, sizeof(OniImageRegistrationMode));
return ONI_STATUS_ERROR;
}
}
break;
default:
XnStatus nRetVal = m_sensor.DeviceModule()->GetProperty(propertyId, data, pDataSize);
if (nRetVal != XN_STATUS_OK)
{
return ONI_STATUS_BAD_PARAMETER;
}
}
return ONI_STATUS_OK;
}
XnStatus XN_CALLBACK_TYPE XnDeviceStream::UpdateRequiredSizeCallback(const XnProperty* /*pSenser*/, void* pCookie)
{
XnDeviceStream* pStream = (XnDeviceStream*)pCookie;
return pStream->UpdateRequiredSize();
}
XnStatus XN_CALLBACK_TYPE XnDeviceStream::SetOutputFormatCallback(XnActualIntProperty* /*pSender*/, XnUInt64 nValue, void* pCookie)
{
XnDeviceStream* pStream = (XnDeviceStream*)pCookie;
return pStream->SetOutputFormat((XnOutputFormats)nValue);
}
XnStatus XN_CALLBACK_TYPE XnDeviceStream::SetIsMirrorCallback(XnActualIntProperty* /*pSender*/, XnUInt64 nValue, void* pCookie)
{
XnDeviceStream* pStream = (XnDeviceStream*)pCookie;
return pStream->SetMirror((XnBool)nValue);
}
XnStatus XnDeviceBase::CreateStreamImpl(const XnChar* strType, const XnChar* strName, const XnActualPropertiesHash* pInitialSet)
{
XnStatus nRetVal = XN_STATUS_OK;
xnLogInfo(XN_MASK_DDK, "Creating stream '%s' of type '%s'...", strName, strType);
XnDeviceModule* pModule;
if (FindModule(strName, &pModule) == XN_STATUS_OK)
{
// already exists. check sharing mode (when shared, we allow "creating" the same stream)
if (GetSharingMode() != XN_DEVICE_SHARED ||
!IsStream(pModule) ||
strcmp(strType, ((XnDeviceStream*)pModule)->GetType()) != 0)
{
XN_LOG_WARNING_RETURN(XN_STATUS_STREAM_ALREADY_EXISTS, XN_MASK_DDK, "A stream with this name already exists!");
}
// OK, we'll allow this. Just set new configuration
if (pInitialSet != NULL)
{
nRetVal = pModule->BatchConfig(*pInitialSet);
XN_IS_STATUS_OK(nRetVal);
}
}
else
{
// create stream
XnDeviceModuleHolder* pNewStreamHolder = NULL;
nRetVal = CreateStreamModule(strType, strName, &pNewStreamHolder);
XN_IS_STATUS_OK(nRetVal);
XnDeviceStream* pNewStream = (XnDeviceStream*)(pNewStreamHolder->GetModule());
if (pNewStream == NULL)
{
DestroyStreamModule(pNewStreamHolder);
XN_LOG_ERROR_RETURN(XN_STATUS_ERROR, XN_MASK_DDK, "Internal Error: Invalid new stream!");
}
// initialize the stream
xnLogVerbose(XN_MASK_DDK, "Initializing stream '%s'...", strName);
nRetVal = pNewStreamHolder->Init(pInitialSet);
if (nRetVal != XN_STATUS_OK)
{
DestroyStreamModule(pNewStreamHolder);
return (nRetVal);
}
// set it's mirror value (if not requested otherwise)
XnBool bSetMirror = TRUE;
if (pInitialSet != NULL)
{
XnActualPropertiesHash::ConstIterator it = pInitialSet->end();
if (XN_STATUS_OK == pInitialSet->Find(XN_MODULE_PROPERTY_MIRROR, it))
{
bSetMirror = FALSE;
}
}
if (bSetMirror)
{
nRetVal = pNewStream->SetMirror((XnBool)m_DeviceMirror.GetValue());
if (nRetVal != XN_STATUS_OK)
{
DestroyStreamModule(pNewStreamHolder);
return (nRetVal);
}
}
// add it to the list of existing modules
nRetVal = AddModule(pNewStreamHolder);
if (nRetVal != XN_STATUS_OK)
{
DestroyStreamModule(pNewStreamHolder);
return (nRetVal);
}
xnLogInfo(XN_MASK_DDK, "Stream '%s' was initialized.", strName);
nRetVal = StreamAdded(pNewStream);
XN_IS_STATUS_OK(nRetVal);
xnLogInfo(XN_MASK_DDK, "'%s' stream was created.", strName);
}
return (XN_STATUS_OK);
}
