unsigned int mitk::IGTLDevice::SendMessagePrivate(igtl::MessageBase::Pointer msg,
igtl::Socket::Pointer socket)
{
//check the input message
if ( msg.IsNull() )
{
MITK_ERROR("IGTLDevice") << "Could not send message because message is not "
"valid. Please check.";
return false;
}
// add the name of this device to the message
msg->SetDeviceName(this->GetName().c_str());
// Pack (serialize) and send
msg->Pack();
// measure the time
AddTrackingMeasurements(5, msg, 0);
int sendSuccess = socket->Send(msg->GetPackPointer(), msg->GetPackSize());
if (sendSuccess)
{
this->InvokeEvent(MessageSentEvent());
return IGTL_STATUS_OK;
}
else
{
return IGTL_STATUS_UNKNOWN_ERROR;
}
}
void mitk::IGTLServer::Send()
{
igtl::MessageBase::Pointer curMessage;
//get the latest message from the queue
curMessage = this->m_SendQueue->PullMessage();
// there is no message => return
if (curMessage.IsNull())
return;
AddTrackingMeasurements(4, curMessage, 0);
//the server can be connected with several clients, therefore it has to check
//all registered clients
//sending a message to all registered clients might not be the best solution,
//it could be better to store the client together with the requested type. Then
//the data would be send to the appropriate client and to noone else.
//(I know it is no excuse but PLUS is doing exactly the same, they broadcast
//everything)
m_SentListMutex->Lock();
SocketListIteratorType it;
auto it_end =
this->m_RegisteredClients.end();
for (it = this->m_RegisteredClients.begin(); it != it_end; ++it)
{
//maybe there should be a check here if the current socket is still active
this->SendMessagePrivate(curMessage.GetPointer(), *it);
MITK_DEBUG("IGTLServer") << "Sent IGTL Message";
}
m_SentListMutex->Unlock();
}
igtl::MessageBase::Pointer mitk::IGTLDevice::GetNextMessage()
{
//copy the next message into the given msg
igtl::MessageBase::Pointer msg = this->m_ReceiveQueue->PullMessage();
if (msg.IsNotNull())
{
AddTrackingMeasurements(8, msg, 0);
unsigned int seconds = 0;
unsigned int frac = 0;
msg->GetTimeStamp(&seconds, &frac);
//std::cout << "8: s: " << seconds << " f: " << frac << std::endl;
}
return msg;
}
void mitk::IGTLDevice::SendMessage(igtl::MessageBase::Pointer msg)
{
AddTrackingMeasurements(3, msg, 0);
//add the message to the queue
m_SendQueue->PushMessage(msg);
}
unsigned int mitk::IGTLDevice::ReceivePrivate(igtl::Socket* socket)
{
// Create a message buffer to receive header
igtl::MessageHeader::Pointer headerMsg;
headerMsg = igtl::MessageHeader::New();
// Initialize receive buffer
headerMsg->InitPack();
// Receive generic header from the socket
int r =
socket->Receive(headerMsg->GetPackPointer(), headerMsg->GetPackSize(), 0);
//MITK_INFO << "Server received r = " << r;
if (r == 0) //connection error
{
// an error was received, therefor the communication with this socket
// must be stoppedy
return IGTL_STATUS_NOT_PRESENT;
}
else if (r == -1 ) //timeout
{
// a timeout was received, this is no error state, thus, do nothing
return IGTL_STATUS_TIME_OUT;
}
else if (r == headerMsg->GetPackSize())
{
// Deserialize the header and check the CRC
// ERROR HERE: This probably means the header data is corrupted...
int crcCheck = headerMsg->Unpack(1);
if (crcCheck & igtl::MessageHeader::UNPACK_HEADER)
{
// Allocate a time stamp
igtl::TimeStamp::Pointer ts;
ts = igtl::TimeStamp::New();
// Get time stamp
igtlUint32 sec;
igtlUint32 nanosec;
headerMsg->GetTimeStamp(ts);
ts->GetTimeStamp(&sec, &nanosec);
// std::cerr << "Time stamp: "
// << sec << "."
// << nanosec << std::endl;
// std::cerr << "Dev type and name: " << headerMsg->GetDeviceType() << " "
// << headerMsg->GetDeviceName() << std::endl;
// headerMsg->Print(std::cout);
//check the type of the received message
//if it is a GET_, STP_ or RTS_ command push it into the command queue
//otherwise continue reading the whole message from the socket
const char* curDevType = headerMsg->GetDeviceType();
if ( std::strstr( curDevType, "GET_" ) != nullptr ||
std::strstr( curDevType, "STP_" ) != nullptr ||
std::strstr( curDevType, "RTS_" ) != nullptr)
{
this->m_CommandQueue->PushMessage(headerMsg);
this->InvokeEvent(CommandReceivedEvent());
return IGTL_STATUS_OK;
}
//Create a message according to the header message
igtl::MessageBase::Pointer curMessage;
curMessage = m_MessageFactory->CreateInstance(headerMsg);
//check if the curMessage is created properly, if not the message type is
//not supported and the message has to be skipped
if ( curMessage.IsNull() )
{
socket->Skip(headerMsg->GetBodySizeToRead(), 0);
// MITK_ERROR("IGTLDevice") << "The received type is not supported. Please "
// "add it to the message factory.";
return IGTL_STATUS_NOT_FOUND;
}
//insert the header to the message and allocate the pack
curMessage->SetMessageHeader(headerMsg);
curMessage->AllocatePack();
// Receive transform data from the socket
int receiveCheck = 0;
receiveCheck = socket->Receive(curMessage->GetPackBodyPointer(),
curMessage->GetPackBodySize(), m_ReadFully);
// measure the time
const long long timeStamp6 = std::chrono::high_resolution_clock::now().time_since_epoch().count();
if ( receiveCheck > 0 )
{
int c = curMessage->Unpack(1);
if ( !(c & igtl::MessageHeader::UNPACK_BODY) )
{
return IGTL_STATUS_CHECKSUM_ERROR;
}
//save timestamp 6 now because we know the index
AddTrackingMeasurements(6, curMessage, timeStamp6);
//check the type of the received message
//if it is a command push it into the command queue
//otherwise into the normal receive queue
//STP_ commands are handled here because they implemented additional
//member variables that are not stored in the header message
if ( std::strstr( curDevType, "STT_" ) != nullptr )
{
this->m_CommandQueue->PushMessage(curMessage);
this->InvokeEvent(CommandReceivedEvent());
}
else
{
AddTrackingMeasurements(7, curMessage, timeStamp6);
this->m_ReceiveQueue->PushMessage(curMessage);
this->InvokeEvent(MessageReceivedEvent());
}
return IGTL_STATUS_OK;
}
else
{
MITK_ERROR("IGTLDevice") << "Received a valid header but could not "
<< "read the whole message.";
return IGTL_STATUS_UNKNOWN_ERROR;
}
}
else
{
//CRC check failed
MITK_ERROR << "CRC Check failed";
return IGTL_STATUS_CHECKSUM_ERROR;
}
}
else
{
//Message size information and actual data size don't match.
//this state is not suppossed to be reached, return unknown error
MITK_ERROR << "IGTL status unknown";
return IGTL_STATUS_UNKNOWN_ERROR;
}
}
