diff --git a/Modules/IGT/Algorithms/mitkNavigationDataToIGTLMessageFilter.cpp b/Modules/IGT/Algorithms/mitkNavigationDataToIGTLMessageFilter.cpp
index 8e6978659a..c930596e85 100644
--- a/Modules/IGT/Algorithms/mitkNavigationDataToIGTLMessageFilter.cpp
+++ b/Modules/IGT/Algorithms/mitkNavigationDataToIGTLMessageFilter.cpp
@@ -1,355 +1,348 @@
/*===================================================================
The Medical Imaging Interaction Toolkit (MITK)
Copyright (c) German Cancer Research Center,
Division of Medical and Biological Informatics.
All rights reserved.
This software is distributed WITHOUT ANY WARRANTY; without
even the implied warranty of MERCHANTABILITY or FITNESS FOR
A PARTICULAR PURPOSE.
See LICENSE.txt or http://www.mitk.org for details.
===================================================================*/
#include "mitkNavigationDataToIGTLMessageFilter.h"
#include "igtlQuaternionTrackingDataMessage.h"
#include "igtlTrackingDataMessage.h"
#include "igtlTransformMessage.h"
#include "igtlPositionMessage.h"
#include <mitkInteractionConst.h>
#include <itksys/SystemTools.hxx>
mitk::NavigationDataToIGTLMessageFilter::NavigationDataToIGTLMessageFilter()
{
mitk::IGTLMessage::Pointer output = mitk::IGTLMessage::New();
this->SetNumberOfRequiredOutputs(1);
this->SetNthOutput(0, output.GetPointer());
this->SetNumberOfRequiredInputs(1);
// m_OperationMode = Mode3D;
m_CurrentTimeStep = 0;
// m_RingBufferSize = 50; //the default ring buffer size
// m_NumberForMean = 100;
}
mitk::NavigationDataToIGTLMessageFilter::~NavigationDataToIGTLMessageFilter()
{
}
void mitk::NavigationDataToIGTLMessageFilter::GenerateData()
{
switch (m_OperationMode)
{
case ModeSendQTDataMsg:
this->GenerateDataModeSendQTDataMsg();
break;
case ModeSendTDataMsg:
this->GenerateDataModeSendTDataMsg();
break;
case ModeSendQTransMsg:
this->GenerateDataModeSendQTransMsg();
break;
case ModeSendTransMsg:
this->GenerateDataModeSendTransMsg();
break;
default:
break;
}
- igtl::MessageBase::Pointer curMessage = this->GetOutput()->GetMessage();
- if (dynamic_cast<igtl::TrackingDataMessage*>(curMessage.GetPointer()) != nullptr)
- {
- igtl::TrackingDataMessage* tdMsg =
- (igtl::TrackingDataMessage*)(curMessage.GetPointer());
- }
-
}
void mitk::NavigationDataToIGTLMessageFilter::SetInput(const NavigationData* nd)
{
// Process object is not const-correct so the const_cast is required here
this->ProcessObject::SetNthInput(0, const_cast<NavigationData*>(nd));
this->CreateOutputsForAllInputs();
}
void mitk::NavigationDataToIGTLMessageFilter::SetInput(unsigned int idx, const NavigationData* nd)
{
// Process object is not const-correct so the const_cast is required here
this->ProcessObject::SetNthInput(idx, const_cast<NavigationData*>(nd));
this->CreateOutputsForAllInputs();
}
const mitk::NavigationData* mitk::NavigationDataToIGTLMessageFilter::GetInput(void)
{
if (this->GetNumberOfInputs() < 1)
return NULL;
return static_cast<const NavigationData*>(this->ProcessObject::GetInput(0));
}
const mitk::NavigationData* mitk::NavigationDataToIGTLMessageFilter::GetInput(unsigned int idx)
{
if (this->GetNumberOfInputs() < 1)
return NULL;
return static_cast<const NavigationData*>(this->ProcessObject::GetInput(idx));
}
void mitk::NavigationDataToIGTLMessageFilter::CreateOutputsForAllInputs()
{
switch (m_OperationMode)
{
case ModeSendQTDataMsg:
// create one message output for all navigation data inputs
this->SetNumberOfIndexedOutputs(this->GetNumberOfIndexedInputs());
// set the type for this filter
this->SetType("QTDATA");
break;
case ModeSendTDataMsg:
// create one message output for all navigation data inputs
this->SetNumberOfIndexedOutputs(this->GetNumberOfIndexedInputs());
// set the type for this filter
this->SetType("TDATA");
break;
case ModeSendQTransMsg:
// create one message output for all navigation data input together
this->SetNumberOfIndexedOutputs(this->GetNumberOfIndexedInputs());
// set the type for this filter
this->SetType("POSITION");
break;
case ModeSendTransMsg:
// create one message output for all navigation data input together
this->SetNumberOfIndexedOutputs(this->GetNumberOfIndexedInputs());
// set the type for this filter
this->SetType("TRANS");
break;
default:
break;
}
for (unsigned int idx = 0; idx < this->GetNumberOfIndexedOutputs(); ++idx)
{
if (this->GetOutput(idx) == NULL)
{
DataObjectPointer newOutput = this->MakeOutput(idx);
this->SetNthOutput(idx, newOutput);
}
this->Modified();
}
}
void ConvertAffineTransformationIntoIGTLMatrix(mitk::AffineTransform3D* trans,
igtl::Matrix4x4 igtlTransform)
{
const mitk::AffineTransform3D::MatrixType& matrix = trans->GetMatrix();
mitk::Vector3D position = trans->GetOffset();
//copy the data into a matrix type that igtl understands
for (unsigned int r = 0; r < 3; r++)
{
for (unsigned int c = 0; c < 3; c++)
{
igtlTransform[r][c] = matrix(r, c);
}
igtlTransform[r][3] = position[r];
}
for (unsigned int c = 0; c < 3; c++)
{
igtlTransform[3][c] = 0.0;
}
igtlTransform[3][3] = 1.0;
}
void mitk::NavigationDataToIGTLMessageFilter::GenerateDataModeSendQTransMsg()
{
// for each output message
for (unsigned int i = 0; i < this->GetNumberOfIndexedInputs(); ++i)
{
mitk::IGTLMessage* output = this->GetOutput(i);
assert(output);
const mitk::NavigationData* input = this->GetInput(i);
assert(input);
// do not add navigation data to message if input is invalid
if (input->IsDataValid() == false)
continue;
//get the navigation data components
mitk::NavigationData::PositionType pos = input->GetPosition();
mitk::NavigationData::OrientationType ori = input->GetOrientation();
//insert this information into the message
igtl::PositionMessage::Pointer posMsg = igtl::PositionMessage::New();
posMsg->SetPosition(pos[0], pos[1], pos[2]);
posMsg->SetQuaternion(ori[0], ori[1], ori[2], ori[3]);
igtl::TimeStamp::Pointer timestamp = ConvertToIGTLTimeStamp(input->GetIGTTimeStamp());
posMsg->SetTimeStamp(timestamp);
posMsg->SetDeviceName(input->GetName());
posMsg->Pack();
//add the igtl message to the mitk::IGTLMessage
output->SetMessage(posMsg.GetPointer());
}
}
void mitk::NavigationDataToIGTLMessageFilter::GenerateDataModeSendTransMsg()
{
// for each output message
for (unsigned int i = 0; i < this->GetNumberOfIndexedInputs(); ++i)
{
mitk::IGTLMessage* output = this->GetOutput(i);
assert(output);
const mitk::NavigationData* input = this->GetInput(i);
assert(input);
// do not add navigation data to message if input is invalid
if (input->IsDataValid() == false)
continue;
//get the navigation data components
mitk::AffineTransform3D::Pointer transform = input->GetAffineTransform3D();
mitk::NavigationData::PositionType position = transform->GetOffset();
//convert the transform into a igtl type
igtl::Matrix4x4 igtlTransform;
ConvertAffineTransformationIntoIGTLMatrix(transform, igtlTransform);
//insert this information into the message
igtl::TransformMessage::Pointer transMsg = igtl::TransformMessage::New();
transMsg->SetMatrix(igtlTransform);
transMsg->SetPosition(position[0], position[1], position[2]);
igtl::TimeStamp::Pointer timestamp = ConvertToIGTLTimeStamp(input->GetIGTTimeStamp());
transMsg->SetTimeStamp(timestamp);
transMsg->SetDeviceName(input->GetName());
transMsg->Pack();
//add the igtl message to the mitk::IGTLMessage
output->SetMessage(transMsg.GetPointer());
}
}
igtl::TimeStamp::Pointer mitk::NavigationDataToIGTLMessageFilter::ConvertToIGTLTimeStamp(double IGTTimeStamp)
{
igtl::TimeStamp::Pointer timestamp = igtl::TimeStamp::New();
timestamp->SetTime(IGTTimeStamp / 1000, (int)(IGTTimeStamp) % 1000);
return timestamp;
}
void mitk::NavigationDataToIGTLMessageFilter::GenerateDataModeSendQTDataMsg()
{
mitk::IGTLMessage* output = this->GetOutput();
assert(output);
//create a output igtl message
igtl::QuaternionTrackingDataMessage::Pointer qtdMsg =
igtl::QuaternionTrackingDataMessage::New();
mitk::NavigationData::PositionType pos;
mitk::NavigationData::OrientationType ori;
for (unsigned int index = 0; index < this->GetNumberOfIndexedInputs(); index++)
{
const mitk::NavigationData* nd = GetInput(index);
assert(nd);
//get the navigation data components
pos = nd->GetPosition();
ori = nd->GetOrientation();
//insert the information into the tracking element
igtl::QuaternionTrackingDataElement::Pointer tde =
igtl::QuaternionTrackingDataElement::New();
tde->SetPosition(pos[0], pos[1], pos[2]);
tde->SetQuaternion(ori[0], ori[1], ori[2], ori[3]);
tde->SetName(nd->GetName());
//insert this element into the tracking data message
qtdMsg->AddQuaternionTrackingDataElement(tde);
MITK_INFO << ConvertToIGTLTimeStamp(nd->GetIGTTimeStamp());
}
qtdMsg->Pack();
//add the igtl message to the mitk::IGTLMessage
output->SetMessage(qtdMsg.GetPointer());
}
void mitk::NavigationDataToIGTLMessageFilter::GenerateDataModeSendTDataMsg()
{
bool isValidData = true;
mitk::IGTLMessage* output = this->GetOutput();
assert(output);
//create a output igtl message
igtl::TrackingDataMessage::Pointer tdMsg = igtl::TrackingDataMessage::New();
mitk::AffineTransform3D::Pointer transform;
Vector3D position;
igtl::Matrix4x4 igtlTransform;
vnl_matrix_fixed<ScalarType, 3, 3> rotationMatrix;
vnl_matrix_fixed<ScalarType, 3, 3> rotationMatrixTransposed;
for (unsigned int index = 0; index < this->GetNumberOfIndexedInputs(); index++)
{
const mitk::NavigationData* nd = GetInput(index);
assert(nd);
//create a new tracking element
igtl::TrackingDataElement::Pointer tde = igtl::TrackingDataElement::New();
//get the navigation data components
transform = nd->GetAffineTransform3D();
position = transform->GetOffset();
//check the rotation matrix
rotationMatrix = transform->GetMatrix().GetVnlMatrix();
rotationMatrixTransposed = rotationMatrix.transpose();
// a quadratic matrix is a rotation matrix exactly when determinant is 1
// and transposed is inverse
if (!Equal(1.0, vnl_det(rotationMatrix), 0.1)
|| !((rotationMatrix*rotationMatrixTransposed).is_identity(0.1)))
{
//the rotation matrix is not valid! => invalidate the current element
isValidData = false;
}
//convert the transform into a igtl type
ConvertAffineTransformationIntoIGTLMatrix(transform, igtlTransform);
//fill the tracking element with life
tde->SetMatrix(igtlTransform);
tde->SetPosition(position[0], position[1], position[2]);
std::stringstream name;
name << nd->GetName();
if (name.rdbuf()->in_avail() == 0)
{
name << "TrackingTool" << index;
}
tde->SetName(name.str().c_str());
//insert this element into the tracking data message
tdMsg->AddTrackingDataElement(tde);
//copy the time stamp
igtl::TimeStamp::Pointer timestamp = ConvertToIGTLTimeStamp(nd->GetIGTTimeStamp());
tdMsg->SetTimeStamp(timestamp);
}
tdMsg->Pack();
//add the igtl message to the mitk::IGTLMessage
output->SetMessage(tdMsg.GetPointer());
output->SetDataValid(isValidData);
}
void mitk::NavigationDataToIGTLMessageFilter::SetOperationMode(OperationMode mode)
{
m_OperationMode = mode;
this->Modified();
}
void mitk::NavigationDataToIGTLMessageFilter::ConnectTo(
mitk::NavigationDataSource* UpstreamFilter)
{
for (DataObjectPointerArraySizeType i = 0;
i < UpstreamFilter->GetNumberOfOutputs(); i++)
{
this->SetInput(i, UpstreamFilter->GetOutput(i));
}
}
diff --git a/Modules/OpenIGTLink/mitkIGTLDevice.cpp b/Modules/OpenIGTLink/mitkIGTLDevice.cpp
index 506300dea5..c4c7d7362b 100644
--- a/Modules/OpenIGTLink/mitkIGTLDevice.cpp
+++ b/Modules/OpenIGTLink/mitkIGTLDevice.cpp
@@ -1,567 +1,564 @@
/*===================================================================
The Medical Imaging Interaction Toolkit (MITK)
Copyright (c) German Cancer Research Center,
Division of Medical and Biological Informatics.
All rights reserved.
This software is distributed WITHOUT ANY WARRANTY; without
even the implied warranty of MERCHANTABILITY or FITNESS FOR
A PARTICULAR PURPOSE.
See LICENSE.txt or http://www.mitk.org for details.
===================================================================*/
#include "mitkIGTLDevice.h"
//#include "mitkIGTException.h"
//#include "mitkIGTTimeStamp.h"
#include <itkMutexLockHolder.h>
#include <itksys/SystemTools.hxx>
#include <cstring>
#include <igtlTransformMessage.h>
#include <mitkIGTLMessageCommon.h>
#include <igtl_status.h>
//remove later
#include <igtlTrackingDataMessage.h>
//TODO: Which timeout is acceptable and also needed to transmit image data? Is there a maximum data limit?
static const int SOCKET_SEND_RECEIVE_TIMEOUT_MSEC = 100;
typedef itk::MutexLockHolder<itk::FastMutexLock> MutexLockHolder;
mitk::IGTLDevice::IGTLDevice(bool ReadFully) :
// m_Data(mitk::DeviceDataUnspecified),
m_State(mitk::IGTLDevice::Setup),
m_Name("Unspecified Device"),
m_StopCommunication(false),
m_Hostname("127.0.0.1"),
m_PortNumber(-1),
m_MultiThreader(nullptr), m_SendThreadID(0), m_ReceiveThreadID(0), m_ConnectThreadID(0)
{
m_ReadFully = ReadFully;
m_StopCommunicationMutex = itk::FastMutexLock::New();
m_StateMutex = itk::FastMutexLock::New();
// m_LatestMessageMutex = itk::FastMutexLock::New();
m_SendingFinishedMutex = itk::FastMutexLock::New();
m_ReceivingFinishedMutex = itk::FastMutexLock::New();
m_ConnectingFinishedMutex = itk::FastMutexLock::New();
// execution rights are owned by the application thread at the beginning
m_SendingFinishedMutex->Lock();
m_ReceivingFinishedMutex->Lock();
m_ConnectingFinishedMutex->Lock();
m_MultiThreader = itk::MultiThreader::New();
// m_Data = mitk::DeviceDataUnspecified;
// m_LatestMessage = igtl::MessageBase::New();
m_MessageFactory = mitk::IGTLMessageFactory::New();
m_MessageQueue = mitk::IGTLMessageQueue::New();
}
mitk::IGTLDevice::~IGTLDevice()
{
/* stop communication and disconnect from igtl device */
if (GetState() == Running)
{
this->StopCommunication();
this->CloseConnection();
}
else if (GetState() == Ready)
{
this->CloseConnection();
}
/* cleanup tracking thread */
if (m_MultiThreader.IsNotNull())
{
if ((m_SendThreadID != 0))
{
m_MultiThreader->TerminateThread(m_SendThreadID);
}
if ((m_ReceiveThreadID != 0))
{
m_MultiThreader->TerminateThread(m_ReceiveThreadID);
}
if ((m_ConnectThreadID != 0))
{
m_MultiThreader->TerminateThread(m_ConnectThreadID);
}
}
m_MultiThreader = nullptr;
}
mitk::IGTLDevice::IGTLDeviceState mitk::IGTLDevice::GetState() const
{
MutexLockHolder lock(*m_StateMutex);
return m_State;
}
void mitk::IGTLDevice::SetState(IGTLDeviceState state)
{
itkDebugMacro("setting m_State to " << state);
m_StateMutex->Lock();
// MutexLockHolder lock(*m_StateMutex); // lock and unlock the mutex
if (m_State == state)
{
m_StateMutex->Unlock();
return;
}
m_State = state;
m_StateMutex->Unlock();
this->Modified();
}
bool mitk::IGTLDevice::TestConnection()
{
return true;
}
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;
//MITK_INFO << "Received r = " << r;
if (r == 0) //connection error
{
// an error was received, therefore 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_MessageQueue->PushCommandMessage(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);
if (receiveCheck > 0)
{
int c = curMessage->Unpack(1);
if (!(c & igtl::MessageHeader::UNPACK_BODY))
{
return IGTL_STATUS_CHECKSUM_ERROR;
}
//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_MessageQueue->PushCommandMessage(curMessage);
this->InvokeEvent(CommandReceivedEvent());
}
else
{
if(m_LogMessages)
MITK_INFO << "Received Message: " << mitk::IGTLMessage::New(curMessage)->ToString();
this->m_MessageQueue->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;
}
}
void mitk::IGTLDevice::SendMessage(mitk::IGTLMessage::Pointer msg)
{
m_MessageQueue->PushSendMessage(msg);
}
unsigned int mitk::IGTLDevice::SendMessagePrivate(mitk::IGTLMessage::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;
}
igtl::MessageBase* sendMessage = msg->GetMessage();
- // add the name of this device to the message
- sendMessage->SetDeviceName(this->GetName().c_str());
-
// Pack (serialize) and send
sendMessage->Pack();
int sendSuccess = socket->Send(sendMessage->GetPackPointer(), sendMessage->GetPackSize());
if (sendSuccess)
{
if(m_LogMessages)
MITK_INFO << "Send IGTL message: " << msg->ToString();
this->InvokeEvent(MessageSentEvent());
return IGTL_STATUS_OK;
}
else
{
return IGTL_STATUS_UNKNOWN_ERROR;
}
}
void mitk::IGTLDevice::RunCommunication(void (IGTLDevice::*ComFunction)(void), itk::FastMutexLock* mutex)
{
if (this->GetState() != Running)
return;
try
{
// keep lock until end of scope
MutexLockHolder communicationFinishedLockHolder(*mutex);
// Because m_StopCommunication is used by two threads, access has to be guarded
// by a mutex. To minimize thread locking, a local copy is used here
bool localStopCommunication;
// update the local copy of m_StopCommunication
this->m_StopCommunicationMutex->Lock();
localStopCommunication = this->m_StopCommunication;
this->m_StopCommunicationMutex->Unlock();
while ((this->GetState() == Running) && (localStopCommunication == false))
{
(this->*ComFunction)();
/* Update the local copy of m_StopCommunication */
this->m_StopCommunicationMutex->Lock();
localStopCommunication = m_StopCommunication;
this->m_StopCommunicationMutex->Unlock();
// time to relax, this sets the maximum ever possible framerate to 1000 Hz
itksys::SystemTools::Delay(1);
}
}
catch (...)
{
mutex->Unlock();
this->StopCommunication();
MITK_ERROR("IGTLDevice::RunCommunication") << "Error while communicating. Thread stopped.";
//mitkThrowException(mitk::IGTException) << "Error while communicating. Thread stopped.";
}
// StopCommunication was called, thus the mode should be changed back to Ready now
// that the tracking loop has ended.
//this->SetState(Ready); //this is done elsewhere
MITK_DEBUG("IGTLDevice::RunCommunication") << "Reached end of communication.";
// returning from this function (and ThreadStartCommunication())
// this will end the thread
return;
}
bool mitk::IGTLDevice::StartCommunication()
{
if (this->GetState() != Ready)
return false;
// go to mode Running
this->SetState(Running);
// set a timeout for the sending and receiving
this->m_Socket->SetTimeout(SOCKET_SEND_RECEIVE_TIMEOUT_MSEC);
// update the local copy of m_StopCommunication
this->m_StopCommunicationMutex->Lock();
this->m_StopCommunication = false;
this->m_StopCommunicationMutex->Unlock();
// transfer the execution rights to tracking thread
m_SendingFinishedMutex->Unlock();
m_ReceivingFinishedMutex->Unlock();
m_ConnectingFinishedMutex->Unlock();
// start new threads that execute the communication
m_SendThreadID =
m_MultiThreader->SpawnThread(this->ThreadStartSending, this);
m_ReceiveThreadID =
m_MultiThreader->SpawnThread(this->ThreadStartReceiving, this);
m_ConnectThreadID =
m_MultiThreader->SpawnThread(this->ThreadStartConnecting, this);
// mitk::IGTTimeStamp::GetInstance()->Start(this);
return true;
}
bool mitk::IGTLDevice::StopCommunication()
{
if (this->GetState() == Running) // Only if the object is in the correct state
{
// m_StopCommunication is used by two threads, so we have to ensure correct
// thread handling
m_StopCommunicationMutex->Lock();
m_StopCommunication = true;
m_StopCommunicationMutex->Unlock();
// we have to wait here that the other thread recognizes the STOP-command
// and executes it
m_SendingFinishedMutex->Lock();
m_ReceivingFinishedMutex->Lock();
m_ConnectingFinishedMutex->Lock();
// mitk::IGTTimeStamp::GetInstance()->Stop(this); // notify realtime clock
// StopCommunication was called, thus the mode should be changed back
// to Ready now that the tracking loop has ended.
this->SetState(Ready);
}
return true;
}
bool mitk::IGTLDevice::CloseConnection()
{
if (this->GetState() == Setup)
{
return true;
}
else if (this->GetState() == Running)
{
this->StopCommunication();
}
m_Socket->CloseSocket();
/* return to setup mode */
this->SetState(Setup);
// this->InvokeEvent(mitk::LostConnectionEvent());
return true;
}
bool mitk::IGTLDevice::SendRTSMessage(const char* type)
{
//construct the device type for the return message, it starts with RTS_ and
//continues with the requested type
std::string returnType("RTS_");
returnType.append(type);
//create a return message
igtl::MessageBase::Pointer rtsMsg =
this->m_MessageFactory->CreateInstance(returnType);
//if retMsg is NULL there is no return message defined and thus it is not
//necessary to send one back
if (rtsMsg.IsNotNull())
{
this->SendMessage(mitk::IGTLMessage::New(rtsMsg));
return true;
}
else
{
return false;
}
}
void mitk::IGTLDevice::Connect()
{
MITK_DEBUG << "mitk::IGTLDevice::Connect();";
}
igtl::ImageMessage::Pointer mitk::IGTLDevice::GetNextImage2dMessage()
{
return this->m_MessageQueue->PullImage2dMessage();
}
igtl::ImageMessage::Pointer mitk::IGTLDevice::GetNextImage3dMessage()
{
return this->m_MessageQueue->PullImage3dMessage();
}
igtl::TransformMessage::Pointer mitk::IGTLDevice::GetNextTransformMessage()
{
return this->m_MessageQueue->PullTransformMessage();
}
igtl::TrackingDataMessage::Pointer mitk::IGTLDevice::GetNextTrackingDataMessage()
{
igtl::TrackingDataMessage::Pointer msg = this->m_MessageQueue->PullTrackingMessage();
return msg;
}
igtl::StringMessage::Pointer mitk::IGTLDevice::GetNextStringMessage()
{
return this->m_MessageQueue->PullStringMessage();
}
igtl::MessageBase::Pointer mitk::IGTLDevice::GetNextMiscMessage()
{
return this->m_MessageQueue->PullMiscMessage();
}
igtl::MessageBase::Pointer mitk::IGTLDevice::GetNextCommand()
{
return m_MessageQueue->PullCommandMessage();
}
void mitk::IGTLDevice::EnableInfiniteBufferingMode(bool enable)
{
m_MessageQueue->EnableInfiniteBuffering(enable);
}
void mitk::IGTLDevice::EnableInfiniteBufferingMode(
mitk::IGTLMessageQueue::Pointer queue,
bool enable)
{
queue->EnableInfiniteBuffering(enable);
}
ITK_THREAD_RETURN_TYPE mitk::IGTLDevice::ThreadStartSending(void* pInfoStruct)
{
/* extract this pointer from Thread Info structure */
struct itk::MultiThreader::ThreadInfoStruct * pInfo =
(struct itk::MultiThreader::ThreadInfoStruct*)pInfoStruct;
if (pInfo == nullptr)
{
return ITK_THREAD_RETURN_VALUE;
}
if (pInfo->UserData == nullptr)
{
return ITK_THREAD_RETURN_VALUE;
}
IGTLDevice *igtlDevice = (IGTLDevice*)pInfo->UserData;
if (igtlDevice != nullptr)
{
igtlDevice->RunCommunication(&mitk::IGTLDevice::Send, igtlDevice->m_SendingFinishedMutex);
}
igtlDevice->m_SendThreadID = 0; // erase thread id because thread will end.
return ITK_THREAD_RETURN_VALUE;
}
ITK_THREAD_RETURN_TYPE mitk::IGTLDevice::ThreadStartReceiving(void* pInfoStruct)
{
/* extract this pointer from Thread Info structure */
struct itk::MultiThreader::ThreadInfoStruct * pInfo =
(struct itk::MultiThreader::ThreadInfoStruct*)pInfoStruct;
if (pInfo == nullptr)
{
return ITK_THREAD_RETURN_VALUE;
}
if (pInfo->UserData == nullptr)
{
return ITK_THREAD_RETURN_VALUE;
}
IGTLDevice *igtlDevice = (IGTLDevice*)pInfo->UserData;
if (igtlDevice != nullptr)
{
igtlDevice->RunCommunication(&mitk::IGTLDevice::Receive,
igtlDevice->m_ReceivingFinishedMutex);
}
igtlDevice->m_ReceiveThreadID = 0; // erase thread id because thread will end.
return ITK_THREAD_RETURN_VALUE;
}
ITK_THREAD_RETURN_TYPE mitk::IGTLDevice::ThreadStartConnecting(void* pInfoStruct)
{
/* extract this pointer from Thread Info structure */
struct itk::MultiThreader::ThreadInfoStruct * pInfo =
(struct itk::MultiThreader::ThreadInfoStruct*)pInfoStruct;
if (pInfo == nullptr)
{
return ITK_THREAD_RETURN_VALUE;
}
if (pInfo->UserData == nullptr)
{
return ITK_THREAD_RETURN_VALUE;
}
IGTLDevice *igtlDevice = (IGTLDevice*)pInfo->UserData;
if (igtlDevice != nullptr)
{
igtlDevice->RunCommunication(&mitk::IGTLDevice::Connect,
igtlDevice->m_ConnectingFinishedMutex);
}
igtlDevice->m_ConnectThreadID = 0; // erase thread id because thread will end.
return ITK_THREAD_RETURN_VALUE;
}
diff --git a/Modules/OpenIGTLink/mitkIGTLMessage.cpp b/Modules/OpenIGTLink/mitkIGTLMessage.cpp
index 6da685c03f..5dd4b976c0 100644
--- a/Modules/OpenIGTLink/mitkIGTLMessage.cpp
+++ b/Modules/OpenIGTLink/mitkIGTLMessage.cpp
@@ -1,180 +1,181 @@
/*===================================================================
The Medical Imaging Interaction Toolkit (MITK)
Copyright (c) German Cancer Research Center,
Division of Medical and Biological Informatics.
All rights reserved.
This software is distributed WITHOUT ANY WARRANTY; without
even the implied warranty of MERCHANTABILITY or FITNESS FOR
A PARTICULAR PURPOSE.
See LICENSE.txt or http://www.mitk.org for details.
===================================================================*/
#include "mitkIGTLMessage.h"
#include "mitkException.h"
#include "mitkIGTLMessageCommon.h"
mitk::IGTLMessage::IGTLMessage() : itk::DataObject(),
m_DataValid(false), m_IGTTimeStamp(0), m_Name()
{
m_Message = igtl::MessageBase::New();
}
mitk::IGTLMessage::IGTLMessage(const mitk::IGTLMessage& toCopy) :
itk::DataObject()
{
// TODO SW: Graft does the same, remove code duplications, set Graft to
// deprecated, remove duplication in tescode
this->Graft(&toCopy);
}
mitk::IGTLMessage::~IGTLMessage()
{
}
mitk::IGTLMessage::IGTLMessage(igtl::MessageBase::Pointer message)
{
this->SetMessage(message);
this->SetName(message->GetDeviceName());
}
void mitk::IGTLMessage::Graft( const DataObject *data )
{
// Attempt to cast data to an IGTLMessage
const Self* msg;
try
{
msg = dynamic_cast<const Self *>(data);
}
catch( ... )
{
itkExceptionMacro( << "mitk::IGTLMessage::Graft cannot cast "
<< typeid(data).name() << " to "
<< typeid(const Self *).name() );
return;
}
if (!msg)
{
// pointer could not be cast back down
itkExceptionMacro( << "mitk::IGTLMessage::Graft cannot cast "
<< typeid(data).name() << " to "
<< typeid(const Self *).name() );
return;
}
// Now copy anything that is needed
this->SetMessage(msg->GetMessage());
this->SetDataValid(msg->IsDataValid());
this->SetIGTTimeStamp(msg->GetIGTTimeStamp());
this->SetName(msg->GetName());
}
void mitk::IGTLMessage::SetMessage(igtl::MessageBase::Pointer msg)
{
m_Message = msg;
unsigned int ts = 0;
unsigned int frac = 0;
m_Message->GetTimeStamp(&ts, &frac); //ts = seconds / frac = nanoseconds
+ this->SetName(m_Message->GetDeviceName());
double timestamp = ts * 1000.0 + frac;
this->SetIGTTimeStamp(timestamp);
this->SetDataValid(true);
}
bool mitk::IGTLMessage::IsDataValid() const
{
return m_DataValid;
}
void mitk::IGTLMessage::PrintSelf(std::ostream& os, itk::Indent indent) const
{
this->Superclass::PrintSelf(os, indent);
os << indent << "name: " << this->GetName() << std::endl;
os << indent << "data valid: " << this->IsDataValid() << std::endl;
os << indent << "TimeStamp: " << this->GetIGTTimeStamp() << std::endl;
os << indent << "OpenIGTLinkMessage: " << std::endl;
m_Message->Print(os);
}
std::string mitk::IGTLMessage::ToString() const
{
std::stringstream output;
output << "name: " << this->GetName() << std::endl <<
"MessageType: " << this->GetIGTLMessageType() << std::endl <<
"TimeStamp: " << this->GetIGTTimeStamp() << std::endl <<
"OpenIGTLinkMessage: " << std::endl;
return output.str();
}
void mitk::IGTLMessage::CopyInformation( const DataObject* data )
{
this->Superclass::CopyInformation( data );
const Self * nd = nullptr;
try
{
nd = dynamic_cast<const Self*>(data);
}
catch( ... )
{
// data could not be cast back down
itkExceptionMacro(<< "mitk::IGTLMessage::CopyInformation() cannot cast "
<< typeid(data).name() << " to "
<< typeid(Self*).name() );
}
if ( !nd )
{
// pointer could not be cast back down
itkExceptionMacro(<< "mitk::IGTLMessage::CopyInformation() cannot cast "
<< typeid(data).name() << " to "
<< typeid(Self*).name() );
}
/* copy all meta data */
}
bool mitk::Equal(const mitk::IGTLMessage& leftHandSide,
const mitk::IGTLMessage& rightHandSide,
ScalarType /*eps*/, bool verbose)
{
bool returnValue = true;
if( std::string(rightHandSide.GetName()) != std::string(leftHandSide.GetName()) )
{
if(verbose)
{
MITK_INFO << "[( IGTLMessage )] Name differs.";
MITK_INFO << "leftHandSide is " << leftHandSide.GetName()
<< "rightHandSide is " << rightHandSide.GetName();
}
returnValue = false;
}
if( rightHandSide.GetIGTTimeStamp() != leftHandSide.GetIGTTimeStamp() )
{
if(verbose)
{
MITK_INFO << "[( IGTLMessage )] IGTTimeStamp differs.";
MITK_INFO << "leftHandSide is " << leftHandSide.GetIGTTimeStamp()
<< "rightHandSide is " << rightHandSide.GetIGTTimeStamp();
}
returnValue = false;
}
return returnValue;
}
const char* mitk::IGTLMessage::GetIGTLMessageType() const
{
return this->m_Message->GetDeviceType();
}
template < typename IGTLMessageType >
IGTLMessageType* mitk::IGTLMessage::GetMessage() const
{
return dynamic_cast<IGTLMessageType*>(this->m_Message);
}