VRDeviceServer::~VRDeviceServer(void)
{
/* Lock client list: */
{
Threads::Mutex::Lock clientListLock(clientListMutex);
/* Stop connection initiating thread: */
listenThread.cancel();
listenThread.join();
/* Disconnect all clients: */
deviceManager->lockState();
for(ClientList::iterator c**t=clientList.begin();c**t!=clientList.end();++c**t)
{
/* Stop client communication thread: */
(*c**t)->communicationThread.cancel();
(*c**t)->communicationThread.join();
/* Delete client data object (closing TCP socket): */
delete *c**t;
}
deviceManager->unlockState();
/* Stop VR devices: */
if(numActiveClients>0)
deviceManager->stop();
}
/* Disable tracker update notification: */
deviceManager->disableTrackerUpdateNotification();
}
void* KinectServer::listeningThreadMethod(void)
{
Threads::Thread::setCancelState(Threads::Thread::CANCEL_ENABLE);
// Threads::Thread::setCancelType(Threads::Thread::CANCEL_ASYNCHRONOUS);
while(true)
{
/* Wait for the next incoming connection: */
Comm::TCPPipe* newClientSocket=0;
try
{
#ifdef VERBOSE
std::cout<<"KinectServer: Waiting for client connection"<<std::endl<<std::flush;
#endif
newClientSocket=new Comm::TCPPipe(listeningSocket);
#ifdef VERBOSE
std::cout<<"KinectServer: Connecting new client from host "<<newClientSocket->getPeerHostName()<<", port "<<newClientSocket->getPeerPortId()<<std::endl<<std::flush;
#endif
}
catch(std::runtime_error err)
{
std::cerr<<"KinectServer: Caught exception "<<err.what()<<" while waiting for new client connection"<<std::endl;
}
try
{
/* Send stream initialization states to the new client: */
#ifdef VERBOSE
std::cout<<"KinectServer: Sending stream headers to new client"<<std::endl<<std::flush;
#endif
newClientSocket->write<unsigned int>(0x12345678U);
newClientSocket->write<unsigned int>(numCameras);
for(unsigned i=0; i<numCameras; ++i)
cameraStates[i]->writeHeaders(*newClientSocket);
newClientSocket->flush();
/* Lock the client list and append the new client: */
#ifdef VERBOSE
std::cout<<"KinectServer: Adding new client to list of clients"<<std::endl<<std::flush;
#endif
{
Threads::Mutex::Lock clientListLock(clientListMutex);
clients.push_back(newClientSocket);
}
}
catch(std::runtime_error err)
{
std::cerr<<"KinectServer: Disconnecting new client due to exception "<<err.what()<<std::endl<<std::flush;
delete newClientSocket;
}
catch(...)
{
std::cerr<<"KinectServer: Disconnecting new client due to spurious exception; terminating"<<std::endl<<std::flush;
delete newClientSocket;
throw;
}
}
return 0;
}
void* VRDeviceServer::listenThreadMethod(void)
{
/* Enable immediate cancellation of this thread: */
Threads::Thread::setCancelState(Threads::Thread::CANCEL_ENABLE);
Threads::Thread::setCancelType(Threads::Thread::CANCEL_ASYNCHRONOUS);
while(true)
{
/* Wait for the next incoming connection: */
#ifdef VERBOSE
printf("VRDeviceServer: Waiting for client connection\n");
fflush(stdout);
#endif
Comm::TCPSocket clientSocket=listenSocket.accept();
/* Connect the new client: */
#ifdef VERBOSE
try
{
printf("VRDeviceServer: Connecting new client from %s, port %d\n",clientSocket.getPeerHostname().c_str(),clientSocket.getPeerPortId());
fflush(stdout);
}
catch(std::runtime_error error)
{
printf("VRDeviceServer: Connecting new client from %s, port %d\n",clientSocket.getPeerAddress().c_str(),clientSocket.getPeerPortId());
fflush(stdout);
}
#endif
{
Threads::Mutex::Lock clientListLock(clientListMutex);
ClientData* newClient=new ClientData(clientSocket);
clientList.push_back(newClient);
newClient->communicationThread.start(this,&VRDeviceServer::clientCommunicationThreadMethod,newClient);
}
}
return 0;
}
void* VRDeviceServer::clientCommunicationThreadMethod(VRDeviceServer::ClientData* clientData)
{
/* Enable immediate cancellation of this thread: */
Threads::Thread::setCancelState(Threads::Thread::CANCEL_ENABLE);
Threads::Thread::setCancelType(Threads::Thread::CANCEL_ASYNCHRONOUS);
Vrui::VRDevicePipe& pipe=clientData->pipe;
enum State
{
START,CONNECTED,ACTIVE,STREAMING,FINISH
};
try
{
/* Execute client communication protocol state machine: */
State state=START; // Current client state
while(state!=FINISH)
{
if(clientData->streaming)
{
/* Wait for next tracker update: */
trackerUpdateCompleteCond.wait();
deviceManager->lockState();
try
{
/* Send packet reply message: */
pipe.writeMessage(Vrui::VRDevicePipe::PACKET_REPLY);
/* Send server state: */
pipe.writeState(deviceManager->getState());
}
catch(std::runtime_error err)
{
/* Unlock the device manager's state and throw the exception again: */
deviceManager->unlockState();
throw;
}
deviceManager->unlockState();
/* Check for messages: */
if(pipe.getSocket().waitForData(0,0,false))
{
switch(pipe.readMessage())
{
case Vrui::VRDevicePipe::PACKET_REQUEST:
/* Ignore message: */
break;
case Vrui::VRDevicePipe::STOPSTREAM_REQUEST:
/* Send stopstream reply message: */
pipe.writeMessage(Vrui::VRDevicePipe::STOPSTREAM_REPLY);
/* Go to active state: */
clientData->streaming=false;
state=ACTIVE;
break;
default:
state=FINISH;
}
}
}
else
{
Vrui::VRDevicePipe::MessageIdType message=pipe.readMessage();
switch(state)
{
case START:
switch(message)
{
case Vrui::VRDevicePipe::CONNECT_REQUEST:
/* Send connect reply message: */
pipe.writeMessage(Vrui::VRDevicePipe::CONNECT_REPLY);
/* Send server layout: */
pipe.writeLayout(deviceManager->getState());
/* Go to connected state: */
state=CONNECTED;
break;
default:
state=FINISH;
}
break;
case CONNECTED:
switch(message)
{
case Vrui::VRDevicePipe::ACTIVATE_REQUEST:
{
Threads::Mutex::Lock clientListLock(clientListMutex);
/* Start VR devices if this is the first active client: */
if(numActiveClients==0)
deviceManager->start();
/* Activate client: */
++numActiveClients;
}
clientData->active=true;
/* Go to active state: */
state=ACTIVE;
break;
default:
state=FINISH;
}
break;
case ACTIVE:
switch(message)
{
case Vrui::VRDevicePipe::PACKET_REQUEST:
deviceManager->lockState();
try
{
/* Send packet reply message: */
pipe.writeMessage(Vrui::VRDevicePipe::PACKET_REPLY);
/* Send server state: */
pipe.writeState(deviceManager->getState());
}
catch(std::runtime_error err)
{
/* Unlock the device manager's state and throw the exception again: */
deviceManager->unlockState();
throw;
}
deviceManager->unlockState();
break;
case Vrui::VRDevicePipe::STARTSTREAM_REQUEST:
deviceManager->lockState();
try
{
/* Send packet reply message: */
pipe.writeMessage(Vrui::VRDevicePipe::PACKET_REPLY);
/* Send server state: */
pipe.writeState(deviceManager->getState());
}
catch(std::runtime_error err)
{
/* Unlock the device manager's state and throw the exception again: */
deviceManager->unlockState();
throw;
}
deviceManager->unlockState();
/* Go to streaming state: */
clientData->streaming=true;
state=STREAMING;
break;
case Vrui::VRDevicePipe::DEACTIVATE_REQUEST:
/* Deactivate client: */
clientData->active=false;
{
Threads::Mutex::Lock clientListLock(clientListMutex);
--numActiveClients;
/* Stop VR devices if this was the last active client: */
if(numActiveClients==0)
deviceManager->stop();
}
/* Go to connected state: */
state=CONNECTED;
break;
default:
state=FINISH;
}
break;
default:
/* Just to make g++ happy... */
;
}
}
}
}
catch(std::runtime_error err)
{
/* Print error message to stderr, but ignore exception otherwise: */
fprintf(stderr,"VRDeviceServer: Terminating client connection due to exception\n %s\n",err.what());
fflush(stderr);
}
catch(...)
{
/* Just ignore the exception: */
fprintf(stderr,"VRDeviceServer: Terminating client connection due to spurious exception\n");
fflush(stderr);
}
/* Cleanly deactivate client: */
{
Threads::Mutex::Lock clientListLock(clientListMutex);
if(clientData->streaming)
{
/* Leave streaming mode: */
clientData->streaming=false;
}
if(clientData->active)
{
/* Deactivate client: */
clientData->active=false;
--numActiveClients;
/* Stop VR devices if this was the last active client: */
if(numActiveClients==0)
deviceManager->stop();
}
/* Remove client from list: */
ClientList::iterator c**t;
for(c**t=clientList.begin();c**t!=clientList.end()&&*c**t!=clientData;++c**t)
;
clientList.erase(c**t);
/* Disconnect client: */
delete clientData;
}
/* Terminate: */
#ifdef VERBOSE
printf("VRDeviceServer: Disconnected client\n");
fflush(stdout);
#endif
return 0;
}
void* VRDeviceServer::streamingThreadMethod(void)
{
/* Enable immediate cancellation of this thread: */
Threads::Thread::setCancelState(Threads::Thread::CANCEL_ENABLE);
Threads::Thread::setCancelType(Threads::Thread::CANCEL_ASYNCHRONOUS);
while(true)
{
/* Wait for the next update notification from the device manager: */
trackerUpdateCompleteCond.wait();
/* Lock client list: */
{
Threads::Mutex::Lock clientListLock(clientListMutex);
/* Lock the device manager's current state: */
deviceManager->lockState();
/* Iterate through all clients in streaming mode: */
std::vector<ClientList::iterator> deadClients;
for(ClientList::iterator c**t=clientList.begin();c**t!=clientList.end();++c**t)
{
/* Lock the client's pipe: */
Threads::Mutex::Lock clientPipeLock((*c**t)->pipeMutex);
if((*c**t)->streaming)
{
try
{
/* Send packet reply message: */
(*c**t)->pipe.writeMessage(Vrui::VRDevicePipe::PACKET_REPLY);
/* Send server state: */
deviceManager->getState().write((*c**t)->pipe);
(*c**t)->pipe.flush();
}
catch(std::runtime_error err)
{
/* Print error message to stderr and mark client for termination: */
fprintf(stderr,"VRDeviceServer: Terminating client connection due to exception\n %s\n",err.what());
fflush(stderr);
deadClients.push_back(c**t);
}
catch(...)
{
/* Print error message to stderr and mark client for termination: */
fprintf(stderr,"VRDeviceServer: Terminating client connection due to spurious exception\n");
fflush(stderr);
deadClients.push_back(c**t);
}
}
}
/* Unlock the device manager's state: */
deviceManager->unlockState();
/* Disconnect all dead clients: */
for(std::vector<ClientList::iterator>::iterator dcIt=deadClients.begin();dcIt!=deadClients.end();++dcIt)
{
/* Stop client communication thread: */
(**dcIt)->communicationThread.cancel();
(**dcIt)->communicationThread.join();
/* Cleanly deactivate client: */
if((**dcIt)->streaming)
{
/* Leave streaming mode: */
(**dcIt)->streaming=false;
}
if((**dcIt)->active)
{
/* Deactivate client: */
(**dcIt)->active=false;
--numActiveClients;
/* Stop VR devices if this was the last active client: */
if(numActiveClients==0)
deviceManager->stop();
}
delete **dcIt;
/* Remove client from list: */
clientList.erase(*dcIt);
}
}
}
return 0;
}
void* KinectServer::streamingThreadMethod(void)
{
Threads::Thread::setCancelState(Threads::Thread::CANCEL_ENABLE);
Threads::Thread::setCancelType(Threads::Thread::CANCEL_DEFERRED);
#ifdef VERBOSE2
std::cout<<"Meta frame "<<metaFrameIndex;
#endif
while(true)
{
while(numMissingDepthFrames>0||numMissingColorFrames>0)
{
/* Find the next missing frame that has just become available: */
bool foundFrame=false;
for(unsigned int i=0; !foundFrame&&i<numCameras; ++i)
{
if(!cameraStates[i]->hasSentColorFrame&&cameraStates[i]->colorFrames.lockNewValue())
{
#ifdef VERBOSE2
std::cout<<" color "<<i<<", "<<cameraStates[i]->colorFrames.getLockedValue().index<<", "<<cameraStates[i]->colorFrames.getLockedValue().timeStamp<<';';
#endif
/* Send the camera's new color frame to all connected clients: */
{
Threads::Mutex::Lock clientListLock(clientListMutex);
unsigned int numClients=clients.size();
for(unsigned int j=0; j<numClients; ++j)
{
try
{
if(clients[j]->waitForData(Misc::Time(0,0)))
{
/* Read the disconnect request: */
clients[j]->read<unsigned int>();
/* Disconnect the client: */
#ifdef VERBOSE
std::cerr<<"Disconnecting client from "<<clients[j]->getPeerHostName()<<", port "<<clients[j]->getPeerPortId()<<std::endl;
#endif
delete clients[j];
clients.erase(clients.begin()+j);
--numClients;
--j;
}
else
{
#ifdef VVERBOSE
std::cout<<metaFrameIndex<<", "<<i*2+0<<", "<<cameraStates[i]->colorFrames.getLockedValue().timeStamp<<std::endl;
#endif
/* Write the meta frame index and frame identifier: */
clients[j]->write<unsigned int>(metaFrameIndex);
clients[j]->write<unsigned int>(i*2+0);
/* Write the compressed color frame: */
cameraStates[i]->colorFrames.getLockedValue().data.writeToSink(*clients[j]);
clients[j]->flush();
}
}
catch(std::runtime_error err)
{
std::cerr<<"Disconnecting client from "<<clients[j]->getPeerHostName()<<", port "<<clients[j]->getPeerPortId()<<" due to exception "<<err.what()<<std::endl;
delete clients[j];
clients.erase(clients.begin()+j);
--numClients;
--j;
}
catch(...)
{
std::cerr<<"Disconnecting client from "<<clients[j]->getPeerHostName()<<", port "<<clients[j]->getPeerPortId()<<" due to spurious exception; terminating"<<std::endl;
delete clients[j];
clients.erase(clients.begin()+j);
--numClients;
--j;
throw;
}
}
}
cameraStates[i]->hasSentColorFrame=true;
--numMissingColorFrames;
foundFrame=true;
}
if(!cameraStates[i]->hasSentDepthFrame&&cameraStates[i]->depthFrames.lockNewValue())
{
#ifdef VERBOSE2
std::cout<<" depth "<<i<<", "<<cameraStates[i]->depthFrames.getLockedValue().index<<", "<<cameraStates[i]->depthFrames.getLockedValue().timeStamp<<';';
#endif
/* Send the camera's new depth frame to all connected clients: */
{
Threads::Mutex::Lock clientListLock(clientListMutex);
unsigned int numClients=clients.size();
for(unsigned int j=0; j<numClients; ++j)
{
try
{
/* Check if the client sent a disconnect request: */
if(clients[j]->waitForData(Misc::Time(0,0)))
{
/* Read the disconnect request: */
clients[j]->read<unsigned int>();
/* Disconnect the client: */
#ifdef VERBOSE
std::cerr<<"Disconnecting client from "<<clients[j]->getPeerHostName()<<", port "<<clients[j]->getPeerPortId()<<std::endl;
#endif
delete clients[j];
clients.erase(clients.begin()+j);
--numClients;
--j;
}
else
{
#ifdef VVERBOSE
std::cout<<metaFrameIndex<<", "<<i*2+1<<", "<<cameraStates[i]->depthFrames.getLockedValue().timeStamp<<std::endl;
#endif
/* Write the meta frame index and frame identifier: */
clients[j]->write<unsigned int>(metaFrameIndex);
clients[j]->write<unsigned int>(i*2+1);
/* Write the compressed depth frame: */
cameraStates[i]->depthFrames.getLockedValue().data.writeToSink(*clients[j]);
clients[j]->flush();
}
}
catch(std::runtime_error err)
{
std::cerr<<"Disconnecting client from "<<clients[j]->getPeerHostName()<<", port "<<clients[j]->getPeerPortId()<<" due to exception "<<err.what()<<std::endl;
delete clients[j];
clients.erase(clients.begin()+j);
--numClients;
--j;
}
catch(...)
{
std::cerr<<"Disconnecting client from "<<clients[j]->getPeerHostName()<<", port "<<clients[j]->getPeerPortId()<<" due to spurious exception; terminating"<<std::endl;
delete clients[j];
clients.erase(clients.begin()+j);
--numClients;
--j;
throw;
}
}
}
cameraStates[i]->hasSentDepthFrame=true;
--numMissingDepthFrames;
foundFrame=true;
}
}
if(!foundFrame)
{
/* No frames ready; sleep until something becomes available: */
newFrameCond.wait();
}
}
/* Start a new meta-frame: */
++metaFrameIndex;
for(unsigned int i=0; i<numCameras; ++i)
{
cameraStates[i]->hasSentColorFrame=false;
cameraStates[i]->hasSentDepthFrame=false;
}
numMissingColorFrames=numCameras;
numMissingDepthFrames=numCameras;
#ifdef VERBOSE2
std::cout<<std::endl;
std::cout<<"Meta frame "<<metaFrameIndex;
#endif
}
return 0;
}