Medical Imaging Interaction Toolkit  2018.4.99-12ad79a3
Medical Imaging Interaction Toolkit
mitkOpenIGTLinkTrackingDevice.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 
15 #include "mitkIGTConfig.h"
16 #include "mitkIGTTimeStamp.h"
17 #include "mitkIGTHardwareException.h"
18 #include "mitkTrackingTypes.h"
19 #include <itksys/SystemTools.hxx>
20 #include <iostream>
21 #include <itkMutexLockHolder.h>
22 #include <itkCommand.h>
24 #include <vtkConeSource.h>
25 
26 //sleep headers
27 #include <chrono>
28 #include <thread>
29 
31 
33 {
34  //set the type of this tracking device
36 
37  m_OpenIGTLinkClient = mitk::IGTLClient::New(true);
38  m_OpenIGTLinkClient->SetName("OpenIGTLink Tracking Device");
39  m_OpenIGTLinkClient->EnableNoBufferingMode(false);
40 
43 }
44 
46 {
47 }
48 
50 {
51  return m_OpenIGTLinkClient->GetPortNumber();
52 }
53 
55 {
56  return true;
57 }
58 
59 mitk::NavigationToolStorage::Pointer mitk::OpenIGTLinkTrackingDevice::AutoDetectTools()
60 {
61  mitk::NavigationToolStorage::Pointer returnValue = mitk::NavigationToolStorage::New();
62 
63  if (m_OpenIGTLinkClient->GetPortNumber() == -1)
64  {
65  MITK_WARN << "Connection not initialized, aborting (invalid port number).";
67  }
68 
69  //open connection
70  try
71  {
72  m_IGTLDeviceSource->Connect();
73  m_IGTLDeviceSource->StartCommunication();
74  }
75  catch (std::runtime_error &e)
76  {
77  MITK_WARN << "AutoDetection: Open IGT Link device retruned an error while trying to connect: " << e.what();
79  }
80 
81  //get a message to find out type
83  mitk::IGTLMessage::Pointer receivedMessage = ReceiveMessage(100);
84 
85  const char* msgType = receivedMessage->GetIGTLMessageType();
86 
87  if (std::string(msgType).empty())
88  {
89  MITK_INFO << "Did not receive a message. Do you have to start the stream manually at the server?";
90  MITK_INFO << "Waiting for 10 seconds ...";
91  receivedMessage = ReceiveMessage(10000);
92  msgType = receivedMessage->GetIGTLMessageType();
93  }
94  MITK_INFO << "################# got message type: " << msgType;
95  mitk::OpenIGTLinkTrackingDevice::TrackingMessageType type = GetMessageTypeFromString(msgType);
96  switch (type)
97  {
98  case UNKNOWN:
100  break;
101  case TDATA:
103  break;
104  case QTDATA:
106  break;
107  case TRANSFORM:
109  break;
110  }
111  returnValue = DiscoverToolsAndConvertToNavigationTools(type);
112 
113  //close connection
114  try
115  {
116  m_IGTLDeviceSource->StopCommunication();
117  m_IGTLDeviceSource->Disconnect();
118  }
119  catch (std::runtime_error &e)
120  {
121  MITK_WARN << "AutoDetection: Open IGT Link device retruned an error while trying to disconnect: " << e.what();
123  }
124 
125 
126  return returnValue;
127 }
128 
129 mitk::NavigationToolStorage::Pointer mitk::OpenIGTLinkTrackingDevice::DiscoverToolsAndConvertToNavigationTools(mitk::OpenIGTLinkTrackingDevice::TrackingMessageType type, int NumberOfMessagesToWait)
130 {
131  MITK_INFO << "Start discovering tools by " << type << " messages";
132  mitk::NavigationToolStorage::Pointer returnValue = mitk::NavigationToolStorage::New();
133  std::map<std::string, int> toolNameMap;
134 
135  for (int j = 0; j<NumberOfMessagesToWait; j++)
136  {
137  std::this_thread::sleep_for(std::chrono::milliseconds(20));
138  m_IGTLDeviceSource->Update();
139  switch (type)
140  {
141  case TRANSFORM:
142  {
143  igtl::TransformMessage::Pointer msg = dynamic_cast<igtl::TransformMessage*>(m_IGTLDeviceSource->GetOutput()->GetMessage().GetPointer());
144  if (msg == nullptr || msg.IsNull()) { MITK_INFO << "Received message is invalid / null. Skipping.."; continue; }
145  int count = toolNameMap[msg->GetDeviceName()];
146  if (count == 0) { toolNameMap[msg->GetDeviceName()] = 1; }
147  else { toolNameMap[msg->GetDeviceName()]++; }
148  }
149  break;
150  case TDATA:
151  {
152  igtl::TrackingDataMessage::Pointer msg = dynamic_cast<igtl::TrackingDataMessage*>(m_IGTLDeviceSource->GetOutput()->GetMessage().GetPointer());
153  if (msg == nullptr || msg.IsNull()) { MITK_INFO << "Received message is invalid / null. Skipping.."; continue; }
154  for (int k = 0; k < msg->GetNumberOfTrackingDataElements(); k++)
155  {
156  igtl::TrackingDataElement::Pointer tde;
157  msg->GetTrackingDataElement(k, tde);
158  if (tde.IsNotNull())
159  {
160  int count = toolNameMap[tde->GetName()];
161  if (count == 0) { toolNameMap[tde->GetName()] = 1; }
162  else { toolNameMap[tde->GetName()]++; }
163  }
164  }
165  }
166  break;
167  default:
168  MITK_WARN << "Only TRANSFORM and TDATA is currently supported, skipping!";
169  break;
170  }
171  }
172 
173  int i = 0;
174  for (std::map<std::string, int>::iterator it = toolNameMap.begin(); it != toolNameMap.end(); ++it)
175  {
176  MITK_INFO << "Found tool: " << it->first;
177 
178  std::stringstream name;
179  name << it->first;
180 
181  std::stringstream identifier;
182  identifier << "AutoDetectedTool-" << i;
183  i++;
184 
185  mitk::NavigationTool::Pointer newTool = ConstructDefaultOpenIGTLinkTool(name.str(), identifier.str());
186 
187  returnValue->AddTool(newTool);
188  }
189 
190  return returnValue;
191 }
192 
194 {
195  return m_OpenIGTLinkClient->GetHostname();
196 }
197 
199 {
200  m_OpenIGTLinkClient->SetPortNumber(portNumber);
201 }
202 
204 {
205  m_OpenIGTLinkClient->SetHostname(hostname);
206 }
207 
209 {
210  return true;
211 }
212 
214 {
215  mitk::OpenIGTLinkTrackingTool::Pointer t;// = mitk::OpenIGTLinkTrackingTool::New();
216  //TODO: Implement
217  if (this->InternalAddTool(t) == false)
218  return nullptr;
219  return t.GetPointer();
220 }
221 
222 bool mitk::OpenIGTLinkTrackingDevice::InternalAddTool(OpenIGTLinkTrackingTool::Pointer tool)
223 {
224  m_AllTools.push_back(tool);
225  return true;
226 }
227 
229 {
230  if (m_OpenIGTLinkClient->GetPortNumber() == -1)
231  {
232  MITK_WARN << "Connection not initialized, aborting (invalid port number).";
233  return false;
234  }
235 
236  try
237  {
238  m_IGTLDeviceSource->Connect();
239  m_IGTLDeviceSource->StartCommunication();
240  }
241  catch (std::runtime_error &e)
242  {
243  MITK_WARN << "Open IGT Link device retruned an error while trying to connect: " << e.what();
244  return false;
245  }
246 
247  mitk::IGTLMessage::Pointer receivedMessage = ReceiveMessage(waitingTime);
248 
249  //check the tracking stream for the number and type of tools
250  //igtl::MessageBase::Pointer receivedMessage = m_OpenIGTLinkClient->GetNextMessage();
251  if (receivedMessage.IsNull())
252  {
253  MITK_WARN << "No message was received. Is there really a server?";
254  return false;
255  }
256  else if (!receivedMessage->IsDataValid())
257  {
258  MITK_WARN << "Received invalid message.";
259  return false;
260  }
261 
262  const char* msgType = receivedMessage->GetIGTLMessageType();
263 
264  mitk::OpenIGTLinkTrackingDevice::TrackingMessageType type = GetMessageTypeFromString(msgType);
265 
266  mitk::NavigationToolStorage::Pointer foundTools = this->DiscoverToolsAndConvertToNavigationTools(type);
267  if (foundTools.IsNull() || (foundTools->GetToolCount() == 0)) { return false; }
268  for (unsigned int i = 0; i < foundTools->GetToolCount(); i++) { AddNewToolForName(foundTools->GetTool(i)->GetToolName(), i); }
269  MITK_INFO << "Found tools: " << foundTools->GetToolCount();
270  return true;
271 }
272 
273 mitk::IGTLMessage::Pointer mitk::OpenIGTLinkTrackingDevice::ReceiveMessage(int waitingTime)
274 {
275  mitk::IGTLMessage::Pointer receivedMessage;
276  //send a message to the server: start tracking stream
277  mitk::IGTLMessageFactory::Pointer msgFactory = m_OpenIGTLinkClient->GetMessageFactory();
278  std::string message[2] = {"STT_QTDATA","STT_TDATA"};
279 
280  for (int i = 0; i < 2; i++)
281  {
282  igtl::MessageBase::Pointer sttMsg = msgFactory->CreateInstance(message[i]);
283  //TODO: Fix this to dynamically get this from GUI
284  ((igtl::StartTrackingDataMessage*)sttMsg.GetPointer())->SetResolution(m_UpdateRate);
285  m_OpenIGTLinkClient->SendMessage(mitk::IGTLMessage::New(sttMsg));
286  }
287 
288  std::chrono::high_resolution_clock::time_point time = std::chrono::high_resolution_clock::now();
289  std::chrono::milliseconds d = std::chrono::milliseconds(waitingTime);
290 
291  while (!(receivedMessage.IsNotNull() && receivedMessage->IsDataValid()))
292  {
293  m_IGTLDeviceSource->Update();
294  receivedMessage = m_IGTLDeviceSource->GetOutput();
295 
296  if ((time + d) < std::chrono::high_resolution_clock::now())
297  break;
298 
299  std::this_thread::sleep_for(std::chrono::milliseconds(100));
300  }
301  return receivedMessage;
302 }
303 
304 void mitk::OpenIGTLinkTrackingDevice::AddNewToolForName(std::string name, int i)
305 {
306  mitk::OpenIGTLinkTrackingTool::Pointer newTool = mitk::OpenIGTLinkTrackingTool::New();
307  if (name == "") //if no name was given create a default name
308  {
309  std::stringstream defaultName;
310  defaultName << "OpenIGTLinkTool#" << i;
311  name = defaultName.str();
312  }
313  MITK_INFO << "Added tool " << name << " to tracking device.";
314  newTool->SetToolName(name);
315  InternalAddTool(newTool);
316 }
317 
318 mitk::NavigationTool::Pointer mitk::OpenIGTLinkTrackingDevice::ConstructDefaultOpenIGTLinkTool(std::string name, std::string identifier)
319 {
320  mitk::NavigationTool::Pointer newTool = mitk::NavigationTool::New();
321  newTool->GetDataNode()->SetName(name);
322  newTool->SetIdentifier(identifier);
323 
325 
326  return newTool;
327 
328 }
329 
331 {
332  if (this->GetState() != Tracking)
333  {
334  MITK_ERROR << "Method was called in the wrong state, something went wrong!";
335  return;
336  }
337 
338  m_IGTLMsgToNavDataFilter->Update();
339 
340  for (std::size_t j = 0; j < m_IGTLMsgToNavDataFilter->GetNumberOfIndexedOutputs(); ++j)
341  {
342  mitk::NavigationData::Pointer currentNavData = m_IGTLMsgToNavDataFilter->GetOutput(j);
343  const char* name = currentNavData->GetName();
344  for (std::size_t i = 0; i < m_AllTools.size(); i++)
345  {
346  if (strcmp(m_AllTools.at(i)->GetToolName(), name) == 0)
347  {
348  m_AllTools.at(i)->SetDataValid(currentNavData->IsDataValid());
349  m_AllTools.at(i)->SetPosition(currentNavData->GetPosition());
350  m_AllTools.at(i)->SetOrientation(currentNavData->GetOrientation());
351  m_AllTools.at(i)->SetIGTTimeStamp(currentNavData->GetIGTTimeStamp());
352  }
353  }
354  }
355 }
356 
358 {
359  //check tracking state
360  if (this->GetState() != Ready)
361  {
362  MITK_WARN << "Cannot start tracking, device is not ready!";
363  return false;
364  }
365 
366  try
367  {
368  m_IGTLDeviceSource->StartCommunication();
369 
370  //send a message to the server: start tracking stream
371  mitk::IGTLMessageFactory::Pointer msgFactory = m_OpenIGTLinkClient->GetMessageFactory();
372  std::string message = "STT_TDATA";
373  //m_OpenIGTLinkClient->SendMessage(msgFactory->CreateInstance(message));
374  }
375  catch (std::runtime_error &e)
376  {
377  MITK_WARN << "Open IGT Link device retruned an error while starting communication: " << e.what();
378  return false;
379  }
380 
381  //create internal igtl pipeline
383  m_IGTLMsgToNavDataFilter->SetNumberOfExpectedOutputs(this->GetToolCount());
385 
386  //connect itk events
387  typedef itk::SimpleMemberCommand< mitk::OpenIGTLinkTrackingDevice > CurCommandType;
388  CurCommandType::Pointer messageReceivedCommand = CurCommandType::New();
389  messageReceivedCommand->SetCallbackFunction(this, &mitk::OpenIGTLinkTrackingDevice::UpdateTools);
390  m_MessageReceivedObserverTag = m_OpenIGTLinkClient->AddObserver(mitk::MessageReceivedEvent(), messageReceivedCommand);
391 
392  m_OpenIGTLinkClient->EnableNoBufferingMode(true);
393  this->SetState(Tracking);
394  return true;
395 }
396 
398 {
399  //check tracking state
400  if (this->GetState() != Tracking)
401  {
402  MITK_WARN << "Cannot open connection, device is already connected!";
403  return false;
404  }
405 
406  m_OpenIGTLinkClient->RemoveObserver(m_MessageReceivedObserverTag); //disconnect itk events
407 
408  try
409  {
410  m_IGTLDeviceSource->StopCommunication();
411  }
412  catch (std::runtime_error &e)
413  {
414  MITK_WARN << "Open IGT Link device retruned an error while stopping communication: " << e.what();
415  return false;
416  }
417  m_OpenIGTLinkClient->EnableNoBufferingMode(false);
418  this->SetState(Ready);
419  return true;
420 }
421 
423 {
424  return (unsigned int)this->m_AllTools.size();
425 }
426 
428 {
429  if (toolNumber >= this->GetToolCount())
430  return nullptr;
431  else
432  return this->m_AllTools[toolNumber];
433 }
434 
436 {
437  //check tracking state
438  if (this->GetState() != Setup)
439  {
440  MITK_WARN << "Cannot open connection, device is already connected!";
441  return false;
442  }
443 
444  try
445  {
446  m_IGTLDeviceSource->Connect();
447  }
448  catch (std::runtime_error &e)
449  {
450  MITK_WARN << "Open IGT Link device retruned an error while trying to connect: " << e.what();
451  return false;
452  }
453  this->SetState(Ready);
454  return true;
455 }
456 
458 {
459  //check tracking state
460  if (this->GetState() != Ready)
461  {
462  MITK_WARN << "Cannot close connection, device is in the wrong state!";
463  return false;
464  }
465 
466  try
467  {
468  m_IGTLDeviceSource->Disconnect();
469  }
470  catch (std::runtime_error &e)
471  {
472  MITK_WARN << "Open IGT Link device retruned an error while trying to disconnect: " << e.what();
473  return false;
474  }
475 
476  this->SetState(Setup);
477 
478  return true;
479 }
480 
481 std::vector<mitk::OpenIGTLinkTrackingTool::Pointer> mitk::OpenIGTLinkTrackingDevice::GetAllTools()
482 {
483  return this->m_AllTools;
484 }
485 
486 mitk::OpenIGTLinkTrackingDevice::TrackingMessageType mitk::OpenIGTLinkTrackingDevice::GetMessageTypeFromString(const char* messageTypeString)
487 {
488  if (strcmp(messageTypeString, "TDATA") == 0)
489  {
490  return mitk::OpenIGTLinkTrackingDevice::TrackingMessageType::TDATA;
491  }
492  else if (strcmp(messageTypeString, "QTDATA") == 0)
493  {
494  return mitk::OpenIGTLinkTrackingDevice::TrackingMessageType::QTDATA;
495  }
496  else if (strcmp(messageTypeString, "TRANSFORM") == 0)
497  {
498  return mitk::OpenIGTLinkTrackingDevice::TrackingMessageType::TRANSFORM;
499  }
500  else
501  {
502  return mitk::OpenIGTLinkTrackingDevice::TrackingMessageType::UNKNOWN;
503  }
504 }
TrackingTool * GetTool(unsigned int toolNumber) const override
Descibes a line.
Definition: mitkLine.h:28
float k(1.0)
Interface for all Tracking Tools.
#define MITK_INFO
Definition: mitkLogMacros.h:18
#define MITK_ERROR
Definition: mitkLogMacros.h:20
bool DiscoverTools(int WaitingTime=10000)
Discover the tools available from the connected OpenIGTLink device and adds these tools to this track...
bool StopTracking() override
Stops the tracking.
DataCollection - Class to facilitate loading/accessing structured data.
OpenIGTLinkTrackingDevice()
Returns the update rate of the device in fps.
itk::MutexLockHolder< itk::FastMutexLock > MutexLockHolder
mitk::TrackingTool * AddTool(const char *toolName, const char *fileName)
Create a new OpenIGTLink tool with toolName and fileName and add it to the list of tools...
mitk::NavigationToolStorage::Pointer AutoDetectTools() override
TrackingDeviceState GetState() const
return current object state (Setup, Ready or Tracking)
bool OpenConnection() override
Opens the connection to the device. This have to be done before the tracking is started.
#define MITK_WARN
Definition: mitkLogMacros.h:19
std::vector< OpenIGTLinkTrackingTool::Pointer > m_AllTools
vector holding all tools
mitk::IGTLMessage::Pointer ReceiveMessage(int waitingTime)
int m_UpdateRate
holds the update rate in FPS (will be set automatically when the OpenIGTLink connection is establishe...
Interface for all Tracking Devices.
static Pointer New()
Constructs a NavigationToolStorage without reference to a DataStorage. The Data Nodes of tools have t...
mitk::IGTLMessageToNavigationDataFilter::Pointer m_IGTLMsgToNavDataFilter
static TrackingDeviceData GetDeviceDataOpenIGTLinkTrackingDeviceConnection()
mitk::IGTLTrackingDataDeviceSource::Pointer m_IGTLDeviceSource
static Pointer New()
bool StartTracking() override
Starts the tracking.
TrackingDeviceData m_Data
current device Data
bool CloseConnection() override
Closes the connection and clears all resources.
static Pointer New()
std::vector< OpenIGTLinkTrackingTool::Pointer > GetAllTools()
void SetState(TrackingDeviceState state)
change object state
bool InternalAddTool(OpenIGTLinkTrackingTool::Pointer tool)
Adds a tool to the tracking device.