Medical Imaging Interaction Toolkit  2018.4.99-a3d2e8fb
Medical Imaging Interaction Toolkit
mitkIGTLDevice.cpp
Go to the documentation of this file.
1 /*============================================================================
2 
3 The Medical Imaging Interaction Toolkit (MITK)
4 
5 Copyright (c) German Cancer Research Center (DKFZ)
6 All rights reserved.
7 
8 Use of this source code is governed by a 3-clause BSD license that can be
9 found in the LICENSE file.
10 
11 ============================================================================*/
12 
13 #include "mitkIGTLDevice.h"
14 //#include "mitkIGTException.h"
15 //#include "mitkIGTTimeStamp.h"
16 #include <itkMutexLockHolder.h>
17 #include <itksys/SystemTools.hxx>
18 #include <cstring>
19 
20 #include <igtlTransformMessage.h>
21 #include <mitkIGTLMessageCommon.h>
22 
23 #include <igtl_status.h>
24 
25 //remove later
26 #include <igtlTrackingDataMessage.h>
27 
28 //TODO: Which timeout is acceptable and also needed to transmit image data? Is there a maximum data limit?
29 static const int SOCKET_SEND_RECEIVE_TIMEOUT_MSEC = 100;
31 
33 // m_Data(mitk::DeviceDataUnspecified),
34 m_State(mitk::IGTLDevice::Setup),
35 m_Name("Unspecified Device"),
36 m_StopCommunication(false),
37 m_Hostname("127.0.0.1"),
38 m_PortNumber(-1),
39 m_LogMessages(false),
40 m_MultiThreader(nullptr), m_SendThreadID(0), m_ReceiveThreadID(0), m_ConnectThreadID(0)
41 {
42  m_ReadFully = ReadFully;
43  m_StopCommunicationMutex = itk::FastMutexLock::New();
44  m_StateMutex = itk::FastMutexLock::New();
45  // m_LatestMessageMutex = itk::FastMutexLock::New();
46  m_SendingFinishedMutex = itk::FastMutexLock::New();
47  m_ReceivingFinishedMutex = itk::FastMutexLock::New();
48  m_ConnectingFinishedMutex = itk::FastMutexLock::New();
49  // execution rights are owned by the application thread at the beginning
50  m_SendingFinishedMutex->Lock();
53  m_MultiThreader = itk::MultiThreader::New();
54  // m_Data = mitk::DeviceDataUnspecified;
55  // m_LatestMessage = igtl::MessageBase::New();
56 
59 }
60 
62 {
63  /* stop communication and disconnect from igtl device */
64  if (GetState() == Running)
65  {
66  this->StopCommunication();
67  this->CloseConnection();
68  }
69  else if (GetState() == Ready)
70  {
71  this->CloseConnection();
72  }
73  /* cleanup tracking thread */
74  if (m_MultiThreader.IsNotNull())
75  {
76  if ((m_SendThreadID != 0))
77  {
78  m_MultiThreader->TerminateThread(m_SendThreadID);
79  }
80  if ((m_ReceiveThreadID != 0))
81  {
82  m_MultiThreader->TerminateThread(m_ReceiveThreadID);
83  }
84  if ((m_ConnectThreadID != 0))
85  {
86  m_MultiThreader->TerminateThread(m_ConnectThreadID);
87  }
88  }
89  m_MultiThreader = nullptr;
90 }
91 
93 {
95  return m_State;
96 }
97 
99 {
100  itkDebugMacro("setting m_State to " << state);
101 
102  m_StateMutex->Lock();
103  // MutexLockHolder lock(*m_StateMutex); // lock and unlock the mutex
104 
105  if (m_State == state)
106  {
107  m_StateMutex->Unlock();
108  return;
109  }
110  m_State = state;
111  m_StateMutex->Unlock();
112  this->Modified();
113 }
114 
116 {
117  return true;
118 }
119 
120 unsigned int mitk::IGTLDevice::ReceivePrivate(igtl::Socket* socket)
121 {
122  // Create a message buffer to receive header
123  igtl::MessageHeader::Pointer headerMsg;
124  headerMsg = igtl::MessageHeader::New();
125 
126  // Initialize receive buffer
127  headerMsg->InitPack();
128 
129  // Receive generic header from the socket
130  int r =
131  socket->Receive(headerMsg->GetPackPointer(), headerMsg->GetPackSize(), 0);
132 
133  //MITK_INFO << "Server received r = " << r;
134 
135  //MITK_INFO << "Received r = " << r;
136 
137  if (r == 0) //connection error
138  {
139  // an error was received, therefore the communication with this socket
140  // must be stoppedy
141  return IGTL_STATUS_NOT_PRESENT;
142  }
143  else if (r == -1) //timeout
144  {
145  // a timeout was received, this is no error state, thus, do nothing
146  return IGTL_STATUS_TIME_OUT;
147  }
148  else if (r == headerMsg->GetPackSize())
149  {
150  // Deserialize the header and check the CRC
151  // ERROR HERE: This probably means the header data is corrupted...
152  int crcCheck = headerMsg->Unpack(1);
153 
154  if (crcCheck & igtl::MessageHeader::UNPACK_HEADER)
155  {
156  // Allocate a time stamp
157  igtl::TimeStamp::Pointer ts;
158  ts = igtl::TimeStamp::New();
159 
160  // Get time stamp
161  igtlUint32 sec;
162  igtlUint32 nanosec;
163 
164  headerMsg->GetTimeStamp(ts);
165  ts->GetTimeStamp(&sec, &nanosec);
166 
167  // std::cerr << "Time stamp: "
168  // << sec << "."
169  // << nanosec << std::endl;
170 
171  // std::cerr << "Dev type and name: " << headerMsg->GetDeviceType() << " "
172  // << headerMsg->GetDeviceName() << std::endl;
173 
174  // headerMsg->Print(std::cout);
175 
176  //check the type of the received message
177  //if it is a GET_, STP_ or RTS_ command push it into the command queue
178  //otherwise continue reading the whole message from the socket
179  const char* curDevType = headerMsg->GetDeviceType();
180  if (std::strstr(curDevType, "GET_") != nullptr ||
181  std::strstr(curDevType, "STP_") != nullptr ||
182  std::strstr(curDevType, "RTS_") != nullptr)
183  {
184  this->m_MessageQueue->PushCommandMessage(headerMsg);
185  this->InvokeEvent(CommandReceivedEvent());
186  return IGTL_STATUS_OK;
187  }
188 
189  //Create a message according to the header message
190  igtl::MessageBase::Pointer curMessage;
191  curMessage = m_MessageFactory->CreateInstance(headerMsg);
192 
193  //check if the curMessage is created properly, if not the message type is
194  //not supported and the message has to be skipped
195  if (curMessage.IsNull())
196  {
197  socket->Skip(headerMsg->GetBodySizeToRead(), 0);
198  // MITK_ERROR("IGTLDevice") << "The received type is not supported. Please "
199  // "add it to the message factory.";
200  return IGTL_STATUS_NOT_FOUND;
201  }
202 
203  //insert the header to the message and allocate the pack
204  curMessage->SetMessageHeader(headerMsg);
205  curMessage->AllocatePack();
206 
207  // Receive transform data from the socket
208  int receiveCheck = 0;
209  receiveCheck = socket->Receive(curMessage->GetPackBodyPointer(),
210  curMessage->GetPackBodySize(), m_ReadFully);
211 
212  if (receiveCheck > 0)
213  {
214  int c = curMessage->Unpack(1);
215  if (!(c & igtl::MessageHeader::UNPACK_BODY))
216  {
217  return IGTL_STATUS_CHECKSUM_ERROR;
218  }
219 
220  //check the type of the received message
221  //if it is a command push it into the command queue
222  //otherwise into the normal receive queue
223  //STP_ commands are handled here because they implemented additional
224  //member variables that are not stored in the header message
225  if (std::strstr(curDevType, "STT_") != nullptr)
226  {
227  this->m_MessageQueue->PushCommandMessage(curMessage);
228  this->InvokeEvent(CommandReceivedEvent());
229  }
230  else
231  {
232  if(m_LogMessages)
233  MITK_INFO << "Received Message: " << mitk::IGTLMessage::New(curMessage)->ToString();
234  this->m_MessageQueue->PushMessage(curMessage);
235  this->InvokeEvent(MessageReceivedEvent());
236  }
237  return IGTL_STATUS_OK;
238  }
239  else
240  {
241  MITK_WARN("IGTLDevice") << "Received a valid header but could not "
242  << "read the whole message.";
243  return IGTL_STATUS_UNKNOWN_ERROR;
244  }
245  }
246  else
247  {
248  //CRC check failed
249  MITK_WARN << "CRC Check failed";
250  return IGTL_STATUS_CHECKSUM_ERROR;
251  }
252  }
253  else
254  {
255  //Message size information and actual data size don't match.
256  //this state is not suppossed to be reached, return unknown error
257  MITK_WARN << "IGTL status unknown";
258  return IGTL_STATUS_UNKNOWN_ERROR;
259  }
260 }
261 
262 void mitk::IGTLDevice::SendMessage(mitk::IGTLMessage::Pointer msg)
263 {
264  m_MessageQueue->PushSendMessage(msg);
265 }
266 
267 unsigned int mitk::IGTLDevice::SendMessagePrivate(mitk::IGTLMessage::Pointer msg,
268  igtl::Socket::Pointer socket)
269 {
270  //check the input message
271  if (msg.IsNull())
272  {
273  MITK_ERROR("IGTLDevice") << "Could not send message because message is not "
274  "valid. Please check.";
275  return false;
276  }
277 
278  igtl::MessageBase* sendMessage = msg->GetMessage();
279 
280  // Pack (serialize) and send
281  sendMessage->Pack();
282 
283  int sendSuccess = socket->Send(sendMessage->GetPackPointer(), sendMessage->GetPackSize());
284 
285  if (sendSuccess)
286  {
287  if (m_LogMessages) { MITK_INFO << "Send IGTL message: " << msg->ToString(); }
288  this->InvokeEvent(MessageSentEvent());
289  return IGTL_STATUS_OK;
290  }
291  else
292  {
293  return IGTL_STATUS_UNKNOWN_ERROR;
294  }
295 }
296 
297 void mitk::IGTLDevice::RunCommunication(void (IGTLDevice::*ComFunction)(void), itk::FastMutexLock* mutex)
298 {
299  if (this->GetState() != Running)
300  return;
301 
302  try
303  {
304  // keep lock until end of scope
305  MutexLockHolder communicationFinishedLockHolder(*mutex);
306 
307  // Because m_StopCommunication is used by two threads, access has to be guarded
308  // by a mutex. To minimize thread locking, a local copy is used here
309  bool localStopCommunication;
310 
311  // update the local copy of m_StopCommunication
312  this->m_StopCommunicationMutex->Lock();
313  localStopCommunication = this->m_StopCommunication;
314  this->m_StopCommunicationMutex->Unlock();
315  while ((this->GetState() == Running) && (localStopCommunication == false))
316  {
317  (this->*ComFunction)();
318 
319  /* Update the local copy of m_StopCommunication */
320  this->m_StopCommunicationMutex->Lock();
321  localStopCommunication = m_StopCommunication;
322  this->m_StopCommunicationMutex->Unlock();
323 
324  // time to relax, this sets the maximum ever possible framerate to 1000 Hz
325  itksys::SystemTools::Delay(1);
326  }
327  }
328  catch (...)
329  {
330  mutex->Unlock();
331  this->StopCommunication();
332  MITK_ERROR("IGTLDevice::RunCommunication") << "Error while communicating. Thread stopped.";
333  //mitkThrowException(mitk::IGTException) << "Error while communicating. Thread stopped.";
334  }
335  // StopCommunication was called, thus the mode should be changed back to Ready now
336  // that the tracking loop has ended.
337  //this->SetState(Ready); //this is done elsewhere
338  MITK_DEBUG("IGTLDevice::RunCommunication") << "Reached end of communication.";
339  // returning from this function (and ThreadStartCommunication())
340  // this will end the thread
341  return;
342 }
343 
345 {
346  if (this->GetState() != Ready)
347  return false;
348 
349  // go to mode Running
350  this->SetState(Running);
351 
352  // set a timeout for the sending and receiving
353  this->m_Socket->SetTimeout(SOCKET_SEND_RECEIVE_TIMEOUT_MSEC);
354 
355  // update the local copy of m_StopCommunication
356  this->m_StopCommunicationMutex->Lock();
357  this->m_StopCommunication = false;
358  this->m_StopCommunicationMutex->Unlock();
359 
360  // transfer the execution rights to tracking thread
361  m_SendingFinishedMutex->Unlock();
362  m_ReceivingFinishedMutex->Unlock();
363  m_ConnectingFinishedMutex->Unlock();
364 
365  // start new threads that execute the communication
366  m_SendThreadID =
367  m_MultiThreader->SpawnThread(this->ThreadStartSending, this);
368  m_ReceiveThreadID =
369  m_MultiThreader->SpawnThread(this->ThreadStartReceiving, this);
370  m_ConnectThreadID =
371  m_MultiThreader->SpawnThread(this->ThreadStartConnecting, this);
372  // mitk::IGTTimeStamp::GetInstance()->Start(this);
373  return true;
374 }
375 
377 {
378  if (this->GetState() == Running) // Only if the object is in the correct state
379  {
380  // m_StopCommunication is used by two threads, so we have to ensure correct
381  // thread handling
382  m_StopCommunicationMutex->Lock();
383  m_StopCommunication = true;
384  m_StopCommunicationMutex->Unlock();
385  // we have to wait here that the other thread recognizes the STOP-command
386  // and executes it
387  m_SendingFinishedMutex->Lock();
388  m_ReceivingFinishedMutex->Lock();
390  // mitk::IGTTimeStamp::GetInstance()->Stop(this); // notify realtime clock
391  // StopCommunication was called, thus the mode should be changed back
392  // to Ready now that the tracking loop has ended.
393  this->SetState(Ready);
394  }
395  return true;
396 }
397 
399 {
400  if (this->GetState() == Setup)
401  {
402  return true;
403  }
404  else if (this->GetState() == Running)
405  {
406  this->StopCommunication();
407  }
408 
409  m_Socket->CloseSocket();
410 
411  /* return to setup mode */
412  this->SetState(Setup);
413 
414  // this->InvokeEvent(mitk::LostConnectionEvent());
415 
416  return true;
417 }
418 
419 bool mitk::IGTLDevice::SendRTSMessage(const char* type)
420 {
421  //construct the device type for the return message, it starts with RTS_ and
422  //continues with the requested type
423  std::string returnType("RTS_");
424  returnType.append(type);
425  //create a return message
426  igtl::MessageBase::Pointer rtsMsg =
427  this->m_MessageFactory->CreateInstance(returnType);
428  //if retMsg is nullptr there is no return message defined and thus it is not
429  //necessary to send one back
430  if (rtsMsg.IsNotNull())
431  {
432  this->SendMessage(mitk::IGTLMessage::New(rtsMsg));
433  return true;
434  }
435  else
436  {
437  return false;
438  }
439 }
440 
442 {
443  MITK_DEBUG << "mitk::IGTLDevice::Connect();";
444 }
445 
446 igtl::ImageMessage::Pointer mitk::IGTLDevice::GetNextImage2dMessage()
447 {
448  return this->m_MessageQueue->PullImage2dMessage();
449 }
450 
451 igtl::ImageMessage::Pointer mitk::IGTLDevice::GetNextImage3dMessage()
452 {
453  return this->m_MessageQueue->PullImage3dMessage();
454 }
455 
456 igtl::TransformMessage::Pointer mitk::IGTLDevice::GetNextTransformMessage()
457 {
458  return this->m_MessageQueue->PullTransformMessage();
459 }
460 
461 igtl::TrackingDataMessage::Pointer mitk::IGTLDevice::GetNextTrackingDataMessage()
462 {
463  igtl::TrackingDataMessage::Pointer msg = this->m_MessageQueue->PullTrackingMessage();
464  return msg;
465 }
466 
467 igtl::StringMessage::Pointer mitk::IGTLDevice::GetNextStringMessage()
468 {
469  return this->m_MessageQueue->PullStringMessage();
470 }
471 
472 igtl::MessageBase::Pointer mitk::IGTLDevice::GetNextMiscMessage()
473 {
474  return this->m_MessageQueue->PullMiscMessage();
475 }
476 
477 igtl::MessageBase::Pointer mitk::IGTLDevice::GetNextCommand()
478 {
479  return m_MessageQueue->PullCommandMessage();
480 }
482 {
483  m_MessageQueue->EnableNoBufferingMode(enable);
484 }
485 
487  mitk::IGTLMessageQueue::Pointer queue,
488  bool enable)
489 {
490  queue->EnableNoBufferingMode(enable);
491 }
492 
493 ITK_THREAD_RETURN_TYPE mitk::IGTLDevice::ThreadStartSending(void* pInfoStruct)
494 {
495  /* extract this pointer from Thread Info structure */
496  struct itk::MultiThreader::ThreadInfoStruct * pInfo =
497  (struct itk::MultiThreader::ThreadInfoStruct*)pInfoStruct;
498  if (pInfo == nullptr)
499  {
500  return ITK_THREAD_RETURN_VALUE;
501  }
502  if (pInfo->UserData == nullptr)
503  {
504  return ITK_THREAD_RETURN_VALUE;
505  }
506  IGTLDevice *igtlDevice = (IGTLDevice*)pInfo->UserData;
507  if (igtlDevice != nullptr)
508  {
510  }
511  igtlDevice->m_SendThreadID = 0; // erase thread id because thread will end.
512  return ITK_THREAD_RETURN_VALUE;
513 }
514 
515 ITK_THREAD_RETURN_TYPE mitk::IGTLDevice::ThreadStartReceiving(void* pInfoStruct)
516 {
517  /* extract this pointer from Thread Info structure */
518  struct itk::MultiThreader::ThreadInfoStruct * pInfo =
519  (struct itk::MultiThreader::ThreadInfoStruct*)pInfoStruct;
520  if (pInfo == nullptr)
521  {
522  return ITK_THREAD_RETURN_VALUE;
523  }
524  if (pInfo->UserData == nullptr)
525  {
526  return ITK_THREAD_RETURN_VALUE;
527  }
528  IGTLDevice *igtlDevice = (IGTLDevice*)pInfo->UserData;
529  if (igtlDevice != nullptr)
530  {
532  igtlDevice->m_ReceivingFinishedMutex);
533  }
534  igtlDevice->m_ReceiveThreadID = 0; // erase thread id because thread will end.
535  return ITK_THREAD_RETURN_VALUE;
536 }
537 
538 ITK_THREAD_RETURN_TYPE mitk::IGTLDevice::ThreadStartConnecting(void* pInfoStruct)
539 {
540  /* extract this pointer from Thread Info structure */
541  struct itk::MultiThreader::ThreadInfoStruct * pInfo =
542  (struct itk::MultiThreader::ThreadInfoStruct*)pInfoStruct;
543  if (pInfo == nullptr)
544  {
545  return ITK_THREAD_RETURN_VALUE;
546  }
547  if (pInfo->UserData == nullptr)
548  {
549  return ITK_THREAD_RETURN_VALUE;
550  }
551  IGTLDevice *igtlDevice = (IGTLDevice*)pInfo->UserData;
552  if (igtlDevice != nullptr)
553  {
555  igtlDevice->m_ConnectingFinishedMutex);
556  }
557  igtlDevice->m_ConnectThreadID = 0; // erase thread id because thread will end.
558  return ITK_THREAD_RETURN_VALUE;
559 }
itk::FastMutexLock::Pointer m_ReceivingFinishedMutex
IGTLDeviceState GetState() const
Returns current object state (Setup, Ready or Running)
itk::FastMutexLock::Pointer m_StopCommunicationMutex
static Pointer New()
mitk::IGTLMessageFactory::Pointer m_MessageFactory
void SetState(IGTLDeviceState state)
change object state
IGTLDeviceState m_State
static ITK_THREAD_RETURN_TYPE ThreadStartConnecting(void *data)
static start method for the connection thread.
bool StartCommunication()
Starts the communication between the two devices.
igtl::MessageBase::Pointer GetNextMiscMessage()
#define MITK_INFO
Definition: mitkLogMacros.h:18
itk::FastMutexLock::Pointer m_StateMutex
#define MITK_ERROR
Definition: mitkLogMacros.h:20
static const int SOCKET_SEND_RECEIVE_TIMEOUT_MSEC
itk::FastMutexLock::Pointer m_ConnectingFinishedMutex
static Pointer New()
#define MITK_DEBUG
Definition: mitkLogMacros.h:22
DataCollection - Class to facilitate loading/accessing structured data.
igtl::MessageBase::Pointer GetNextCommand()
Returns the oldest message in the command queue.
igtl::ImageMessage::Pointer GetNextImage2dMessage()
Returns the oldest message in the receive queue.
igtl::TransformMessage::Pointer GetNextTransformMessage()
igtl::ImageMessage::Pointer GetNextImage3dMessage()
virtual void Connect()
Call this method to check for other devices that want to connect to this one.
virtual void Send()=0
Call this method to send a message. The message will be read from the queue.
bool SendRTSMessage(const char *type)
Send RTS message of given type.
unsigned int ReceivePrivate(igtl::Socket *device)
Call this method to receive a message from the given device.
unsigned int SendMessagePrivate(mitk::IGTLMessage::Pointer msg, igtl::Socket::Pointer socket)
Sends a message.
~IGTLDevice() override
#define MITK_WARN
Definition: mitkLogMacros.h:19
virtual bool TestConnection()
TestConnection() tries to connect to a IGTL device on the current ip and port.
virtual void Receive()=0
Call this method to receive a message.
virtual bool CloseConnection()
Closes the connection to the device.
itk::MutexLockHolder< itk::FastMutexLock > MutexLockHolder
IGTLDeviceState
Type for state variable. The IGTLDevice is always in one of these states.
mitk::IGTLMessageQueue::Pointer m_MessageQueue
itk::FastMutexLock::Pointer m_SendingFinishedMutex
Interface for all OpenIGTLink Devices.
igtl::TrackingDataMessage::Pointer GetNextTrackingDataMessage()
void EnableNoBufferingMode(mitk::IGTLMessageQueue::Pointer queue, bool enable=true)
Sets the buffering mode of the given queue.
virtual bool StopCommunication()
Stops the communication between the two devices.
static ITK_THREAD_RETURN_TYPE ThreadStartReceiving(void *data)
static start method for the receiving thread.
static ITK_THREAD_RETURN_TYPE ThreadStartSending(void *data)
static start method for the sending thread.
igtl::Socket::Pointer m_Socket
static Pointer New()
void SendMessage(mitk::IGTLMessage::Pointer msg)
Adds the given message to the sending queue.
igtl::StringMessage::Pointer GetNextStringMessage()
static void Setup()
void RunCommunication(void(IGTLDevice::*ComFunction)(void), itk::FastMutexLock *mutex)
Continuously calls the given function.