diff --git a/Modules/IGT/TrackingDevices/mitkOpenIGTLinkTrackingDevice.cpp b/Modules/IGT/TrackingDevices/mitkOpenIGTLinkTrackingDevice.cpp
index ed8a5eaa34..1ab9de89c5 100644
--- a/Modules/IGT/TrackingDevices/mitkOpenIGTLinkTrackingDevice.cpp
+++ b/Modules/IGT/TrackingDevices/mitkOpenIGTLinkTrackingDevice.cpp
@@ -1,588 +1,522 @@
 /*===================================================================
 
 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 "mitkOpenIGTLinkTrackingDevice.h"
 #include "mitkOpenIGTLinkTrackingTool.h"
 #include "mitkIGTConfig.h"
 #include "mitkIGTTimeStamp.h"
 #include "mitkIGTHardwareException.h"
 #include "mitkTrackingTypes.h"
 #include <itksys/SystemTools.hxx>
 #include <iostream>
 #include <itkMutexLockHolder.h>
 #include <itkCommand.h>
 #include <mitkOpenIGTLinkTypeInformation.h>
 #include <vtkConeSource.h>
 
 //sleep headers
 #include <chrono>
 #include <thread>
 
 typedef itk::MutexLockHolder<itk::FastMutexLock> MutexLockHolder;
 
 mitk::OpenIGTLinkTrackingDevice::OpenIGTLinkTrackingDevice() : mitk::TrackingDevice(), m_UpdateRate(60)
 {
   //set the type of this tracking device
   this->m_Data = mitk::OpenIGTLinkTypeInformation::GetDeviceDataOpenIGTLinkTrackingDeviceConnection();
 
   m_OpenIGTLinkClient = mitk::IGTLClient::New(true);
   m_OpenIGTLinkClient->SetName("OpenIGTLink Tracking Device");
 
   m_IGTLDeviceSource = mitk::IGTLTransformDeviceSource::New();
   m_IGTLDeviceSource->SetIGTLDevice(m_OpenIGTLinkClient);
 }
 
 mitk::OpenIGTLinkTrackingDevice::~OpenIGTLinkTrackingDevice()
 {
 }
 
 int mitk::OpenIGTLinkTrackingDevice::GetPortNumber()
 {
   return m_OpenIGTLinkClient->GetPortNumber();
 }
 
 bool mitk::OpenIGTLinkTrackingDevice::AutoDetectToolsAvailable()
 {
   return true;
 }
 
 mitk::NavigationToolStorage::Pointer mitk::OpenIGTLinkTrackingDevice::AutoDetectTools()
 {
   mitk::NavigationToolStorage::Pointer returnValue = mitk::NavigationToolStorage::New();
 
   if (m_OpenIGTLinkClient->GetPortNumber() == -1)
   {
     MITK_WARN << "Connection not initialized, aborting (invalid port number).";
     return mitk::NavigationToolStorage::New();
   }
 
   //open connection
   try
   {
     m_IGTLDeviceSource->Connect();
     m_IGTLDeviceSource->StartCommunication();
   }
   catch (std::runtime_error &e)
   {
     MITK_WARN << "AutoDetection: Open IGT Link device retruned an error while trying to connect: " << e.what();
     return mitk::NavigationToolStorage::New();
   }
 
-  mitk::IGTLMessage::Pointer receivedMessage = ReceiveMessage(10000);
-
-  //code for transform messages
-
-  igtl::TransformMessage::Pointer msg = dynamic_cast<igtl::TransformMessage*>(m_IGTLDeviceSource->GetOutput()->GetMessage().GetPointer());
-  if (msg == nullptr || msg.IsNull())
-  {
-    MITK_INFO << "Message could not be casted to TransformMessage. Aborting..";
-    return mitk::NavigationToolStorage::New();
-  }
-
-  std::map<std::string, int> toolNameMap;
-  int count = toolNameMap[msg->GetDeviceName()];
-  if (count == 0)
-  {
-    MITK_WARN << "ADDED NEW TOOL TO TOOLCHAIN: " << msg->GetDeviceName() << " - 1";
-    toolNameMap[msg->GetDeviceName()] = 1;
-  }
-  else
-  {
-    toolNameMap[msg->GetDeviceName()]++;
-    MITK_WARN << "INCREMENTED TOOL COUNT IN TOOLCHAIN: " << msg->GetDeviceName() << " - " << toolNameMap[msg->GetDeviceName()];
-  }
-
-  for (std::map<std::string, int>::iterator it = toolNameMap.begin(); it != toolNameMap.end(); ++it)
-  {
-    if (it->second < 5)
-    {
-      break;
-    }
-  }
-
-int i = 0;
-for (std::map<std::string, int>::iterator it = toolNameMap.begin(); it != toolNameMap.end(); ++it)
-{
-  MITK_INFO << "Found tool: " << it->first;
-
-  mitk::DataNode::Pointer newNode = mitk::DataNode::New();
-  std::stringstream name;
-  name << it->first;
-  newNode->SetName(name.str());
-
-  mitk::Surface::Pointer myCone = mitk::Surface::New();
-  vtkConeSource *vtkData = vtkConeSource::New();
-  vtkData->SetAngle(5.0);
-  vtkData->SetResolution(50);
-  vtkData->SetHeight(6.0f);
-  vtkData->SetRadius(2.0f);
-  vtkData->SetCenter(0.0, 0.0, 0.0);
-  vtkData->Update();
-  myCone->SetVtkPolyData(vtkData->GetOutput());
-  vtkData->Delete();
-  newNode->SetData(myCone);
-
-  mitk::NavigationTool::Pointer newTool = mitk::NavigationTool::New();
-  newTool->SetDataNode(newNode);
-
-  std::stringstream identifier;
-  identifier << "AutoDetectedTool-" << i;
-  i++;
-  newTool->SetIdentifier(identifier.str());
-
-  newTool->SetTrackingDeviceType(mitk::OpenIGTLinkTypeInformation::GetDeviceDataOpenIGTLinkTrackingDeviceConnection().Line);
-  returnValue->AddTool(newTool);
-}
-
-
-  //check the tracking stream for the number and type of tools
-  //igtl::MessageBase::Pointer receivedMessage = m_OpenIGTLinkClient->GetNextMessage();
-  if (receivedMessage.IsNull())
-  {
-    MITK_WARN << "AutoDetection: No message was received. Is there really a server?";
-    return mitk::NavigationToolStorage::New();
-  }
-  else if (!receivedMessage->IsDataValid())
-  {
-    MITK_WARN << "AutoDetection: Received invalid message.";
-    return mitk::NavigationToolStorage::New();
-  }
-
-  const char* msgType = receivedMessage->GetIGTLMessageType();
-
-  MITK_INFO << "AutoDetection: Received message of type " << msgType;
+  returnValue = DiscoverToolsFromTransform();
 
   //close connection
   try
   {
     m_IGTLDeviceSource->StopCommunication();
     m_IGTLDeviceSource->Disconnect();
   }
   catch (std::runtime_error &e)
   {
     MITK_WARN << "AutoDetection: Open IGT Link device retruned an error while trying to disconnect: " << e.what();
     return mitk::NavigationToolStorage::New();
   }
 
   return returnValue;
 }
 
 std::string mitk::OpenIGTLinkTrackingDevice::GetHostname()
 {
   return m_OpenIGTLinkClient->GetHostname();
 }
 
 void mitk::OpenIGTLinkTrackingDevice::SetPortNumber(int portNumber)
 {
   m_OpenIGTLinkClient->SetPortNumber(portNumber);
 }
 
 void mitk::OpenIGTLinkTrackingDevice::SetHostname(std::string hostname)
 {
   m_OpenIGTLinkClient->SetHostname(hostname);
 }
 
 bool mitk::OpenIGTLinkTrackingDevice::IsDeviceInstalled()
 {
   return true;
 }
 
 mitk::TrackingTool* mitk::OpenIGTLinkTrackingDevice::AddTool(const char* toolName, const char* fileName)
 {
   mitk::OpenIGTLinkTrackingTool::Pointer t;// = mitk::OpenIGTLinkTrackingTool::New();
   //TODO: Implement
   if (this->InternalAddTool(t) == false)
     return NULL;
   return t.GetPointer();
 }
 
 bool mitk::OpenIGTLinkTrackingDevice::InternalAddTool(OpenIGTLinkTrackingTool::Pointer tool)
 {
   m_AllTools.push_back(tool);
   return true;
 }
 
 bool mitk::OpenIGTLinkTrackingDevice::DiscoverTools(int waitingTime)
 {
   if (m_OpenIGTLinkClient->GetPortNumber() == -1)
   {
     MITK_WARN << "Connection not initialized, aborting (invalid port number).";
     return false;
   }
 
   try
   {
     m_IGTLDeviceSource->Connect();
     m_IGTLDeviceSource->StartCommunication();
   }
   catch (std::runtime_error &e)
   {
     MITK_WARN << "Open IGT Link device retruned an error while trying to connect: " << e.what();
     return false;
   }
 
   mitk::IGTLMessage::Pointer receivedMessage = ReceiveMessage(waitingTime);
 
   //check the tracking stream for the number and type of tools
   //igtl::MessageBase::Pointer receivedMessage = m_OpenIGTLinkClient->GetNextMessage();
   if (receivedMessage.IsNull())
   {
     MITK_WARN << "No message was received. Is there really a server?";
     return false;
   }
   else if (!receivedMessage->IsDataValid())
   {
     MITK_WARN << "Received invalid message.";
     return false;
   }
 
   const char* msgType = receivedMessage->GetIGTLMessageType();
 
   mitk::OpenIGTLinkTrackingDevice::TrackingMessageType type = GetMessageTypeFromString(msgType);
 
   switch (type)
   {
   case TDATA:
     return DiscoverToolsFromTData(dynamic_cast<igtl::TrackingDataMessage*>(receivedMessage->GetMessage().GetPointer()));
   case QTDATA:
     return DiscoverToolsFromQTData(dynamic_cast<igtl::QuaternionTrackingDataMessage*>(receivedMessage->GetMessage().GetPointer()));
   case TRANSFORM:
-    return DiscoverToolsFromTransform();
+    {
+    mitk::NavigationToolStorage::Pointer foundTools = DiscoverToolsFromTransform(200);
+    if (foundTools->GetToolCount() == 0) { return false; }
+    for (int i = 0; i < foundTools->GetToolCount(); i++) { AddNewToolForName(foundTools->GetTool(i)->GetToolName(), i); }
+    return true;
+    }
   default:
     MITK_INFO << "Server does not send tracking data. Received data is not of a compatible type. Received type: " << msgType;
     return false;
   }
 }
 
 mitk::IGTLMessage::Pointer mitk::OpenIGTLinkTrackingDevice::ReceiveMessage(int waitingTime)
 {
   mitk::IGTLMessage::Pointer receivedMessage;
   //send a message to the server: start tracking stream
   mitk::IGTLMessageFactory::Pointer msgFactory = m_OpenIGTLinkClient->GetMessageFactory();
   std::string message = "STT_TDATA";
   igtl::MessageBase::Pointer sttMsg = msgFactory->CreateInstance(message);
   //TODO: Fix this to dynamically get this from GUI
   ((igtl::StartTrackingDataMessage*)sttMsg.GetPointer())->SetResolution(m_UpdateRate);
   m_OpenIGTLinkClient->SendMessage(sttMsg);
 
 
 
   std::chrono::high_resolution_clock::time_point time = std::chrono::high_resolution_clock::now();
   std::chrono::milliseconds d = std::chrono::milliseconds(waitingTime);
 
   while (!(receivedMessage.IsNotNull() && receivedMessage->IsDataValid()))
   {
     m_IGTLDeviceSource->Update();
     receivedMessage = m_IGTLDeviceSource->GetOutput();
 
     if ((time + d) < std::chrono::high_resolution_clock::now())
       break;
 
     std::this_thread::sleep_for(std::chrono::milliseconds(100));
   }
   return receivedMessage;
 
 }
 
 bool mitk::OpenIGTLinkTrackingDevice::DiscoverToolsFromTData(igtl::TrackingDataMessage::Pointer tdMsg)
 {
   MITK_INFO << "Start discovering tools by TDATA messages";
   if (tdMsg == nullptr)
   {
     MITK_WARN << "Message was not a TrackingDataMessage, aborting!";
     return false;
   }
 
   int numberOfTools = tdMsg->GetNumberOfTrackingDataElements();
   MITK_INFO << "Found " << numberOfTools << " tools";
   for (int i = 0; i < numberOfTools; i++)
   {
     igtl::TrackingDataElement::Pointer currentTrackingData;
     tdMsg->GetTrackingDataElement(i, currentTrackingData);
     std::string name = currentTrackingData->GetName();
     AddNewToolForName(name, i);
   }
 
   m_IGTLDeviceSource->StopCommunication();
   SetState(Ready);
   return true;
 }
 
 bool mitk::OpenIGTLinkTrackingDevice::DiscoverToolsFromQTData(igtl::QuaternionTrackingDataMessage::Pointer msg)
 {
   MITK_INFO << "Start discovering tools by QTDATA messages";
   if (msg == nullptr)
   {
     MITK_WARN << "Message was not a QuaternionTrackingDataMessage, aborting!";
     return false;
   }
   int numberOfTools = msg->GetNumberOfQuaternionTrackingDataElements();
   MITK_INFO << "Found " << numberOfTools << " tools";
   for (int i = 0; i < numberOfTools; i++)
   {
     igtl::QuaternionTrackingDataElement::Pointer currentTrackingData;
     msg->GetQuaternionTrackingDataElement(i, currentTrackingData);
     std::string name = currentTrackingData->GetName();
     AddNewToolForName(name, i);
   }
   m_IGTLDeviceSource->StopCommunication();
   SetState(Ready);
   return true;
 }
 
 void mitk::OpenIGTLinkTrackingDevice::AddNewToolForName(std::string name, int i)
 {
   mitk::OpenIGTLinkTrackingTool::Pointer newTool = mitk::OpenIGTLinkTrackingTool::New();
   if (name == "") //if no name was given create a default name
   {
     std::stringstream defaultName;
     defaultName << "OpenIGTLinkTool#" << i;
     name = defaultName.str();
   }
   MITK_INFO << "Added tool " << name << " to tracking device.";
   newTool->SetToolName(name);
   InternalAddTool(newTool);
 }
 
-bool mitk::OpenIGTLinkTrackingDevice::DiscoverToolsFromTransform()
+mitk::NavigationToolStorage::Pointer mitk::OpenIGTLinkTrackingDevice::DiscoverToolsFromTransform(int NumberOfMessagesToWait)
 {
   MITK_INFO << "Start discovering tools by TRANSFORM messages";
+  mitk::NavigationToolStorage::Pointer returnValue = mitk::NavigationToolStorage::New();
   std::map<std::string, int> toolNameMap;
-  bool condition = false;
-  int j = 0;
-  while (!condition)
+
+  for (int j=0; j<NumberOfMessagesToWait; j++)
   {
-    //TODO: Fix this.. :/
     std::this_thread::sleep_for(std::chrono::milliseconds(20));
     m_IGTLDeviceSource->Update();
     igtl::TransformMessage::Pointer msg = dynamic_cast<igtl::TransformMessage*>(m_IGTLDeviceSource->GetOutput()->GetMessage().GetPointer());
     if (msg == nullptr || msg.IsNull())
     {
       MITK_INFO << "Received message could not be casted to TransformMessage. Skipping..";
       continue;
     }
 
-    condition = true;
     int count = toolNameMap[msg->GetDeviceName()];
     if (count == 0)
     {
-      MITK_WARN << "ADDED NEW TOOL TO TOOLCHAIN: " << msg->GetDeviceName() << " - 1";
+      //MITK_WARN << "ADDED NEW TOOL TO TOOLCHAIN: " << msg->GetDeviceName() << " - 1";
       toolNameMap[msg->GetDeviceName()] = 1;
     }
     else
     {
       toolNameMap[msg->GetDeviceName()]++;
-      MITK_WARN << "INCREMENTED TOOL COUNT IN TOOLCHAIN: " << msg->GetDeviceName() << " - " << toolNameMap[msg->GetDeviceName()];
+      //MITK_WARN << "INCREMENTED TOOL COUNT IN TOOLCHAIN: " << msg->GetDeviceName() << " - " << toolNameMap[msg->GetDeviceName()];
     }
-
-    j++;
-    if (j > 100) { condition = true; }
-    else { condition = false; }
-    /*
-    for (std::map<std::string, int>::iterator it = toolNameMap.begin(); it != toolNameMap.end(); ++it)
-    {
-      //if (it->second < 5)
-      if (i>100)
-      {
-        condition = false;
-        break;
-      }
-    }
-    */
   }
 
   int i = 0;
   for (std::map<std::string, int>::iterator it = toolNameMap.begin(); it != toolNameMap.end(); ++it)
   {
-    AddNewToolForName(it->first, i++);
+    MITK_INFO << "Found tool: " << it->first;
+
+    mitk::DataNode::Pointer newNode = mitk::DataNode::New();
+    std::stringstream name;
+    name << it->first;
+    newNode->SetName(name.str());
+
+    mitk::Surface::Pointer myCone = mitk::Surface::New();
+    vtkConeSource *vtkData = vtkConeSource::New();
+    vtkData->SetAngle(5.0);
+    vtkData->SetResolution(50);
+    vtkData->SetHeight(6.0f);
+    vtkData->SetRadius(2.0f);
+    vtkData->SetCenter(0.0, 0.0, 0.0);
+    vtkData->Update();
+    myCone->SetVtkPolyData(vtkData->GetOutput());
+    vtkData->Delete();
+    newNode->SetData(myCone);
+
+    mitk::NavigationTool::Pointer newTool = mitk::NavigationTool::New();
+    newTool->SetDataNode(newNode);
+
+    std::stringstream identifier;
+    identifier << "AutoDetectedTool-" << i;
+    i++;
+    newTool->SetIdentifier(identifier.str());
+
+    newTool->SetTrackingDeviceType(mitk::OpenIGTLinkTypeInformation::GetDeviceDataOpenIGTLinkTrackingDeviceConnection().Line);
+    returnValue->AddTool(newTool);
   }
-  //TODO InternalAddTool for all tools
 
-  return true;
+
+  return returnValue;
 }
 
 void mitk::OpenIGTLinkTrackingDevice::UpdateTools()
 {
   if (this->GetState() != Tracking)
   {
     MITK_ERROR << "Method was called in the wrong state, something went wrong!";
     return;
   }
 
   m_IGTLMsgToNavDataFilter->Update();
 
   mitk::NavigationData::Pointer currentNavData = m_IGTLMsgToNavDataFilter->GetOutput();
   const char* name = currentNavData->GetName();
   //MITK_WARN << name;
 
   for (int i = 0; i < m_AllTools.size(); i++)
   {
     if (strcmp(m_AllTools.at(i)->GetToolName(), name) == 0)
     {
       m_AllTools.at(i)->SetDataValid(currentNavData->IsDataValid());
       m_AllTools.at(i)->SetPosition(currentNavData->GetPosition());
       m_AllTools.at(i)->SetOrientation(currentNavData->GetOrientation());
       m_AllTools.at(i)->SetIGTTimeStamp(currentNavData->GetIGTTimeStamp());
     }
   }
 }
 
 bool mitk::OpenIGTLinkTrackingDevice::StartTracking()
 {
   //check tracking state
   if (this->GetState() != Ready)
   {
     MITK_WARN << "Cannot start tracking, device is not ready!";
     return false;
   }
 
   try
   {
     m_IGTLDeviceSource->StartCommunication();
 
     //send a message to the server: start tracking stream
     mitk::IGTLMessageFactory::Pointer msgFactory = m_OpenIGTLinkClient->GetMessageFactory();
     std::string message = "STT_TDATA";
     //m_OpenIGTLinkClient->SendMessage(msgFactory->CreateInstance(message));
   }
   catch (std::runtime_error &e)
   {
     MITK_WARN << "Open IGT Link device retruned an error while starting communication: " << e.what();
     return false;
   }
 
   //create internal igtl pipeline
   m_IGTLMsgToNavDataFilter = mitk::IGTLMessageToNavigationDataFilter::New();
   m_IGTLMsgToNavDataFilter->SetNumberOfExpectedOutputs(this->GetToolCount());
   m_IGTLMsgToNavDataFilter->ConnectTo(m_IGTLDeviceSource);
 
   //connect itk events
   typedef itk::SimpleMemberCommand< mitk::OpenIGTLinkTrackingDevice > CurCommandType;
   CurCommandType::Pointer messageReceivedCommand = CurCommandType::New();
   messageReceivedCommand->SetCallbackFunction(this, &mitk::OpenIGTLinkTrackingDevice::UpdateTools);
   m_MessageReceivedObserverTag = m_OpenIGTLinkClient->AddObserver(mitk::MessageReceivedEvent(), messageReceivedCommand);
 
   this->SetState(Tracking);
   return true;
 }
 
 bool mitk::OpenIGTLinkTrackingDevice::StopTracking()
 {
   //check tracking state
   if (this->GetState() != Tracking)
   {
     MITK_WARN << "Cannot open connection, device is already connected!";
     return false;
   }
 
   m_OpenIGTLinkClient->RemoveObserver(m_MessageReceivedObserverTag); //disconnect itk events
 
   try
   {
     m_IGTLDeviceSource->StopCommunication();
   }
   catch (std::runtime_error &e)
   {
     MITK_WARN << "Open IGT Link device retruned an error while stopping communication: " << e.what();
     return false;
   }
   this->SetState(Ready);
   return true;
 }
 
 unsigned int mitk::OpenIGTLinkTrackingDevice::GetToolCount() const
 {
   return (unsigned int)this->m_AllTools.size();
 }
 
 mitk::TrackingTool* mitk::OpenIGTLinkTrackingDevice::GetTool(unsigned int toolNumber) const
 {
   if (toolNumber >= this->GetToolCount())
     return NULL;
   else
     return this->m_AllTools[toolNumber];
 }
 
 bool mitk::OpenIGTLinkTrackingDevice::OpenConnection()
 {
   //check tracking state
   if (this->GetState() != Setup)
   {
     MITK_WARN << "Cannot open connection, device is already connected!";
     return false;
   }
 
   try
   {
     m_IGTLDeviceSource->Connect();
   }
   catch (std::runtime_error &e)
   {
     MITK_WARN << "Open IGT Link device retruned an error while trying to connect: " << e.what();
     return false;
   }
   this->SetState(Ready);
   return true;
 }
 
 bool mitk::OpenIGTLinkTrackingDevice::CloseConnection()
 {
   //check tracking state
   if (this->GetState() != Ready)
   {
     MITK_WARN << "Cannot close connection, device is in the wrong state!";
     return false;
   }
 
   try
   {
     m_IGTLDeviceSource->Disconnect();
   }
   catch (std::runtime_error &e)
   {
     MITK_WARN << "Open IGT Link device retruned an error while trying to disconnect: " << e.what();
     return false;
   }
 
   this->SetState(Setup);
 
   return true;
 }
 
 std::vector<mitk::OpenIGTLinkTrackingTool::Pointer> mitk::OpenIGTLinkTrackingDevice::GetAllTools()
 {
   return this->m_AllTools;
 }
 
 mitk::OpenIGTLinkTrackingDevice::TrackingMessageType mitk::OpenIGTLinkTrackingDevice::GetMessageTypeFromString(const char* messageTypeString)
 {
   if (strcmp(messageTypeString, "TDATA") == 0)
   {
     return mitk::OpenIGTLinkTrackingDevice::TrackingMessageType::TDATA;
   }
   else if (strcmp(messageTypeString, "QTDATA") == 0)
   {
     return mitk::OpenIGTLinkTrackingDevice::TrackingMessageType::QTDATA;
   }
   else if (strcmp(messageTypeString, "TRANSFORM") == 0)
   {
     return mitk::OpenIGTLinkTrackingDevice::TrackingMessageType::TRANSFORM;
   }
   else
   {
     return mitk::OpenIGTLinkTrackingDevice::TrackingMessageType::UNKNOWN;
   }
 }
\ No newline at end of file
diff --git a/Modules/IGT/TrackingDevices/mitkOpenIGTLinkTrackingDevice.h b/Modules/IGT/TrackingDevices/mitkOpenIGTLinkTrackingDevice.h
index fd1c494e9e..0c701ae10a 100644
--- a/Modules/IGT/TrackingDevices/mitkOpenIGTLinkTrackingDevice.h
+++ b/Modules/IGT/TrackingDevices/mitkOpenIGTLinkTrackingDevice.h
@@ -1,179 +1,180 @@
 /*===================================================================
 
 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.
 
 ===================================================================*/
 
 #ifndef MITKOPENIGTLINKTRACKINGDEVICE_H_HEADER_INCLUDED_
 #define MITKOPENIGTLINKTRACKINGDEVICE_H_HEADER_INCLUDED_
 
 #include <mitkIGTConfig.h>
 #include <mitkTrackingDevice.h>
 #include <mitkOpenIGTLinkTrackingTool.h>
 #include <mitkIGTLClient.h>
 #include <mitkIGTLDeviceSource.h>
 #include <mitkIGTLMessageToNavigationDataFilter.h>
 #include <itkMultiThreader.h>
 #include <igtlQuaternionTrackingDataMessage.h>
 #include <igtlTrackingDataMessage.h>
 #include <igtlTransformMessage.h>
 #include "mitkIGTLTransformDeviceSource.h"
 
 namespace mitk
 {
   /** Documentation:
   *   \brief An object of this class represents the MicronTracker device. You can add tools to this
   *          device, then open the connection and start tracking. The tracking device will then
   *          continuously update the tool coordinates.
   *   \ingroup IGT
   */
   class MITKIGT_EXPORT OpenIGTLinkTrackingDevice : public TrackingDevice
   {
   public:
     mitkClassMacro(OpenIGTLinkTrackingDevice, TrackingDevice);
     itkFactorylessNewMacro(Self)
       itkCloneMacro(Self)
 
       /** Sets the port number for the Open IGT Link connection. Default value is -1 (invalid). */
       void SetPortNumber(int portNumber);
 
     /** Sets the hostname for the Open IGT Link connection. Default value is 127.0.0.1 (localhost). */
     void SetHostname(std::string hostname);
 
     int GetPortNumber();
 
     std::string GetHostname();
 
     /**
     * \brief Starts the tracking.
     * \return Returns true if the tracking is started. Throws an exception if an error occures.
     * @throw mitk::IGTHardwareException Throws an exception if there is an error during start tracking.
     */
     virtual bool StartTracking();
 
     /**
     * \brief Stops the tracking.
     * \return Returns true if the tracking is stopped.
     */
     virtual bool StopTracking();
 
     /**
     * \brief Opens the connection to the device. This have to be done before the tracking is started.
     * @throw mitk::IGTHardwareException Throws an exception if there is an error during open connection.
     */
     virtual bool OpenConnection();
 
     /**
     * \brief Closes the connection and clears all resources.
     */
     virtual bool CloseConnection();
 
     /**
     * \return Returns the number of tools which have been added to the device.
     */
     virtual unsigned int GetToolCount() const;
 
     /**
     * \param toolNumber The number of the tool which should be given back.
     * \return Returns the tool which the number "toolNumber". Returns NULL, if there is
     * no tool with this number.
     */
     TrackingTool* GetTool(unsigned int toolNumber)  const;
 
     /**
     * \brief Discover the tools available from the connected OpenIGTLink device and adds these tools to this tracking device. Therefore, a connection
     *        is opened, the tools are discovered and added.
     * \param WaitingTime Defines how long the method waits for an answer from the server (in milliseconds). Default value is 10000 (10 seconds).
     * \return Returns true if the connection was established and the tools were discovered successfully and - if at least one tool was found - were added to this device.
     *         Retruns false if no valid connection is available.
     */
     bool DiscoverTools(int WaitingTime = 10000);
 
     /**
     * \brief Create a new OpenIGTLink tool with toolName and fileName and add it to the list of tools
     *
     * Note that tools are usually provided by the OpenIGTLink connection. In most cases, the method DiscoverTools() should be used
     * instead which automatically finds the provided tools. If you use this method to manually add tools be sure that you add the
     * same number and type of tools that are provided by the connected device. Otherwise problems might occur when you try to start
     * tracking.
     */
     mitk::TrackingTool* AddTool(const char* toolName, const char* fileName);
 
     /** @return Returns true if this device can autodetects its tools. */
     virtual bool AutoDetectToolsAvailable();
 
     /** Autodetects tools from the current OpenIGTLink connection and returns them as a navigation tool storage.
     *  @return Returns the detected tools. Returns an empty storage if no tools are present
     *          or if OpenIGTLink Connection is not possible
     */
     virtual mitk::NavigationToolStorage::Pointer AutoDetectTools();
 
     bool IsDeviceInstalled();
 
     itkSetMacro(UpdateRate, int);               ///< Sets the update rate of the device in fps. Default value is 60 fps.
     itkGetConstMacro(UpdateRate, int);          ///< Returns the update rate of the device in fps
 
   protected:
     OpenIGTLinkTrackingDevice();
     ~OpenIGTLinkTrackingDevice();
 
     /**
     * \brief Adds a tool to the tracking device.
     *
     * \param tool  The tool which will be added.
     * \return Returns true if the tool has been added, false otherwise.
     */
     bool InternalAddTool(OpenIGTLinkTrackingTool::Pointer tool);
 
     /** Updates the tools from the open IGT link connection. Is called every time a message received event is invoked.*/
     void UpdateTools();
     unsigned long m_MessageReceivedObserverTag;
 
     /** Receives one message from the OpenIGTLink connection. Starts the tracking stream if required.
      */
     mitk::IGTLMessage::Pointer ReceiveMessage(int waitingTime);
 
     /**
     * \return Returns all tools of the tracking device.
     */
     std::vector<OpenIGTLinkTrackingTool::Pointer> GetAllTools();
 
     //OpenIGTLink connection class
     mitk::IGTLClient::Pointer m_OpenIGTLinkClient;
 
     //OpenIGTLink pipeline
     mitk::IGTLTransformDeviceSource::Pointer m_IGTLDeviceSource;
     mitk::IGTLMessageToNavigationDataFilter::Pointer m_IGTLMsgToNavDataFilter;
 
     std::vector<OpenIGTLinkTrackingTool::Pointer> m_AllTools; ///< vector holding all tools
 
     int m_UpdateRate; ///< holds the update rate in FPS (will be set automatically when the OpenIGTLink connection is established)
 
   private:
     enum TrackingMessageType
     {
       TDATA, TRANSFORM, QTDATA, UNKNOWN
     };
 
     mitk::OpenIGTLinkTrackingDevice::TrackingMessageType GetMessageTypeFromString(const char* messageTypeString);
 
     bool DiscoverToolsFromTData(igtl::TrackingDataMessage::Pointer msg);
 
     bool DiscoverToolsFromQTData(igtl::QuaternionTrackingDataMessage::Pointer msg);
 
-    bool DiscoverToolsFromTransform();
+    /** Discovers tools from the input and waits for the given number of messages */
+    mitk::NavigationToolStorage::Pointer DiscoverToolsFromTransform(int NumberOfMessagesToWait = 100);
 
     void AddNewToolForName(std::string name, int i);
   };
 }//mitk
 #endif /* MITKOpenIGTLinkTRACKINGDEVICE_H_HEADER_INCLUDED_ */