diff --git a/Modules/IGT/TrackingDevices/mitkOpenIGTLinkTrackingDevice.cpp b/Modules/IGT/TrackingDevices/mitkOpenIGTLinkTrackingDevice.cpp
index 1ab9de89c5..2316bad5d8 100644
--- a/Modules/IGT/TrackingDevices/mitkOpenIGTLinkTrackingDevice.cpp
+++ b/Modules/IGT/TrackingDevices/mitkOpenIGTLinkTrackingDevice.cpp
@@ -1,522 +1,553 @@
 /*===================================================================
 
 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_OpenIGTLinkClient->EnableInfiniteBufferingMode(false);
 
   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();
   }
 
   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);
 
+  mitk::NavigationToolStorage::Pointer foundTools;
   switch (type)
   {
   case TDATA:
-    return DiscoverToolsFromTData(dynamic_cast<igtl::TrackingDataMessage*>(receivedMessage->GetMessage().GetPointer()));
+    foundTools = DiscoverToolsFromTData(dynamic_cast<igtl::TrackingDataMessage*>(receivedMessage->GetMessage().GetPointer()));
+    break;
   case QTDATA:
-    return DiscoverToolsFromQTData(dynamic_cast<igtl::QuaternionTrackingDataMessage*>(receivedMessage->GetMessage().GetPointer()));
+    foundTools = DiscoverToolsFromQTData(dynamic_cast<igtl::QuaternionTrackingDataMessage*>(receivedMessage->GetMessage().GetPointer()));
+    break;
   case TRANSFORM:
-    {
-    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;
-    }
+    foundTools = DiscoverToolsFromTransform();
+    break;
   default:
     MITK_INFO << "Server does not send tracking data. Received data is not of a compatible type. Received type: " << msgType;
     return false;
   }
+  if (foundTools.IsNull() || (foundTools->GetToolCount() == 0)) { return false; }
+  for (int i = 0; i < foundTools->GetToolCount(); i++) { AddNewToolForName(foundTools->GetTool(i)->GetToolName(), i); }
+  MITK_INFO << "Found tools: " << foundTools->GetToolCount();
+  return true;
 }
 
 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::NavigationToolStorage::Pointer mitk::OpenIGTLinkTrackingDevice::DiscoverToolsFromTData(igtl::TrackingDataMessage::Pointer tdMsg)
 {
+  mitk::NavigationToolStorage::Pointer returnValue = mitk::NavigationToolStorage::New();
   MITK_INFO << "Start discovering tools by TDATA messages";
   if (tdMsg == nullptr)
   {
     MITK_WARN << "Message was not a TrackingDataMessage, aborting!";
-    return false;
+    return returnValue;
   }
 
   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);
+
+    std::stringstream identifier;
+    identifier << "AutoDetectedTool-" << i;
+    i++;
+
+    mitk::NavigationTool::Pointer newTool = ConstructDefaultOpenIGTLinkTool(name, identifier.str());
+
+    returnValue->AddTool(newTool);
   }
 
   m_IGTLDeviceSource->StopCommunication();
   SetState(Ready);
-  return true;
+  return returnValue;
 }
 
-bool mitk::OpenIGTLinkTrackingDevice::DiscoverToolsFromQTData(igtl::QuaternionTrackingDataMessage::Pointer msg)
+mitk::NavigationToolStorage::Pointer mitk::OpenIGTLinkTrackingDevice::DiscoverToolsFromQTData(igtl::QuaternionTrackingDataMessage::Pointer msg)
 {
+  mitk::NavigationToolStorage::Pointer returnValue = mitk::NavigationToolStorage::New();
   MITK_INFO << "Start discovering tools by QTDATA messages";
   if (msg == nullptr)
   {
     MITK_WARN << "Message was not a QuaternionTrackingDataMessage, aborting!";
-    return false;
+    return returnValue;
   }
   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);
+
+    std::stringstream identifier;
+    identifier << "AutoDetectedTool-" << i;
+    i++;
+
+    mitk::NavigationTool::Pointer newTool = ConstructDefaultOpenIGTLinkTool(name, identifier.str());
+
+    returnValue->AddTool(newTool);
   }
   m_IGTLDeviceSource->StopCommunication();
   SetState(Ready);
-  return true;
+  return returnValue;
 }
 
 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);
 }
 
 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;
 
   for (int j=0; j<NumberOfMessagesToWait; j++)
   {
     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;
     }
 
     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()];
     }
   }
 
   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);
+    mitk::NavigationTool::Pointer newTool = ConstructDefaultOpenIGTLinkTool(name.str(), identifier.str());
+
     returnValue->AddTool(newTool);
   }
 
-
   return returnValue;
 }
 
+mitk::NavigationTool::Pointer mitk::OpenIGTLinkTrackingDevice::ConstructDefaultOpenIGTLinkTool(std::string name, std::string identifier)
+{
+  mitk::DataNode::Pointer newNode = mitk::DataNode::New();
+
+  newNode->SetName(name);
+
+  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);
+  newTool->SetIdentifier(identifier);
+
+  newTool->SetTrackingDeviceType(mitk::OpenIGTLinkTypeInformation::GetDeviceDataOpenIGTLinkTrackingDeviceConnection().Line);
+
+  return newTool;
+
+}
+
 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 0c701ae10a..7b70f36af2 100644
--- a/Modules/IGT/TrackingDevices/mitkOpenIGTLinkTrackingDevice.h
+++ b/Modules/IGT/TrackingDevices/mitkOpenIGTLinkTrackingDevice.h
@@ -1,180 +1,184 @@
 /*===================================================================
 
 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);
+    /** Discovers tools from the input (type TDATA) */
+    mitk::NavigationToolStorage::Pointer DiscoverToolsFromTData(igtl::TrackingDataMessage::Pointer msg);
 
-    bool DiscoverToolsFromQTData(igtl::QuaternionTrackingDataMessage::Pointer msg);
+    /** Discovers tools from the input (type QTDATA) */
+    mitk::NavigationToolStorage::Pointer DiscoverToolsFromQTData(igtl::QuaternionTrackingDataMessage::Pointer msg);
 
-    /** Discovers tools from the input and waits for the given number of messages */
-    mitk::NavigationToolStorage::Pointer DiscoverToolsFromTransform(int NumberOfMessagesToWait = 100);
+    /** Discovers tools from the input (type TRANSFORM) and waits for the given number of messages */
+    mitk::NavigationToolStorage::Pointer DiscoverToolsFromTransform(int NumberOfMessagesToWait = 50);
 
     void AddNewToolForName(std::string name, int i);
+
+    mitk::NavigationTool::Pointer ConstructDefaultOpenIGTLinkTool(std::string name, std::string identifier);
   };
 }//mitk
 #endif /* MITKOpenIGTLinkTRACKINGDEVICE_H_HEADER_INCLUDED_ */
diff --git a/Modules/OpenIGTLink/mitkIGTLDevice.cpp b/Modules/OpenIGTLink/mitkIGTLDevice.cpp
index 54d8e2b87e..823b809c72 100644
--- a/Modules/OpenIGTLink/mitkIGTLDevice.cpp
+++ b/Modules/OpenIGTLink/mitkIGTLDevice.cpp
@@ -1,562 +1,566 @@
 /*===================================================================
 
 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
         {
           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(const mitk::IGTLMessage* msg)
 {
   this->SendMessage(msg->GetMessage());
 }
 
 void mitk::IGTLDevice::SendMessage(igtl::MessageBase::Pointer msg)
 {
   //add the message to the queue
   m_MessageQueue->PushSendMessage(msg);
 }
 
 unsigned int mitk::IGTLDevice::SendMessagePrivate(igtl::MessageBase::Pointer msg,
   igtl::Socket::Pointer socket)
 {
   //check the input message
   if (msg.IsNull())
   {
     MITK_ERROR("IGTLDevice") << "Could not send message because message is not "
       "valid. Please check.";
     return false;
   }
 
   // add the name of this device to the message
   msg->SetDeviceName(this->GetName().c_str());
 
   // Pack (serialize) and send
   msg->Pack();
 
   int sendSuccess = socket->Send(msg->GetPackPointer(), msg->GetPackSize());
 
   if (sendSuccess)
   {
     this->InvokeEvent(MessageSentEvent());
     return IGTL_STATUS_OK;
   }
   else
   {
     return IGTL_STATUS_UNKNOWN_ERROR;
   }
 }
 
 void mitk::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(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/mitkIGTLDevice.h b/Modules/OpenIGTLink/mitkIGTLDevice.h
index f337a304f2..30f34abb37 100644
--- a/Modules/OpenIGTLink/mitkIGTLDevice.h
+++ b/Modules/OpenIGTLink/mitkIGTLDevice.h
@@ -1,417 +1,419 @@
 /*===================================================================
 
 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 MITKIGTLDEVICE_H
 #define MITKIGTLDEVICE_H
 
 #include "mitkCommon.h"
 
 //itk
 #include "itkObject.h"
 #include "itkFastMutexLock.h"
 #include "itkMultiThreader.h"
 
 //igtl
 #include "igtlSocket.h"
 #include "igtlMessageBase.h"
 #include "igtlTransformMessage.h"
 
 //mitkIGTL
 #include "MitkOpenIGTLinkExports.h"
 #include "mitkIGTLMessageFactory.h"
 #include "mitkIGTLMessageQueue.h"
 #include "mitkIGTLMessage.h"
 
 namespace mitk {
   /**
   * \brief Interface for all OpenIGTLink Devices
   *
   * Defines the methods that are common for all devices using OpenIGTLink. It
   * can open/close a connection, start/stop a communication and send/receive
   * messages.
   *
   * It uses message queues to store the incoming and outgoing mails. They are
   * configurable, you can set buffering on and off.
   *
   * The device is in one of three different states: Setup, Ready or Running.
   * Setup is the initial state. From this state on you can call
   * OpenConnection() and arrive in the Ready state. From the Ready state you
   * call StartCommunication() to arrive in the Running state. Now the device
   * is continuosly checking for new connections, receiving messages and
   * sending messages. This runs in a seperate thread. To stop the communication
   * call StopCommunication() (to arrive in Ready state) or CloseConnection()
   * (to arrive in the Setup state).
   *
   * \ingroup OpenIGTLink
   *
   */
   class MITKOPENIGTLINK_EXPORT IGTLDevice : public itk::Object
   {
   public:
     mitkClassMacroItkParent(IGTLDevice, itk::Object)
 
       IGTLDevice(bool ReadFully);
 
     /**
      * \brief Type for state variable.
      * The IGTLDevice is always in one of these states.
      *
      */
     enum IGTLDeviceState { Setup, Ready, Running };
 
     /**
      * \brief Opens a connection to the device
      *
      * This may only be called if there is currently no connection to the
      * device. If OpenConnection() is successful, the object will change from
      * Setup state to Ready state.
      */
     virtual bool OpenConnection() = 0;
 
     /**
      * \brief Closes the connection to the device
      *
      * This may only be called if there is currently a connection to the
      * device, but device is not running (e.g. object is in Ready state)
      */
     virtual bool CloseConnection();
 
     /**
      * \brief Stops the communication between the two devices
      *
      * This may only be called if the device is in Running state.
      */
     virtual bool StopCommunication();
 
     /**
      * \brief Starts the communication between the two devices
      *
      * This may only be called if the device is in Ready state.
      */
     bool StartCommunication();
 
     /**
      * \brief Continuously calls the given function
      *
      * This may only be called if the device is in Running state and only from
      * a seperate thread.
      *
      * \param ComFunction function pointer that specifies the method to be executed
      * \param mutex the mutex that corresponds to the function pointer
      */
     void RunCommunication(void (IGTLDevice::*ComFunction)(void), itk::FastMutexLock* mutex);
 
     /**
      * \brief Adds the given message to the sending queue
      *
      * This may only be called after the connection to the device has been
      * established with a call to OpenConnection(). Note that the message
      * is not send directly. This method just adds it to the send queue.
      * \param msg The message to be added to the sending queue
      */
     void SendMessage(igtl::MessageBase::Pointer msg);
 
     /**
      * \brief Adds the given message to the sending queue
      *
      * Convenience function to work with mitk::IGTLMessage directly.
      * \param msg The message to be added to the sending queue
      */
     void SendMessage(const IGTLMessage* msg);
 
     /**
      * \brief Returns current object state (Setup, Ready or Running)
      */
     IGTLDeviceState GetState() const;
 
     /**
      * \brief Returns the oldest message in the command queue
      * \return The oldest message from the command queue.
      */
     igtl::MessageBase::Pointer GetNextCommand();
 
     /**
      * \brief Returns the oldest message in the receive queue
      * \return The oldest message from the receive queue
      */
     igtl::ImageMessage::Pointer GetNextImage2dMessage();
 
     igtl::ImageMessage::Pointer GetNextImage3dMessage();
 
     igtl::TransformMessage::Pointer GetNextTransformMessage();
 
     igtl::TrackingDataMessage::Pointer GetNextTrackingDataMessage();
 
     igtl::StringMessage::Pointer GetNextStringMessage();
 
     igtl::MessageBase::Pointer GetNextMiscMessage();
 
     /**
      * \brief Sets the port number of the device
      */
     itkSetMacro(PortNumber, int);
 
     /**
      * \brief Returns the port number of the device
      */
     itkGetMacro(PortNumber, int);
 
     /**
      * \brief Sets the ip/hostname of the device
      */
     itkSetMacro(Hostname, std::string);
 
     /**
      * \brief Returns the ip/hostname of the device
      */
     itkGetMacro(Hostname, std::string);
 
     /**
      * \brief Returns the name of this device
      */
     itkGetConstMacro(Name, std::string);
 
     /**
      * \brief Sets the name of this device
      */
     itkSetMacro(Name, std::string);
 
     /**
     * \brief Advises this IGTL Device to always block until the whole message is read.
     */
     itkSetMacro(ReadFully, bool);
 
     /**
      * \brief Returns a const reference to the receive queue
      */
     itkGetConstMacro(MessageQueue, mitk::IGTLMessageQueue::Pointer);
 
     /**
      * \brief Returns the message factory
      */
     itkGetMacro(MessageFactory, mitk::IGTLMessageFactory::Pointer);
 
     /**
     * \brief static start method for the sending thread.
     * \param data a void pointer to the IGTLDevice object.
     */
     static ITK_THREAD_RETURN_TYPE ThreadStartSending(void* data);
 
     /**
     * \brief static start method for the receiving thread.
     * \param data a void pointer to the IGTLDevice object.
     */
     static ITK_THREAD_RETURN_TYPE ThreadStartReceiving(void* data);
 
     /**
     * \brief static start method for the connection thread.
     * \param data a void pointer to the IGTLDevice object.
     */
     static ITK_THREAD_RETURN_TYPE ThreadStartConnecting(void* data);
 
     /**
      * \brief TestConnection() tries to connect to a IGTL device on the current
      * ip and port
      *
      * \todo Implement this method. Send a status message and check the answer.
      *
      * TestConnection() tries to connect to a IGTL server on the current
      * ip and port and returns which device it has found.
      * \return It returns the type of the device that answers. Throws an
      * exception
      * if no device is available on that ip/port.
      * @throw mitk::Exception Throws an exception if there are errors
      * while connecting to the device.
      */
     virtual bool TestConnection();
 
     /**
      * \brief Send RTS message of given type
      */
     bool SendRTSMessage(const char* type);
 
     /**
     * \brief Sets the buffering mode of the given queue
     */
     void EnableInfiniteBufferingMode(mitk::IGTLMessageQueue::Pointer queue,
       bool enable = true);
 
+    void EnableInfiniteBufferingMode(bool enable = true);
+
     /**
     * \brief Returns the number of connections of this device
     */
     virtual unsigned int GetNumberOfConnections() = 0;
 
   protected:
     /**
      * \brief Sends a message.
      *
      * This may only be called after the connection to the device has been
      * established with a call to OpenConnection(). This method uses the given
      * socket to send the given MessageReceivedEvent
      *
      * \param msg the message to be sent
      * \param socket the socket used to communicate with the other device
      *
      * \retval IGTL_STATUS_OK the message was sent
      * \retval IGTL_STATUS_UNKONWN_ERROR the message was not sent because an
      * unknown error occurred
      */
     unsigned int SendMessagePrivate(igtl::MessageBase::Pointer msg,
       igtl::Socket::Pointer socket);
 
     /**
     * \brief Call this method to receive a message.
     *
     * The message will be saved in the receive queue.
     */
     virtual void Receive() = 0;
 
     /**
     * \brief Call this method to receive a message from the given device.
     *
     * The message will be saved in the receive queue.
     *
     * \param device the socket that connects this device with the other one.
     *
     * \retval IGTL_STATUS_OK a message or a command was received
     * \retval IGTL_STATUS_NOT_PRESENT the socket is not connected anymore
     * \retval IGTL_STATUS_TIME_OUT the socket timed out
     * \retval IGTL_STATUS_CHECKSUM_ERROR the checksum of the received msg was
     * incorrect
     * \retval IGTL_STATUS_UNKNOWN_ERROR an unknown error occurred
     */
     unsigned int ReceivePrivate(igtl::Socket* device);
 
     /**
     * \brief Call this method to send a message. The message will be read from
     * the queue.
     */
     virtual void Send() = 0;
 
     /**
     * \brief Call this method to check for other devices that want to connect
     * to this one.
     *
     * In case of a client this method is doing nothing. In case of a server it
     * is checking for other devices and if there is one it establishes a
     * connection.
     */
     virtual void Connect();
 
     /**
     * \brief Stops the communication with the given socket
     *
     */
     virtual void StopCommunicationWithSocket(igtl::Socket* socket) = 0;
 
     /**
     * \brief change object state
     */
     void SetState(IGTLDeviceState state);
 
     IGTLDevice();
     virtual ~IGTLDevice();
 
     /** current object state (Setup, Ready or Running) */
     IGTLDeviceState m_State;
     /** the name of this device */
     std::string m_Name;
 
     /** signal used to stop the thread*/
     bool m_StopCommunication;
     /** mutex to control access to m_StopCommunication */
     itk::FastMutexLock::Pointer m_StopCommunicationMutex;
     /** mutex used to make sure that the send thread is just started once */
     itk::FastMutexLock::Pointer m_SendingFinishedMutex;
     /** mutex used to make sure that the receive thread is just started once */
     itk::FastMutexLock::Pointer m_ReceivingFinishedMutex;
     /** mutex used to make sure that the connect thread is just started once */
     itk::FastMutexLock::Pointer m_ConnectingFinishedMutex;
     /** mutex to control access to m_State */
     itk::FastMutexLock::Pointer m_StateMutex;
 
     /** the hostname or ip of the device */
     std::string m_Hostname;
     /** the port number of the device */
     int m_PortNumber;
     /** the socket used to communicate with other IGTL devices */
     igtl::Socket::Pointer m_Socket;
 
     /** The message receive queue */
     mitk::IGTLMessageQueue::Pointer m_MessageQueue;
 
     /** A message factory that provides the New() method for all msg types */
     mitk::IGTLMessageFactory::Pointer m_MessageFactory;
 
   private:
 
     /** creates worker thread that continuously polls interface for new
     messages */
     itk::MultiThreader::Pointer m_MultiThreader;
     /** ID of sending thread */
     int m_SendThreadID;
     /** ID of receiving thread */
     int m_ReceiveThreadID;
     /** ID of connecting thread */
     int m_ConnectThreadID;
     /** Always try to read the full message. */
     bool m_ReadFully;
   };
 
   /**
   * \brief connect to this Event to get notified when a message was successfully sent
   *
   * \note This event is invoked in the communication thread, therefore do not use it to make
   * changes in the GUI!!! Use the QT signal slot system to decouple this call from the com thread
   * */
   itkEventMacro(MessageSentEvent, itk::AnyEvent);
 
   /**
   * \brief connect to this Event to get notified when a message was received
   *
   * \note Check if you can invoke this events like this or if you have to make
   * it thread-safe. They are not invoked in the main thread!!!
   * */
   itkEventMacro(MessageReceivedEvent, itk::AnyEvent);
 
   /**
   * \brief connect to this Event to get notified when a command was received
   *
   * \note Check if you can invoke this events like this or if you have to make
   * it thread-safe. They are not invoked in the main thread!!!
   * */
   itkEventMacro(CommandReceivedEvent, itk::AnyEvent);
 
   /**
   * \brief connect to this Event to get notified when another igtl device
   * connects with this device.
   *
   * \note Check if you can invoke this events like this or if you have to make
   * it thread-safe. They are not invoked in the main thread!!!
   * */
   itkEventMacro(NewClientConnectionEvent, itk::AnyEvent);
 
   /**
   * \brief connect to this Event to get notified when this device looses the
   * connection to a socket.
   *
   * \note Check if you can invoke this events like this or if you have to make
   * it thread-safe. They are not invoked in the main thread!!!
   * */
   itkEventMacro(LostConnectionEvent, itk::AnyEvent);
 } // namespace mitk
 
 #endif /* MITKIGTLDEVICE_H */