OniStatus XnSensorDepthStream::GetSensorCalibrationInfo(void* data, int* pDataSize)
{
if ((size_t)*pDataSize < sizeof(DepthUtilsSensorCalibrationInfo))
{
return ONI_STATUS_BAD_PARAMETER;
}
if (m_depthUtilsHandle == NULL)
{
// Depth utils not initialized - probably not supported for this device
return ONI_STATUS_NOT_SUPPORTED;
}
*pDataSize = sizeof(DepthUtilsSensorCalibrationInfo);
xnOSMemCopy(data, &m_calibrationInfo, sizeof(DepthUtilsSensorCalibrationInfo));
return ONI_STATUS_OK;
}
OniStatus Context::getDeviceList(OniDeviceInfo** pDevices, int* pDeviceCount)
{
m_cs.Lock();
*pDeviceCount = m_devices.Size();
*pDevices = XN_NEW_ARR(OniDeviceInfo, *pDeviceCount);
int idx = 0;
for (xnl::List<Device*>::ConstIterator iter = m_devices.Begin(); iter != m_devices.End(); ++iter, ++idx)
{
xnOSMemCopy((*pDevices)+idx, (*iter)->getInfo(), sizeof(OniDeviceInfo));
}
m_cs.Unlock();
return ONI_STATUS_OK;
}
XnStatus XnSensor::GetFixedParams(XnDynamicSizeBuffer* pBuffer)
{
XnStatus nRetVal = XN_STATUS_OK;
if (pBuffer->nMaxSize < sizeof(XnFixedParams))
{
return (XN_STATUS_OUTPUT_BUFFER_OVERFLOW);
}
XnFixedParams fixed;
nRetVal = XnHostProtocolGetFixedParams(GetDevicePrivateData(), fixed);
XN_IS_STATUS_OK(nRetVal);
xnOSMemCopy(pBuffer->pData, &fixed, sizeof(XnFixedParams));
pBuffer->nDataSize = sizeof(XnFixedParams);
return (XN_STATUS_OK);
}
ONI_NAMESPACE_IMPLEMENTATION_BEGIN
Device::Device(DeviceDriver* pDeviceDriver, const DriverHandler& driverHandler, FrameManager& frameManager, const OniDeviceInfo* pDeviceInfo, xnl::ErrorLogger& errorLogger) :
m_driverHandler(driverHandler),
m_frameManager(frameManager),
m_errorLogger(errorLogger),
m_active(false),
m_openCount(0),
m_deviceHandle(NULL),
m_pDeviceDriver(pDeviceDriver),
m_depthColorSyncHandle(NULL),
m_pContext(NULL),
m_syncEnabled(FALSE)
{
m_pInfo = XN_NEW(OniDeviceInfo);
xnOSMemCopy(m_pInfo, pDeviceInfo, sizeof(OniDeviceInfo));
xnOSMemSet(&m_sensors, 0, sizeof(m_sensors));
}
XnStatus LinkResponseMsgParser::ParsePacketImpl(XnLinkFragmentation /*fragmentation*/,
const XnUInt8* pSrc,
const XnUInt8* pSrcEnd,
XnUInt8*& pDst,
const XnUInt8* pDstEnd)
{
XnStatus nRetVal = XN_STATUS_OK;
XnSizeT nPacketDataSize = pSrcEnd - pSrc;
if (nPacketDataSize < sizeof(XnLinkResponseInfo))
{
XN_ASSERT(FALSE);
return XN_STATUS_LINK_MISSING_RESPONSE_INFO;
}
XnUInt16 nResponseCode = XN_PREPARE_VAR16_IN_BUFFER(((XnLinkResponseInfo*)pSrc)->m_nResponseCode);
nPacketDataSize -= sizeof(XnLinkResponseInfo);
pSrc += sizeof(XnLinkResponseInfo);
if (pDst + nPacketDataSize > pDstEnd)
{
XN_ASSERT(FALSE);
return XN_STATUS_OUTPUT_BUFFER_OVERFLOW;
}
////////////////////////////////////////////
xnOSMemCopy(pDst, pSrc, nPacketDataSize);
////////////////////////////////////////////
nRetVal = xnLinkResponseCodeToStatus(nResponseCode);
if (nRetVal != XN_STATUS_OK)
{
xnLogWarning(XN_MASK_LINK, "Received error from link layer response: '%s' (%u)",
xnGetStatusString(nRetVal), nResponseCode);
xnLogWriteBinaryData(XN_MASK_LINK, XN_LOG_WARNING,
__FILE__, __LINE__, (XnUChar*)pSrc, (XnUInt32)nPacketDataSize, "Response extra data: ");
XN_ASSERT(FALSE);
return nRetVal;
}
pDst += nPacketDataSize;
return XN_STATUS_OK;
}
OniStatus PlayerDevice::getSensorInfoList(OniSensorInfo** pSources, int* numSources)
{
Lock();
// Update source count.
*numSources = (int)m_sources.Size();
*pSources = XN_NEW_ARR(OniSensorInfo, *numSources);
// Copy sources.
SourceList::Iterator iter = m_sources.Begin();
for (int i = 0; i < *numSources; ++i, ++iter)
{
xnOSMemCopy(&(*pSources)[i], (*iter)->GetInfo(), sizeof(OniSensorInfo));
}
Unlock();
return ONI_STATUS_OK;
}
OniStatus Recorder::recordStreamProperty(
VideoStream& stream,
int propertyId,
const void* pData,
int dataSize)
{
xnl::LockGuard< AttachedStreams > guard(m_streams);
VideoStream* pStream = &stream;
if (m_streams.Find(pStream) == m_streams.End())
{
return ONI_STATUS_BAD_PARAMETER;
}
// The original pData will not be valid after this function ends.
// Free this pointer when soon after handling the PropertyMessage!
void *newPtr = xnOSMalloc(dataSize);
xnOSMemCopy(newPtr, pData, dataSize);
send(Message::MESSAGE_RECORDPROPERTY, pStream, newPtr, propertyId, dataSize, m_propertyPriority);
return ONI_STATUS_OK;
}
XnStatus XnPixelStream::CropImpl(OniFrame* pFrame, const OniCropping* pCropping)
{
XnUChar* pPixelData = (XnUChar*)pFrame->data;
XnUInt32 nCurDataSize = 0;
for (XnUInt32 y = pCropping->originY; y < XnUInt32(pCropping->originY + pCropping->height); ++y)
{
XnUChar* pOrigLine = &pPixelData[y * GetXRes() * GetBytesPerPixel()];
// move line
xnOSMemCopy(pPixelData + nCurDataSize, pOrigLine + pCropping->originX * GetBytesPerPixel(), pCropping->width * GetBytesPerPixel());
nCurDataSize += pCropping->width * GetBytesPerPixel();
}
// update size
pFrame->dataSize = nCurDataSize;
return XN_STATUS_OK;
}
XnStatus XnStreamReaderStream::ReadImpl(XnStreamData* pStreamData)
{
pStreamData->nFrameID = m_pLastData->nFrameID;
pStreamData->nTimestamp = m_pLastData->nTimestamp;
if (pStreamData->pInternal->bAllocated)
{
// don't take more than required size
pStreamData->nDataSize = XN_MIN(m_pLastData->nDataSize, GetRequiredDataSize());
xnOSMemCopy(pStreamData->pData, m_pLastData->pData, pStreamData->nDataSize);
}
else
{
pStreamData->nDataSize = m_pLastData->nDataSize;
pStreamData->pData = m_pLastData->pData;
}
return (XN_STATUS_OK);
}
XnStatus MockDepthGenerator::SetGeneralProperty(const XnChar* strName, XnUInt32 nBufferSize, const void* pBuffer)
{
XN_VALIDATE_INPUT_PTR(strName);
XN_VALIDATE_INPUT_PTR(pBuffer);
XnStatus nRetVal = XN_STATUS_OK;
if (strcmp(strName, XN_PROP_FIELD_OF_VIEW) == 0)
{
if (nBufferSize != sizeof(XnFieldOfView))
{
XN_LOG_ERROR_RETURN(XN_STATUS_INVALID_BUFFER_SIZE, XN_MASK_OPEN_NI, "Cannot set XN_PROP_FIELD_OF_VIEW - buffer size is incorrect");
}
const XnFieldOfView* pFOV = (const XnFieldOfView*)pBuffer;
nRetVal = SetFieldOfView(*pFOV);
XN_IS_STATUS_OK(nRetVal);
}
else if (strcmp(strName, XN_PROP_USER_POSITIONS) == 0)
{
if (m_bSupportedUserPositionsCountReceived)
{
m_bSupportedUserPositionsCountReceived = FALSE;
if (nBufferSize != m_nSupportedUserPositionsCount * sizeof(XnBoundingBox3D))
{
XN_LOG_ERROR_RETURN(XN_STATUS_INVALID_BUFFER_SIZE, XN_MASK_OPEN_NI, "Cannot set XN_PROP_USER_POSITIONS - buffer size is incorrect");
}
XN_DELETE_ARR(m_pUserPositions);
m_pUserPositions = XN_NEW_ARR(XnBoundingBox3D, m_nSupportedUserPositionsCount);
XN_VALIDATE_ALLOC_PTR(m_pUserPositions);
xnOSMemCopy(m_pUserPositions, pBuffer, nBufferSize);
}
else
{
/*We got XN_PROP_USER_POSITIONS without
XN_PROP_SUPPORTED_USER_POSITIONS_COUNT before it - that's an error*/
XN_ASSERT(FALSE);
XN_LOG_ERROR_RETURN(XN_STATUS_ERROR, XN_MASK_OPEN_NI, "got XN_PROP_USER_POSITIONS without XN_PROP_SUPPORTED_USER_POSITIONS_COUNT before it.")
}
}
else
{
XnStatus LinkContInputStream::UpdateData()
{
xnl::AutoCSLocker csLock(m_hCriticalSection);
if (!m_bInitialized)
{
xnLogError(XN_MASK_INPUT_STREAM, "Attempted to update data from stream %u which is not initialized", m_nStreamID);
XN_ASSERT(FALSE);
return XN_STATUS_NOT_INIT;
}
if (m_bNewDataAvailable)
{
//Copy working buffer to user buffer
xnOSMemCopy(m_pUserBuffer, m_pWorkingBuffer, m_nUserBufferMaxSize);
m_nUserBufferCurrentSize = m_nWorkingBufferCurrentSize;
m_bNewDataAvailable = FALSE;
}
return XN_STATUS_OK;
}
XnStatus XnPixelStream::CropImpl(XnStreamData* pStreamOutput, const XnCropping* pCropping)
{
XnStatus nRetVal = XN_STATUS_OK;
XnUChar* pPixelData = (XnUChar*)pStreamOutput->pData;
XnUInt32 nCurDataSize = 0;
for (XnUInt32 y = pCropping->nYOffset; y < XnUInt32(pCropping->nYOffset + pCropping->nYSize); ++y)
{
XnUChar* pOrigLine = &pPixelData[y * GetXRes() * GetBytesPerPixel()];
// move line
xnOSMemCopy(pPixelData + nCurDataSize, pOrigLine + pCropping->nXOffset * GetBytesPerPixel(), pCropping->nXSize * GetBytesPerPixel());
nCurDataSize += pCropping->nXSize * GetBytesPerPixel();
}
// update size
pStreamOutput->nDataSize = nCurDataSize;
return XN_STATUS_OK;
}
void DeviceManager::EnumerateDevices()
{
XnStatus nRetVal = XN_STATUS_OK;
xn::NodeInfoList deviceList;
nRetVal = context_->EnumerateProductionTrees(XN_NODE_TYPE_DEVICE, NULL, deviceList, NULL);
CHECK_RV(nRetVal, "Enumerate");
for(xn::NodeInfoList::Iterator iter = deviceList.Begin(); iter != deviceList.End(); ++iter)
{
xn::NodeInfo node = (*iter);
SensorDevice* sensor = new SensorDevice();
char deviceName[256];
//Create Device Context in the global production tree
nRetVal = context_->CreateProductionTree(node);
CHECK_RV(nRetVal, "Create Device");
Query query;
query.AddNeededNode(node.GetInstanceName());
xnOSMemCopy(deviceName, node.GetInstanceName(), xnOSStrLen(node.GetInstanceName()));
sensor->SetDeviceName(deviceName);
//Create Image and Depth generators for this device
nRetVal = context_->CreateAnyProductionTree(XN_NODE_TYPE_IMAGE, &query, *(sensor->GetImageGenerator()));
CHECK_RV(nRetVal, "Create Image Generator");
nRetVal = context_->CreateAnyProductionTree(XN_NODE_TYPE_DEPTH, &query, *(sensor->GetDepthGenerator()));
CHECK_RV(nRetVal, "Create Depth Generator");
//Align the RGB and Depth Cameras
sensor->AlignSensors();
devices_.push_back(sensor);
}
context_->StartGeneratingAll();
}
XnStatus MockGenerator::SetNextData(const void *pData, XnUInt32 nSize)
{
XnStatus nRetVal = XN_STATUS_OK;
DataInfo& nextData = m_data[m_nNextDataIdx];
if (!m_bAggregateData)
{
nextData.nDataSize = 0;
}
nRetVal = ResizeBuffer(m_nNextDataIdx, nextData.nDataSize + nSize);
XN_IS_STATUS_OK(nRetVal);
xnOSMemCopy((XnUChar*)nextData.pData + nextData.nDataSize, pData, nSize);
nextData.nDataSize += nSize;
nRetVal = SetNewDataAvailable();
XN_IS_STATUS_OK(nRetVal);
return XN_STATUS_OK;
}
void Mainloop()
{
int frameId = 1;
while (m_running)
{
m_osEvent.Wait(XN_WAIT_INFINITE);
m_osEvent.Reset();
OniFrame* pFrame = getServices().acquireFrame();
if (pFrame == NULL) {printf("Didn't get frame...\n"); continue;}
// Fill frame
xnOSMemSet(pFrame->data, 0, pFrame->dataSize);
pFrame->frameIndex = frameId;
pFrame->videoMode.pixelFormat = ONI_PIXEL_FORMAT_RGB888;
pFrame->videoMode.resolutionX = DEPTHSENSE_COLOR_RESOLUTION_X;
pFrame->videoMode.resolutionY = DEPTHSENSE_COLOR_RESOLUTION_Y;
pFrame->videoMode.fps = 30;
pFrame->width = DEPTHSENSE_COLOR_RESOLUTION_X;
pFrame->height = DEPTHSENSE_COLOR_RESOLUTION_Y;
xnOSMemCopy( pFrame->data, &m_data[0], m_data.size() );
pFrame->cropOriginX = pFrame->cropOriginY = 0;
pFrame->croppingEnabled = FALSE;
pFrame->sensorType = ONI_SENSOR_COLOR;
pFrame->stride = DEPTHSENSE_COLOR_RESOLUTION_X*sizeof(OniDepthPixel);
pFrame->timestamp = frameId * 33000;
raiseNewFrame(pFrame);
getServices().releaseFrame(pFrame);
frameId++;
}
}
XnStatus XnServerSession::SendReply(XnSensorServerCustomMessages Type, XnStatus nRC, XnUInt32 nDataSize /* = 0 */, void* pAdditionalData /* = NULL */)
{
XnStatus nRetVal = XN_STATUS_OK;
XnUChar message[XN_SENSOR_SERVER_MAX_REPLY_SIZE];
XnSensorClientServerReply* pReply = (XnSensorClientServerReply*)message;
pReply->nRetVal = nRC;
pReply->Type = Type;
pReply->nDataSize = nDataSize;
xnOSMemCopy(pReply->pData, pAdditionalData, nDataSize);
XnUChar* pEnd = pReply->pData + nDataSize;
m_pLogger->DumpMessage("Reply", nDataSize, m_nID);
// lock this so that messages don't mix up
{
XnAutoCSLocker lock(m_hCommLock);
nRetVal = m_privateOutgoingPacker.WriteCustomData(Type, message, (XnUInt32)(pEnd - message));
}
XN_IS_STATUS_OK(nRetVal);
return (XN_STATUS_OK);
}
XnStatus XnMirrorYUYVPixels(XnUChar* pBuffer, XnUInt32 nBufferSize, XnUInt32 nLineSize)
{
// Local function variables
XnUInt8* pSrc = pBuffer;
XnUInt8 pLineBuffer[XN_MIRROR_MAX_LINE_SIZE];
XnUInt8* pSrcEnd = (XnUInt8*)pSrc + nBufferSize;
XnUInt8* pDest = NULL;
XnUInt8* pDestVal = &pLineBuffer[(nLineSize/2-1)*sizeof(XnUInt32)]; // last element
XnUInt8* pDestEnd = &pLineBuffer[0]; // first element
XnUInt32 nMemCpyLineSize = nLineSize/2*sizeof(XnUInt32);
if (nMemCpyLineSize > XN_MIRROR_MAX_LINE_SIZE)
{
return (XN_STATUS_INTERNAL_BUFFER_TOO_SMALL);
}
while (pSrc < pSrcEnd)
{
xnOSMemCopy(pLineBuffer, pSrc, nMemCpyLineSize);
pDest = pDestVal;
while (pDest >= pDestEnd)
{
pSrc[0] = pDest[2]; // y1 <-> y2
pSrc[1] = pDest[3]; // u
pSrc[2] = pDest[0]; // y2 <-> y1
pSrc[3] = pDest[1]; // v
pSrc += 4;
pDest -= 4;
}
}
// All is good...
return (XN_STATUS_OK);
}
int main (int argc, char * const argv[]) {
xn::Context context;
IplImage *grayImage = NULL;
try {
// OpenNIコンテキストの作成
xn::EnumerationErrors ctxErrors;
XnStatus rc = context.InitFromXmlFile(CONFIG_XML_PATH, &ctxErrors);
if (rc != XN_STATUS_OK) {
XnChar strError[1024];
ctxErrors.ToString(strError, 1024);
std::cout << strError << std::endl;
return 1;
}
// DepthGeneratorの作成
xn::DepthGenerator depth;
rc = context.FindExistingNode(XN_NODE_TYPE_DEPTH, depth);
if (rc != XN_STATUS_OK) {
throw std::runtime_error(xnGetStatusString(rc));
}
XnMapOutputMode mapMode;
depth.GetMapOutputMode(mapMode);
int width = mapMode.nXRes;
int height = mapMode.nYRes;
int numPixels = width * height;
// グレースケール用ピクセルデータ
unsigned char * grayPixels = new unsigned char[numPixels];
xnOSMemSet(grayPixels, 0, numPixels * sizeof(unsigned char));
// 最大深度
const unsigned short int maxDepth = 8000;
// 8ビット1チャンネルの画像データの作成
grayImage = cvCreateImage(cvSize(width, height), IPL_DEPTH_8U, 1);
while(1) {
// ノードの更新を待つ
context.WaitAndUpdateAll();
// 深度の生データの取得
xn::DepthMetaData depthMetaData;
depth.GetMetaData(depthMetaData);
const XnDepthPixel* depthPixels = depthMetaData.Data();
// グレースケールに変換
for (int i=0; i<numPixels; i++) {
grayPixels[i] = (int)(min(maxDepth, depthPixels[i])/(maxDepth/255));
}
// メモリコピー
xnOSMemCopy(grayImage->imageData, grayPixels, numPixels);
// 表示
cvShowImage("Depth View", grayImage);
// ESCもしくはqが押されたら終了させる
char key = cvWaitKey(10);
if (key == 27 || key == 'q') {
break;
}
}
} catch (std::exception& ex) {
std::cout << ex.what() << std::endl;
}
if (grayImage != NULL) {
cvReleaseImage(&grayImage);
}
context.Shutdown();
return 0;
}
XN_C_API XnStatus xnOSCreateSocket(const XnOSSocketType SocketType, const XnChar* cpIPAddress, const XnUInt16 nPort, XN_SOCKET_HANDLE* SocketPtr)
{
// Local function variables
hostent* HostEnt = NULL;
XN_SOCKET_HANDLE Socket = NULL;
// Validate the input/output pointers (to make sure none of them is NULL)
XN_VALIDATE_INPUT_PTR(cpIPAddress);
XN_VALIDATE_OUTPUT_PTR(SocketPtr);
XN_VALIDATE_ALIGNED_CALLOC(*SocketPtr, xnOSSocket, 1, XN_DEFAULT_MEM_ALIGN);
Socket = *SocketPtr;
if (SocketType == XN_OS_UDP_SOCKET)
{
// Create a UDP socket
Socket->Socket = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
}
else if (SocketType == XN_OS_TCP_SOCKET)
{
// Create a TCP socket
Socket->Socket = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
}
else
{
// Unknown socket type...
XN_ALIGNED_FREE_AND_NULL(Socket);
return (XN_STATUS_OS_NETWORK_INVALID_SOCKET_TYPE);
}
if (Socket->Socket == INVALID_SOCKET)
{
XN_ALIGNED_FREE_AND_NULL(Socket);
XN_LOG_ERROR_RETURN(XN_STATUS_OS_NETWORK_SOCKET_CREATION_FAILED, XN_MASK_OS, "socket() returned WinSock error: %d", WSAGetLastError());
}
// Set the socket server address
Socket->SocketAddress.sin_family = AF_INET;
if (isalpha(cpIPAddress[0]))
{
HostEnt = gethostbyname(cpIPAddress);
if (HostEnt == NULL)
{
XN_ALIGNED_FREE_AND_NULL(Socket);
return (XN_STATUS_OS_NETWORK_BAD_HOST_NAME);
}
xnOSMemCopy(&Socket->SocketAddress.sin_addr, HostEnt->h_addr, HostEnt->h_length);
}
else
{
Socket->SocketAddress.sin_addr.s_addr = inet_addr(cpIPAddress);
}
Socket->SocketAddress.sin_port = htons(nPort);
// Update socket address size
Socket->nSocketAddressLen = sizeof(Socket->SocketAddress);
// Remember the socket type
Socket->nSocketType = SocketType;
// All is good...
return (XN_STATUS_OK);
}
XN_C_API XnStatus xnOSConnectSocket(XN_SOCKET_HANDLE Socket, XnUInt32 nMillisecsTimeout)
{
// Local function variables
XnInt32 nRetVal = 0;
sockaddr SocketAddress;
fd_set fdWriteHandles;
fd_set fdExceptHandles;
struct timeval selectTimeOut;
struct timeval* pTimeout = xnOSMillisecsToTimeVal(nMillisecsTimeout, &selectTimeOut);
// Validate the input/output pointers (to make sure none of them is NULL)
XN_VALIDATE_INPUT_PTR(Socket);
// Make sure the actual socket handle isn't NULL
XN_RET_IF_NULL(Socket->Socket, XN_STATUS_OS_INVALID_SOCKET);
// Connect to the socket and make sure it succeeded
xnOSMemCopy(&SocketAddress, &Socket->SocketAddress, sizeof(SocketAddress));
// if timeout is XN_SOCKET_DEFAULT_TIMEOUT, leave the socket as a blocking one
if (nMillisecsTimeout != XN_SOCKET_DEFAULT_TIMEOUT)
{
// Make the socket non-blocking temporarily
u_long nNonBlockingSocket = 1;
if (ioctlsocket(Socket->Socket, FIONBIO, &nNonBlockingSocket) != 0)
{
XN_LOG_ERROR_RETURN(XN_STATUS_OS_NETWORK_SOCKET_CONNECT_FAILED, XN_MASK_OS, "ioctlsocket() failed with error %d", WSAGetLastError());
}
}
nRetVal = connect(Socket->Socket, &SocketAddress, sizeof(SocketAddress));
if ((nRetVal == SOCKET_ERROR) && (WSAGetLastError() != WSAEWOULDBLOCK))
{
xnLogError(XN_MASK_OS, "connect() failed with winsock error %d", WSAGetLastError());
return(XN_STATUS_OS_NETWORK_SOCKET_CONNECT_FAILED);
}
if (nMillisecsTimeout != XN_SOCKET_DEFAULT_TIMEOUT)
{
XN_PRAGMA_START_DISABLED_WARNING_SECTION(XN_CONDITION_IS_CONST_WARNING_ID);
FD_ZERO(&fdWriteHandles);
FD_SET(Socket->Socket, &fdWriteHandles);
FD_ZERO(&fdExceptHandles);
FD_SET(Socket->Socket, &fdExceptHandles);
XN_PRAGMA_STOP_DISABLED_WARNING_SECTION;
nRetVal = select(1 /* ignored */, NULL, &fdWriteHandles, &fdExceptHandles, pTimeout);
// in any case, make the socket blocking again before we check select()'s success
u_long nBlockingSocket = 0;
ioctlsocket(Socket->Socket, FIONBIO, &nBlockingSocket);
if (nRetVal == 0)
{
return (XN_STATUS_OS_NETWORK_TIMEOUT);
}
else if (nRetVal == SOCKET_ERROR)
{
XN_LOG_ERROR_RETURN(XN_STATUS_OS_NETWORK_SOCKET_CONNECT_FAILED, XN_MASK_OS, "select() returned WinSock error: %d", WSAGetLastError());
}
else
{
// select returned due to socket state change. Check if an error occurred or everything is OK.
if (FD_ISSET(Socket->Socket, &fdExceptHandles))
{
XnUInt32 nLastError = 0;
XnInt32 nLastErrorSize = sizeof(nLastError);
getsockopt(Socket->Socket, SOL_SOCKET, SO_ERROR, (char*)&nLastError, &nLastErrorSize);
XN_LOG_ERROR_RETURN(XN_STATUS_OS_NETWORK_SOCKET_CONNECT_FAILED, XN_MASK_OS, "Connect failed with error: %u", nLastError);
}
// else, it means it's in the writable state, which means connect succeeded.
XN_ASSERT(FD_ISSET(Socket->Socket, &fdWriteHandles));
}
}
// All is good...
return (XN_STATUS_OK);
}
XnStatus XnDeviceModuleHolder::CreateProperty(XnProperty* pRequestProp)
{
XnStatus nRetVal = XN_STATUS_OK;
XnProperty* pNewProp = NULL;
switch (pRequestProp->GetType())
{
case XN_PROPERTY_TYPE_INTEGER:
{
XnActualIntProperty* pProp = (XnActualIntProperty*)pRequestProp;
XN_VALIDATE_NEW(pNewProp, XnActualIntProperty, pProp->GetName(), pProp->GetValue());
break;
}
case XN_PROPERTY_TYPE_REAL:
{
XnActualRealProperty* pProp = (XnActualRealProperty*)pRequestProp;
XN_VALIDATE_NEW(pNewProp, XnActualRealProperty, pProp->GetName(), pProp->GetValue());
break;
}
case XN_PROPERTY_TYPE_STRING:
{
XnActualStringProperty* pProp = (XnActualStringProperty*)pRequestProp;
XN_VALIDATE_NEW(pNewProp, XnActualStringProperty, pProp->GetName(), pProp->GetValue());
break;
}
case XN_PROPERTY_TYPE_GENERAL:
{
XnActualGeneralProperty* pProp = (XnActualGeneralProperty*)pRequestProp;
// create new buffer
XnGeneralBuffer gbNew;
nRetVal = XnGeneralBufferAlloc(&gbNew, pProp->GetValue().nDataSize);
XN_IS_STATUS_OK(nRetVal);
// copy content
xnOSMemCopy(gbNew.pData, pProp->GetValue().pData, pProp->GetValue().nDataSize);
XnActualGeneralProperty* pNewGeneralProp = NULL;
XN_VALIDATE_NEW(pNewGeneralProp, XnActualGeneralProperty, pProp->GetName(), gbNew);
pNewGeneralProp->SetAsBufferOwner(TRUE);
pNewProp = pNewGeneralProp;
break;
}
default:
XN_LOG_WARNING_RETURN(XN_STATUS_ERROR, XN_MASK_DDK, "Unknown property type: %d\n", pRequestProp->GetType());
} // switch
// add the property to the module
nRetVal = m_pModule->AddProperty(pNewProp);
if (nRetVal != XN_STATUS_OK)
{
XN_DELETE(pNewProp);
return (nRetVal);
}
// and add it to the list of allocated ones (so we'll delete it afterwards)
m_Allocated.AddLast(pNewProp);
return XN_STATUS_OK;
}
void XnAudioProcessor::ProcessWholePacket(const XnSensorProtocolResponseHeader* pHeader, const XnUChar* pData)
{
xnOSEnterCriticalSection(&m_pBuffer->hLock);
// take write packet
XnUChar* pWritePacket = m_pBuffer->pAudioBuffer + (m_pBuffer->nAudioWriteIndex * m_pBuffer->nAudioPacketSize);
if (m_bDeleteChannel)
{
XnUInt16* pSamples = (XnUInt16*)pData;
XnUInt16* pSamplesEnd = (XnUInt16*)(pData + pHeader->nBufSize);
XnUInt16* pOutput = (XnUInt16*)pWritePacket;
while (pSamples < pSamplesEnd)
{
*pOutput = *pSamples;
pOutput++;
// skip a sample
pSamples += 2;
}
}
else
{
// copy data
xnOSMemCopy(pWritePacket, pData, pHeader->nBufSize);
}
// mark timestamp
if (ShouldUseHostTimestamps())
{
m_pBuffer->pAudioPacketsTimestamps[m_pBuffer->nAudioWriteIndex] = GetHostTimestamp();
}
else
{
m_pBuffer->pAudioPacketsTimestamps[m_pBuffer->nAudioWriteIndex] = CreateTimestampFromDevice(pHeader->nTimeStamp);
}
if (m_nLastPacketID % 10 == 0)
{
XnUInt64 nSysTime;
xnOSGetTimeStamp(&nSysTime);
xnDumpFileWriteString(m_pDevicePrivateData->BandwidthDump, "%llu,%s,%d,%d\n",
nSysTime, "Audio", -1, m_nBytesReceived);
m_nBytesReceived = 0;
}
// move write index forward
m_pBuffer->nAudioWriteIndex = (m_pBuffer->nAudioWriteIndex + 1) % m_pBuffer->nAudioBufferNumOfPackets;
// if write index got to read index (end of buffer), move read index forward (and loose a packet)
if (m_pBuffer->nAudioWriteIndex == m_pBuffer->nAudioReadIndex)
{
m_pBuffer->nAudioReadIndex = (m_pBuffer->nAudioReadIndex + 1) % m_pBuffer->nAudioBufferNumOfPackets;
}
xnOSLeaveCriticalSection(&m_pBuffer->hLock);
xnDumpFileWriteBuffer(m_AudioInDump, pData, pHeader->nBufSize);
if (m_pBuffer->pAudioCallback != NULL)
{
m_pBuffer->pAudioCallback(m_pBuffer->pAudioCallbackCookie);
}
}
XnStatus XnSensor::InitSensor(const XnDeviceConfig* pDeviceConfig)
{
XnStatus nRetVal = XN_STATUS_OK;
XnDevicePrivateData* pDevicePrivateData = GetDevicePrivateData();
pDevicePrivateData->pSensor = this;
pDevicePrivateData->nDepthFramePos = 0;
pDevicePrivateData->nImageFramePos = 0;
xnOSMemCopy(&pDevicePrivateData->DeviceConfig, pDeviceConfig, sizeof(XnDeviceConfig));
xnOSMemSet(pDevicePrivateData->cpSensorID, 0, XN_SENSOR_PROTOCOL_SENSOR_ID_LENGTH);
pDevicePrivateData->bSyncAudio = TRUE;
switch (pDeviceConfig->DeviceMode)
{
case XN_DEVICE_MODE_READ:
break;
case XN_DEVICE_MODE_WRITE:
return (XN_STATUS_IO_DEVICE_MODE_NOT_SUPPORTED);
default:
return (XN_STATUS_IO_DEVICE_INVALID_MODE);
}
// Register USB event callback
#if WIN32
nRetVal = m_SensorIO.SetCallback(&USBEventCallback, this);
XN_IS_STATUS_OK(nRetVal);
#endif
// open IO
nRetVal = m_SensorIO.OpenDevice(pDeviceConfig->cpConnectionString);
XN_IS_STATUS_OK(nRetVal);
nRetVal = m_USBPath.UnsafeUpdateValue(m_SensorIO.GetDevicePath());
XN_IS_STATUS_OK(nRetVal);
// initialize
nRetVal = XnDeviceSensorInit(pDevicePrivateData);
XN_IS_STATUS_OK(nRetVal);
// init firmware
nRetVal = m_Firmware.Init((XnBool)m_ResetSensorOnStartup.GetValue());
XN_IS_STATUS_OK(nRetVal);
m_bInitialized = TRUE;
m_ResetSensorOnStartup.UpdateSetCallback(NULL, NULL);
// Init modules
nRetVal = m_FixedParams.Init();
XN_IS_STATUS_OK(nRetVal);
// update serial number
nRetVal = m_ID.UnsafeUpdateValue(m_FixedParams.GetSensorSerial());
XN_IS_STATUS_OK(nRetVal);
AddSupportedStream(XN_STREAM_TYPE_DEPTH);
AddSupportedStream(XN_STREAM_TYPE_IMAGE);
AddSupportedStream(XN_STREAM_TYPE_IR);
AddSupportedStream(XN_STREAM_TYPE_AUDIO);
return XN_STATUS_OK;
}
XnStatus XnSensor::InitSensor(const XnDeviceConfig* pDeviceConfig)
{
XnStatus nRetVal = XN_STATUS_OK;
XnDevicePrivateData* pDevicePrivateData = GetDevicePrivateData();
pDevicePrivateData->pSensor = this;
pDevicePrivateData->nDepthFramePos = 0;
pDevicePrivateData->nImageFramePos = 0;
xnOSMemCopy(&pDevicePrivateData->DeviceConfig, pDeviceConfig, sizeof(XnDeviceConfig));
xnOSMemSet(pDevicePrivateData->cpSensorID, 0, XN_SENSOR_PROTOCOL_SENSOR_ID_LENGTH);
switch (pDeviceConfig->DeviceMode)
{
case XN_DEVICE_MODE_READ:
break;
case XN_DEVICE_MODE_WRITE:
return (XN_STATUS_IO_DEVICE_MODE_NOT_SUPPORTED);
default:
return (XN_STATUS_IO_DEVICE_INVALID_MODE);
}
// Register USB event callback
#if WIN32
nRetVal = m_SensorIO.SetCallback(&USBEventCallback, this);
XN_IS_STATUS_OK(nRetVal);
#endif
// open IO
nRetVal = m_SensorIO.OpenDevice(pDeviceConfig->cpConnectionString, (XnBool)m_LeanInit.GetValue());
XN_IS_STATUS_OK(nRetVal);
nRetVal = m_USBPath.UnsafeUpdateValue(m_SensorIO.GetDevicePath());
XN_IS_STATUS_OK(nRetVal);
// initialize
nRetVal = XnDeviceSensorInit(pDevicePrivateData);
XN_IS_STATUS_OK(nRetVal);
// init firmware
nRetVal = m_Firmware.Init((XnBool)m_ResetSensorOnStartup.GetValue(), (XnBool)m_LeanInit.GetValue());
XN_IS_STATUS_OK(nRetVal);
m_bInitialized = TRUE;
m_ResetSensorOnStartup.UpdateSetCallback(NULL, NULL);
m_LeanInit.UpdateSetCallback(NULL, NULL);
// update device info properties
nRetVal = m_DeviceName.UnsafeUpdateValue(GetFixedParams()->GetDeviceName());
XN_IS_STATUS_OK(nRetVal);
nRetVal = m_VendorSpecificData.UnsafeUpdateValue(GetFixedParams()->GetVendorData());
XN_IS_STATUS_OK(nRetVal);
nRetVal = m_ID.UnsafeUpdateValue(GetFixedParams()->GetSensorSerial());
XN_IS_STATUS_OK(nRetVal);
nRetVal = m_PlatformString.UnsafeUpdateValue(GetFixedParams()->GetPlatformString());
XN_IS_STATUS_OK(nRetVal);
// Add supported streams
AddSupportedStream(XN_STREAM_TYPE_DEPTH);
AddSupportedStream(XN_STREAM_TYPE_IR);
if (GetFirmware()->GetInfo()->bImageSupported)
{
AddSupportedStream(XN_STREAM_TYPE_IMAGE);
}
if (GetFirmware()->GetInfo()->bAudioSupported)
{
AddSupportedStream(XN_STREAM_TYPE_AUDIO);
}
return XN_STATUS_OK;
}
XnStatus XnSensor::InitSensor(const XnDeviceConfig* pDeviceConfig)
{
XnStatus nRetVal = XN_STATUS_OK;
XnDevicePrivateData* pDevicePrivateData = GetDevicePrivateData();
pDevicePrivateData->pSensor = this;
pDevicePrivateData->nDepthFramePos = 0;
pDevicePrivateData->nImageFramePos = 0;
xnOSMemCopy(&pDevicePrivateData->DeviceConfig, pDeviceConfig, sizeof(XnDeviceConfig));
xnOSMemSet(pDevicePrivateData->cpSensorID, 0, XN_SENSOR_PROTOCOL_SENSOR_ID_LENGTH);
pDevicePrivateData->bSyncAudio = TRUE;
switch (pDeviceConfig->DeviceMode)
{
case XN_DEVICE_MODE_READ:
break;
case XN_DEVICE_MODE_WRITE:
return (XN_STATUS_IO_DEVICE_MODE_NOT_SUPPORTED);
default:
return (XN_STATUS_IO_DEVICE_INVALID_MODE);
}
// Register USB event callback
#if WIN32
nRetVal = m_SensorIO.SetCallback(&USBEventCallback, this);
XN_IS_STATUS_OK(nRetVal);
#endif
// open IO
nRetVal = m_SensorIO.OpenDevice(pDeviceConfig->cpConnectionString);
XN_IS_STATUS_OK(nRetVal);
nRetVal = m_USBPath.UnsafeUpdateValue(m_SensorIO.GetDevicePath());
XN_IS_STATUS_OK(nRetVal);
// initialize
nRetVal = XnDeviceSensorInit(pDevicePrivateData);
XN_IS_STATUS_OK(nRetVal);
// init firmware
nRetVal = m_Firmware.Init((XnBool)m_ResetSensorOnStartup.GetValue());
XN_IS_STATUS_OK(nRetVal);
m_bInitialized = TRUE;
m_ResetSensorOnStartup.UpdateSetCallback(NULL, NULL);
// Init modules
nRetVal = m_FixedParams.Init();
XN_IS_STATUS_OK(nRetVal);
XnDeviceInformation deviceInfo;
strcpy(deviceInfo.strDeviceName, "PrimeSense Sensor");
strcpy(deviceInfo.strVendorData, "");
// try to take device information (only supported from 5.3.25)
if (pDevicePrivateData->Version.nMajor > 5 ||
(pDevicePrivateData->Version.nMajor == 5 && pDevicePrivateData->Version.nMinor > 3) ||
(pDevicePrivateData->Version.nMajor == 5 && pDevicePrivateData->Version.nMinor == 3 && pDevicePrivateData->Version.nBuild >= 25))
{
nRetVal = XnHostProtocolAlgorithmParams(pDevicePrivateData, XN_HOST_PROTOCOL_ALGORITHM_DEVICE_INFO,
&deviceInfo, sizeof(deviceInfo), (XnResolutions)0, 0);
XN_IS_STATUS_OK(nRetVal);
}
nRetVal = m_DeviceName.UnsafeUpdateValue(deviceInfo.strDeviceName);
XN_IS_STATUS_OK(nRetVal);
nRetVal = m_VendorSpecificData.UnsafeUpdateValue(deviceInfo.strVendorData);
XN_IS_STATUS_OK(nRetVal);
// update serial number
nRetVal = m_ID.UnsafeUpdateValue(m_FixedParams.GetSensorSerial());
XN_IS_STATUS_OK(nRetVal);
AddSupportedStream(XN_STREAM_TYPE_DEPTH);
AddSupportedStream(XN_STREAM_TYPE_IMAGE);
AddSupportedStream(XN_STREAM_TYPE_IR);
AddSupportedStream(XN_STREAM_TYPE_AUDIO);
return XN_STATUS_OK;
}
// --------------------------------
// Code
// --------------------------------
void audioPlay()
{
if (g_AudioData.hWaveOut == NULL) // not initialized
return;
const AudioMetaData* pAudioMD = getAudioMetaData();
if (pAudioMD == NULL || pAudioMD->DataSize() == 0 || !pAudioMD->IsDataNew())
return;
if (g_AudioData.bFlush)
{
printf("Audio is falling behind. Flushing all queue.\n");
xnDumpFileWriteString(g_AudioData.SyncDump, "Flushing queue...\n");
// mark not to check all dropped headers
g_AudioData.nFirstToCheck = g_AudioData.nAudioNextBuffer;
// flush all queued headers
waveOutReset(g_AudioData.hWaveOut);
g_AudioData.bFlush = false;
return;
}
int nBufferSize = pAudioMD->DataSize();
WAVEHDR* pHeader = &g_AudioData.pAudioBuffers[g_AudioData.nAudioNextBuffer];
if ((pHeader->dwFlags & WHDR_DONE) == 0)
{
printf("No audio buffer is available!. Audio buffer will be lost!\n");
return;
}
// first unprepare this header
MMRESULT mmRes = waveOutUnprepareHeader(g_AudioData.hWaveOut, pHeader, sizeof(WAVEHDR));
if (mmRes != MMSYSERR_NOERROR)
{
CHAR msg[250];
waveOutGetErrorText(mmRes, msg, 250);
printf("Failed unpreparing header: %s\n", msg);
}
int nMaxPlayedAudio = (int)(pAudioMD->SampleRate() / 1000.0 * pAudioMD->NumberOfChannels() * 2 * AUDIO_LATENCY_THRESHOLD);
if (nBufferSize > nMaxPlayedAudio)
{
printf("Dropping %d bytes of audio to keep synch.\n", nBufferSize - nMaxPlayedAudio);
nBufferSize = nMaxPlayedAudio;
}
const XnUInt8* pData = pAudioMD->Data();
if (nBufferSize > g_AudioData.nBufferSize)
{
printf("Dropping %d bytes of audio to match buffer size.\n", nBufferSize - g_AudioData.nBufferSize);
pData += (nBufferSize - g_AudioData.nBufferSize);
nBufferSize = g_AudioData.nBufferSize;
}
pHeader->dwFlags = 0;
xnOSMemCopy(pHeader->lpData, pData, nBufferSize);
pHeader->dwBufferLength = nBufferSize;
// prepare header
mmRes = waveOutPrepareHeader(g_AudioData.hWaveOut, pHeader, sizeof(WAVEHDR));
if (mmRes != MMSYSERR_NOERROR)
{
CHAR msg[250];
waveOutGetErrorText(mmRes, msg, 250);
printf("Unable to prepare header: %s\n", msg);
return;
}
// queue header
mmRes = waveOutWrite(g_AudioData.hWaveOut, pHeader, sizeof(WAVEHDR));
if (mmRes != MMSYSERR_NOERROR)
{
CHAR msg[250];
waveOutGetErrorText(mmRes, msg, 250);
printf("Unable to queue header: %s\n", msg);
return;
}
// place end-time as a timestamp
g_AudioData.pAudioTimestamps[g_AudioData.nAudioNextBuffer] = (XnUInt64)(pAudioMD->Timestamp() + nBufferSize / (pAudioMD->BitsPerSample() / 8.0) / pAudioMD->NumberOfChannels() / (pAudioMD->SampleRate() / 1e6));
xnDumpFileWriteString(g_AudioData.SyncDump, "Queued index %d with timestamp %llu (%u bytes, %f ms, end timestamp: %llu)\n", g_AudioData.nAudioNextBuffer, pAudioMD->Timestamp(), nBufferSize, nBufferSize / 2.0 / pAudioMD->NumberOfChannels() / (pAudioMD->SampleRate() / 1e3), g_AudioData.pAudioTimestamps[g_AudioData.nAudioNextBuffer]);
g_AudioData.nAudioNextBuffer = (g_AudioData.nAudioNextBuffer + 1) % NUMBER_OF_AUDIO_BUFFERS;
}
XnStatus XnFileDevice::BCReadFrame(XnBool* pbWrapOccured)
{
// Local function variables
XnStatus nRetVal = XN_STATUS_OK;
XnDeviceFileFrameHeaderV3 FileFrameHeader;
*pbWrapOccured = FALSE;
// If we've reached the last frame, seek back to the first one
if (m_pBCData->nFramePos > m_pBCData->StreamProperties.nNumOfFrames)
{
nRetVal = HandleEndOfStream();
XN_IS_STATUS_OK(nRetVal);
*pbWrapOccured = TRUE;
if (m_bEOF)
{
return XN_STATUS_OK;
}
}
m_bFileHasData = TRUE;
// Note: Since this is an internal function, the pointers are assumed to be checked by the caller
// Read the frame header
switch (m_nFileVersion)
{
case 3:
{
nRetVal = m_pInputStream->ReadData((XnUChar*)&FileFrameHeader, sizeof(XnDeviceFileFrameHeaderV3));
XN_IS_STATUS_OK(nRetVal);
nRetVal = XnDeviceFileAdjustFileFrameHeaderV3(&FileFrameHeader, &FileFrameHeader);
XN_IS_STATUS_OK(nRetVal);
}
break;
case 2:
{
XnDeviceFileFrameHeaderV2 FileFrameHeaderV2;
nRetVal = m_pInputStream->ReadData((XnUChar*)&FileFrameHeaderV2, sizeof(XnDeviceFileFrameHeaderV2));
XN_IS_STATUS_OK(nRetVal);
nRetVal = XnDeviceFileAdjustFileFrameHeaderV2(&FileFrameHeaderV2, &FileFrameHeader);
XN_IS_STATUS_OK(nRetVal);
}
break;
case 1:
{
XnDeviceFileFrameHeaderV1 FileFrameHeaderV1;
nRetVal = m_pInputStream->ReadData((XnUChar*)&FileFrameHeaderV1, sizeof(XnDeviceFileFrameHeaderV1));
XN_IS_STATUS_OK(nRetVal);
nRetVal = XnDeviceFileAdjustFileFrameHeaderV1(&FileFrameHeaderV1, &FileFrameHeader);
XN_IS_STATUS_OK(nRetVal);
}
break;
default:
return XN_STATUS_IO_INVALID_STREAM_HEADER;
}
FileFrameHeader.FrameProperties.nDepthFrameID = m_pBCData->nFramePos;
FileFrameHeader.FrameProperties.nImageFrameID = m_pBCData->nFramePos;
// Make sure we aren't going to overflow the packed stream buffer
if (FileFrameHeader.nPackedStreamSize > m_pBCData->nPackedStreamBufferSize)
{
return (XN_STATUS_INPUT_BUFFER_OVERFLOW);
}
// Read the frame packed stream data into the packed stream buffer
nRetVal = m_pInputStream->ReadData(m_pBCData->pPackedStreamBuffer, FileFrameHeader.nPackedStreamSize);
XN_IS_STATUS_OK(nRetVal);
// read the frame header
XnPackedStreamFrameHeaderV3 PackedStreamHeader;
XnUChar* pPackedBuffer = m_pBCData->pPackedStreamBuffer;
switch (m_nFileVersion)
{
case 0:
case 3:
{
xnOSMemCopy(&PackedStreamHeader, pPackedBuffer, sizeof(XnPackedStreamFrameHeaderV3));
pPackedBuffer += sizeof(XnPackedStreamFrameHeaderV3);
nRetVal = XnIOAdjustPackedStreamFrameHeaderV3(&PackedStreamHeader, &PackedStreamHeader);
XN_IS_STATUS_OK(nRetVal);
break;
}
case 2:
{
XnPackedStreamFrameHeaderV2* pPackedStreamHeaderV2 = (XnPackedStreamFrameHeaderV2*)pPackedBuffer;
pPackedBuffer += sizeof(XnPackedStreamFrameHeaderV2);
nRetVal = XnIOAdjustPackedStreamFrameHeaderV2(pPackedStreamHeaderV2, &PackedStreamHeader);
XN_IS_STATUS_OK(nRetVal);
break;
}
case 1:
{
XnPackedStreamFrameHeaderV1* pPackedStreamHeaderV1 = (XnPackedStreamFrameHeaderV1*)pPackedBuffer;
pPackedBuffer += sizeof(XnPackedStreamFrameHeaderV1);
nRetVal = XnIOAdjustPackedStreamFrameHeaderV1(pPackedStreamHeaderV1, &PackedStreamHeader);
XN_IS_STATUS_OK(nRetVal);
break;
}
default:
return XN_STATUS_IO_INVALID_STREAM_HEADER;
}
// Depth
XnNodeInfo* pNodeInfo;
if (XN_STATUS_OK == m_nodeInfoMap.Get(XN_STREAM_NAME_DEPTH, pNodeInfo))
{
m_pStreamData->nDataSize = m_pBCData->StreamProperties.nDepthBufferSize;
nRetVal = pNodeInfo->pXnCodec->Decompress(pPackedBuffer, PackedStreamHeader.nCompDepthBufferSize, (XnUChar*)m_pStreamData->pData, &m_pStreamData->nDataSize);
XN_IS_STATUS_OK(nRetVal);
nRetVal = m_pNotifications->OnNodeNewData(m_pNotificationsCookie, XN_STREAM_NAME_DEPTH, FileFrameHeader.FrameProperties.nDepthTimeStamp * 1000, m_pBCData->nFramePos, m_pStreamData->pData, m_pBCData->StreamProperties.nDepthBufferSize);
XN_IS_STATUS_OK(nRetVal);
pNodeInfo->nCurrFrameID++;
pPackedBuffer += PackedStreamHeader.nCompDepthBufferSize;
}
// Image
if (XN_STATUS_OK == m_nodeInfoMap.Get(XN_STREAM_NAME_IMAGE, pNodeInfo))
{
m_pStreamData->nDataSize = m_pBCData->StreamProperties.nImageBufferSize;
nRetVal = pNodeInfo->pXnCodec->Decompress(pPackedBuffer, PackedStreamHeader.nCompImageBufferSize, (XnUChar*)m_pStreamData->pData, &m_pStreamData->nDataSize);
XN_IS_STATUS_OK(nRetVal);
nRetVal = m_pNotifications->OnNodeNewData(m_pNotificationsCookie, XN_STREAM_NAME_IMAGE, FileFrameHeader.FrameProperties.nImageTimeStamp * 1000, m_pBCData->nFramePos, m_pStreamData->pData, m_pBCData->StreamProperties.nImageBufferSize);
XN_IS_STATUS_OK(nRetVal);
pNodeInfo->nCurrFrameID++;
pPackedBuffer += PackedStreamHeader.nCompImageBufferSize;
}
// we do not support MISC
pPackedBuffer += PackedStreamHeader.nCompMiscBufferSize;
// Audio
if (XN_STATUS_OK == m_nodeInfoMap.Get(XN_STREAM_NAME_AUDIO, pNodeInfo))
{
m_pStreamData->nDataSize = m_pBCData->StreamProperties.nAudioBufferSize;
nRetVal = pNodeInfo->pXnCodec->Decompress(pPackedBuffer, PackedStreamHeader.nCompAudioBufferSize, (XnUChar*)m_pStreamData->pData, &m_pStreamData->nDataSize);
XN_IS_STATUS_OK(nRetVal);
nRetVal = m_pNotifications->OnNodeNewData(m_pNotificationsCookie, XN_STREAM_NAME_AUDIO, FileFrameHeader.FrameProperties.nAudioTimeStamp * 1000, m_pBCData->nFramePos, m_pStreamData->pData, m_pStreamData->nDataSize);
XN_IS_STATUS_OK(nRetVal);
pNodeInfo->nCurrFrameID++;
pPackedBuffer += PackedStreamHeader.nCompAudioBufferSize;
}
// Increase the file frame position
m_pBCData->nFramePos++;
// All is good...
return (XN_STATUS_OK);
}
XN_C_API XnStatus xnUSBEnumerateDevices(XnUInt16 nVendorID, XnUInt16 nProductID, const XnUSBConnectionString** pastrDevicePaths, XnUInt32* pnCount)
{
// support up to 30 devices
XnUSBConnectionString cpUSBID;
XnUSBConnectionString cpUSBPathCmp;
XnUSBConnectionString aNames[MAX_POTENTIAL_DEVICES];
HDEVINFO hDevInfo = NULL;
ULONG nDevices = 0;
XnUInt32 nFoundDevices = 0;
SP_DEVICE_INTERFACE_DATA devInterfaceData;
XnBool bReachedEnd = FALSE;
// Validate xnUSB
XN_VALIDATE_USB_INIT();
LPCGUID pInterfaceGuid = &GUID_CLASS_PSDRV_USB;
// See if the driver is installed
hDevInfo = SetupDiGetClassDevs (pInterfaceGuid, NULL, NULL, (DIGCF_PRESENT | DIGCF_DEVICEINTERFACE));
if (hDevInfo == INVALID_HANDLE_VALUE)
{
// No devices are present...
return (XN_STATUS_USB_DRIVER_NOT_FOUND);
}
// Scan the hardware for any devices that are attached to our driver.
devInterfaceData.cbSize = sizeof(SP_DEVICE_INTERFACE_DATA);
while (nDevices < MAX_POTENTIAL_DEVICES)
{
// Get information about the device
if (SetupDiEnumDeviceInterfaces(hDevInfo, 0, pInterfaceGuid, nDevices, &devInterfaceData))
{
PSP_DEVICE_INTERFACE_DETAIL_DATA pDevInterfaceDetailData = NULL;
ULONG nPredictedLength = 0;
ULONG nRequiredLength = 0;
// Probe how much memory is needed to read the device info
SetupDiGetDeviceInterfaceDetail(hDevInfo, &devInterfaceData, NULL, 0, &nRequiredLength, NULL);
// Allocate memory for the device info
nPredictedLength = nRequiredLength;
pDevInterfaceDetailData = (PSP_DEVICE_INTERFACE_DETAIL_DATA)malloc(nPredictedLength);
if(pDevInterfaceDetailData == NULL)
{
// Not enough memory...
return XN_STATUS_ALLOC_FAILED;
}
// Read the device info
pDevInterfaceDetailData->cbSize = sizeof(SP_DEVICE_INTERFACE_DETAIL_DATA);
if (!SetupDiGetDeviceInterfaceDetail(hDevInfo, &devInterfaceData, pDevInterfaceDetailData, nPredictedLength, &nRequiredLength, NULL))
{
// Something bad has happened...
free(pDevInterfaceDetailData);
return XN_STATUS_ERROR;
}
// Make sure we have the right VID/PID
cpUSBID[0] = 0;
sprintf_s (cpUSBID, "vid_%04x&pid_%04x", nVendorID, nProductID);
cpUSBPathCmp[0] = 0;
StringCchCopy(cpUSBPathCmp, MAX_DEVICE_STR_LENGTH, pDevInterfaceDetailData->DevicePath);
if (strstr(_strlwr(cpUSBPathCmp), cpUSBID) != 0)
{
StringCchCopy(aNames[nFoundDevices], MAX_DEVICE_STR_LENGTH, pDevInterfaceDetailData->DevicePath);
++nFoundDevices;
}
++nDevices;
}
else if (ERROR_NO_MORE_ITEMS == GetLastError())
{
// no more devices
bReachedEnd = TRUE;
break;
}
}
SetupDiDestroyDeviceInfoList(hDevInfo);
if (!bReachedEnd)
{
// we probably passed our limit
XN_LOG_ERROR_RETURN(XN_STATUS_ERROR, XN_MASK_USB, "Found more than %d devices! This is not supported.", MAX_POTENTIAL_DEVICES);
}
XnUSBConnectionString* pNames;
XN_VALIDATE_CALLOC(pNames, XnUSBConnectionString, nFoundDevices);
xnOSMemCopy(pNames, aNames, sizeof(XnUSBConnectionString) * nFoundDevices);
*pastrDevicePaths = pNames;
*pnCount = nFoundDevices;
// All is good...
return (XN_STATUS_OK);
}
//---------------------------------------------------------------------------
// Code
//---------------------------------------------------------------------------
XnBool XN_CALLBACK_TYPE XnDeviceSensorProtocolUsbEpCb(XnUChar* pBuffer, XnUInt32 nBufferSize, void* pCallbackData)
{
XN_PROFILING_START_MT_SECTION("XnDeviceSensorProtocolUsbEpCb");
XnUInt32 nReadBytes;
XnUInt16 nMagic;
XnSpecificUsbDevice* pDevice = (XnSpecificUsbDevice*)pCallbackData;
XnDevicePrivateData* pDevicePrivateData = pDevice->pDevicePrivateData;
XnUChar* pBufEnd = pBuffer + nBufferSize;
XnSpecificUsbDeviceState* pCurrState = &pDevice->CurrState;
while (pBuffer < pBufEnd)
{
switch (pCurrState->State)
{
case XN_WAITING_FOR_CONFIGURATION:
if (pDevicePrivateData->bIgnoreDataPackets)
{
// ignore this packet
xnLogVerbose(XN_MASK_SENSOR_PROTOCOL, "ignoring %d bytes - device requested to ignore!", nBufferSize);
pBuffer = pBufEnd;
}
else
{
pCurrState->State = XN_IGNORING_GARBAGE;
pCurrState->nMissingBytesInState = pDevice->nIgnoreBytes;
}
break;
case XN_IGNORING_GARBAGE:
// ignore first bytes on this endpoint. NOTE: due to a bug in the firmware, the first data received
// on each endpoint is corrupt, causing wrong timestamp calculation, causing future (true) timestamps
// to be calculated wrongly. By ignoring the first data received on each endpoint we hope to get
// only valid data.
nReadBytes = XN_MIN((XnUInt32)(pBufEnd - pBuffer), pCurrState->nMissingBytesInState);
if (nReadBytes > 0)
{
xnLogVerbose(XN_MASK_SENSOR_PROTOCOL, "ignoring %d bytes - ignore garbage phase!", nReadBytes);
pCurrState->nMissingBytesInState -= nReadBytes;
pBuffer += nReadBytes;
}
if (pCurrState->nMissingBytesInState == 0)
{
pCurrState->State = XN_LOOKING_FOR_MAGIC;
pCurrState->nMissingBytesInState = sizeof(XnUInt16);
}
break;
case XN_LOOKING_FOR_MAGIC:
nMagic = XN_PREPARE_VAR16_IN_BUFFER(pDevicePrivateData->FWInfo.nFWMagic);
if (pCurrState->nMissingBytesInState == sizeof(XnUInt8) && // first byte already found
pBuffer[0] == ((XnUInt8*)&nMagic)[1]) // we have here second byte
{
// move to next byte
pBuffer++;
// move to next state
pCurrState->CurrHeader.nMagic = nMagic;
pCurrState->State = XN_PACKET_HEADER;
pCurrState->nMissingBytesInState = sizeof(XnSensorProtocolResponseHeader);
break;
}
while (pBuffer < pBufEnd)
{
if (nMagic == *(XnUInt16*)(pBuffer))
{
pCurrState->CurrHeader.nMagic = nMagic;
pCurrState->State = XN_PACKET_HEADER;
pCurrState->nMissingBytesInState = sizeof(XnSensorProtocolResponseHeader);
break;
}
else
{
pBuffer++;
}
}
if (pBuffer == pBufEnd && // magic wasn't found
pBuffer[-1] == ((XnUInt8*)&nMagic)[0]) // last byte in buffer is first in magic
{
// mark that we found first one
pCurrState->nMissingBytesInState--;
}
break;
case XN_PACKET_HEADER:
nReadBytes = XN_MIN((XnUInt32)(pBufEnd - pBuffer), pCurrState->nMissingBytesInState);
xnOSMemCopy((XnUChar*)&pCurrState->CurrHeader + sizeof(XnSensorProtocolResponseHeader) - pCurrState->nMissingBytesInState,
pBuffer, nReadBytes);
pCurrState->nMissingBytesInState -= nReadBytes;
pBuffer += nReadBytes;
if (pCurrState->nMissingBytesInState == 0)
{
// we have entire header. Fix it
pCurrState->CurrHeader.nBufSize = XN_PREPARE_VAR16_IN_BUFFER(pCurrState->CurrHeader.nBufSize);
pCurrState->CurrHeader.nMagic = XN_PREPARE_VAR16_IN_BUFFER(pCurrState->CurrHeader.nMagic);
pCurrState->CurrHeader.nPacketID = XN_PREPARE_VAR16_IN_BUFFER(pCurrState->CurrHeader.nPacketID);
pCurrState->CurrHeader.nTimeStamp = XN_PREPARE_VAR32_IN_BUFFER(pCurrState->CurrHeader.nTimeStamp);
pCurrState->CurrHeader.nType = XN_PREPARE_VAR16_IN_BUFFER(pCurrState->CurrHeader.nType);
pCurrState->CurrHeader.nBufSize = xnOSEndianSwapUINT16(pCurrState->CurrHeader.nBufSize);
pCurrState->CurrHeader.nBufSize -= sizeof(XnSensorProtocolResponseHeader);
pCurrState->State = XN_PACKET_DATA;
pCurrState->nMissingBytesInState = pCurrState->CurrHeader.nBufSize;
}
break;
case XN_PACKET_DATA:
nReadBytes = XN_MIN((XnUInt32)(pBufEnd - pBuffer), pCurrState->nMissingBytesInState);
pDevicePrivateData->pSensor->GetFirmware()->GetStreams()->ProcessPacketChunk(&pCurrState->CurrHeader, pBuffer, pCurrState->CurrHeader.nBufSize - pCurrState->nMissingBytesInState, nReadBytes);
pBuffer += nReadBytes;
pCurrState->nMissingBytesInState -= nReadBytes;
if (pCurrState->nMissingBytesInState == 0)
{
pCurrState->State = XN_LOOKING_FOR_MAGIC;
pCurrState->nMissingBytesInState = sizeof(XnUInt16);
}
break;
}
}
XN_PROFILING_END_SECTION;
return TRUE;
}
XnStatus XnFileDevice::BCReadInitialState(XnPropertySet* pSet)
{
// Local function variables
XnStatus nRetVal = XN_STATUS_OK;
XnDeviceFileHeader DeviceFileHeader;
XN_STREAM_FLAGS_TYPE nStreamFlags = 0;
m_pBCData->nFramePos = 1;
xnOSFreeAligned(m_pBCData->pPackedStreamBuffer);
m_pBCData->pPackedStreamBuffer = NULL;
m_pBCData->nPackedStreamBufferSize = 0;
// read StreamProperties
if (m_nFileVersion == 3)
{
// Current Version
nRetVal = m_pInputStream->ReadData((XnUChar*)&DeviceFileHeader.nMajorVersion, sizeof(XnUInt16));
XN_IS_STATUS_OK(nRetVal);
nRetVal = m_pInputStream->ReadData((XnUChar*)&DeviceFileHeader.nMinorVersion, sizeof(XnUInt16));
XN_IS_STATUS_OK(nRetVal);
nRetVal = m_pInputStream->ReadData((XnUChar*)&DeviceFileHeader.StreamProperties, sizeof(XnStreamPropertiesV3));
XN_IS_STATUS_OK(nRetVal);
DeviceFileHeader.nMajorVersion = XN_PREPARE_VAR16_IN_BUFFER(DeviceFileHeader.nMajorVersion);
DeviceFileHeader.nMinorVersion = XN_PREPARE_VAR16_IN_BUFFER(DeviceFileHeader.nMinorVersion);
nRetVal = XnIOAdjustStreamPropertiesV3(&DeviceFileHeader.StreamProperties, &DeviceFileHeader.StreamProperties);
XN_IS_STATUS_OK(nRetVal);
}
else if (m_nFileVersion == 2)
{
// Version 2
DeviceFileHeader.nMajorVersion = 0;
DeviceFileHeader.nMinorVersion = 0;
XnStreamPropertiesV2 StreamPropertiesV2;
nRetVal = m_pInputStream->ReadData((XnUChar*)&StreamPropertiesV2, sizeof(XnStreamPropertiesV2));
XN_IS_STATUS_OK(nRetVal);
nRetVal = XnIOAdjustStreamPropertiesV2(&StreamPropertiesV2, &DeviceFileHeader.StreamProperties);
XN_IS_STATUS_OK(nRetVal);
}
else if (m_nFileVersion == 1)
{
// Version 1
DeviceFileHeader.nMajorVersion = 0;
DeviceFileHeader.nMinorVersion = 0;
XnStreamPropertiesV1 StreamPropertiesV1;
nRetVal = m_pInputStream->ReadData((XnUChar*)&StreamPropertiesV1, sizeof(XnStreamPropertiesV1));
XN_IS_STATUS_OK(nRetVal);
nRetVal = XnIOAdjustStreamPropertiesV1(&StreamPropertiesV1, &DeviceFileHeader.StreamProperties);
XN_IS_STATUS_OK(nRetVal);
}
else
{
// Bad Magic
return XN_STATUS_IO_INVALID_STREAM_HEADER;
}
// read packed stream properties
switch (m_nFileVersion)
{
case 3:
{
nRetVal = m_pInputStream->ReadData((XnUChar*)&DeviceFileHeader.PackedStreamProperties, sizeof(XnPackedStreamProperties));
XN_IS_STATUS_OK(nRetVal);
nRetVal = XnIOAdjustPackedStreamPropertiesV3(&DeviceFileHeader.PackedStreamProperties, &DeviceFileHeader.PackedStreamProperties);
XN_IS_STATUS_OK(nRetVal);
}
break;
case 2:
{
XnPackedStreamPropertiesV2 PackedStreamPropertiesV2;
nRetVal = m_pInputStream->ReadData((XnUChar*)&PackedStreamPropertiesV2, sizeof(XnPackedStreamPropertiesV2));
XN_IS_STATUS_OK(nRetVal);
nRetVal = XnIOAdjustPackedStreamPropertiesV2(&PackedStreamPropertiesV2, &DeviceFileHeader.PackedStreamProperties);
XN_IS_STATUS_OK(nRetVal);
}
break;
case 1:
{
XnPackedStreamPropertiesV1 PackedStreamPropertiesV1;
nRetVal = m_pInputStream->ReadData((XnUChar*)&PackedStreamPropertiesV1, sizeof(XnPackedStreamPropertiesV1));
XN_IS_STATUS_OK(nRetVal);
nRetVal = XnIOAdjustPackedStreamPropertiesV1(&PackedStreamPropertiesV1, &DeviceFileHeader.PackedStreamProperties);
XN_IS_STATUS_OK(nRetVal);
}
break;
default:
return XN_STATUS_IO_INVALID_STREAM_HEADER;
}
// Save the stream properties into the private data (but keep the original stream flags)
nStreamFlags = m_pBCData->StreamProperties.nStreamFlags;
xnOSMemCopy(&m_pBCData->StreamProperties, &DeviceFileHeader.StreamProperties, sizeof(XnStreamProperties));
m_pBCData->StreamProperties.nStreamFlags = nStreamFlags;
if (m_pBCData->StreamProperties.Shift2DepthData.bShift2DepthData)
{
m_pBCData->StreamProperties.Shift2DepthData.nMaxDepthValue = 10000;
m_pBCData->StreamProperties.nDepthMaxValue = 10000;
}
// Save the packed stream properties into the private data
xnOSMemCopy(&m_pBCData->PackedStreamProperties, &DeviceFileHeader.PackedStreamProperties, sizeof(XnPackedStreamProperties));
XnUInt32 nBufferSize = BCCalculatePackedBufferSize();
if (nBufferSize != m_pBCData->nPackedStreamBufferSize)
{
xnOSFreeAligned(m_pBCData->pPackedStreamBuffer);
XN_VALIDATE_ALIGNED_CALLOC(m_pBCData->pPackedStreamBuffer, XnUChar, nBufferSize, XN_DEFAULT_MEM_ALIGN);
m_pBCData->nPackedStreamBufferSize = nBufferSize;
}
nRetVal = ConvertStreamPropertiesToPropertySet(&m_pBCData->StreamProperties, &m_pBCData->PackedStreamProperties, pSet);
XN_IS_STATUS_OK(nRetVal);
// All is good...
return (XN_STATUS_OK);
}
