mitkNDITrackingDevice.cpp

Go to the documentation of this file.

/*============================================================================
 
The Medical Imaging Interaction Toolkit (MITK)
 
Copyright (c) German Cancer Research Center (DKFZ)
All rights reserved.
 
Use of this source code is governed by a 3-clause BSD license that can be
found in the LICENSE file.
 
============================================================================*/
 
#include "mitkNDITrackingDevice.h"
#include "mitkIGTTimeStamp.h"
#include "mitkIGTHardwareException.h"
#include <cstdio>
 
#include <itksys/SystemTools.hxx>
 
#include <mitkUnspecifiedTrackingTypeInformation.h>
 
#include <mitkNDIPolarisTypeInformation.h>
#include <mitkNDIAuroraTypeInformation.h>
 
// vtk
#include <vtkSphereSource.h>
 
const unsigned char CR = 0xD; // == '\r' - carriage return
const unsigned char LF = 0xA; // == '\n' - line feed
 
mitk::NDITrackingDevice::NDITrackingDevice() :
TrackingDevice(), m_DeviceName(""), m_PortNumber(mitk::SerialCommunication::COM5), m_BaudRate(mitk::SerialCommunication::BaudRate9600),
m_DataBits(mitk::SerialCommunication::DataBits8), m_Parity(mitk::SerialCommunication::None), m_StopBits(mitk::SerialCommunication::StopBits1),
m_HardwareHandshake(mitk::SerialCommunication::HardwareHandshakeOff),
m_IlluminationActivationRate(Hz20), m_DataTransferMode(TX), m_6DTools(),
m_SerialCommunication(nullptr), m_DeviceProtocol(nullptr),
m_OperationMode(ToolTracking6D), m_MarkerPoints()
{
  m_Data = mitk::UnspecifiedTrackingTypeInformation::GetDeviceDataUnspecified();
  m_6DTools.clear();
  m_DeviceProtocol = NDIProtocol::New();
  m_DeviceProtocol->SetTrackingDevice(this);
  m_DeviceProtocol->UseCRCOn();
  m_MarkerPoints.reserve(50);   // a maximum of 50 marker positions can be reported by the tracking device
}
 
bool mitk::NDITrackingDevice::UpdateTool(mitk::TrackingTool* tool)
{
  if (this->GetState() != Setup)
  {
    mitk::NDIPassiveTool* ndiTool = dynamic_cast<mitk::NDIPassiveTool*>(tool);
    if (ndiTool == nullptr)
      return false;
 
    std::string portHandle = ndiTool->GetPortHandle();
 
    //return false if the SROM Data has not been set
    if (ndiTool->GetSROMData() == nullptr)
      return false;
 
    NDIErrorCode returnvalue;
    returnvalue = m_DeviceProtocol->PVWR(&portHandle, ndiTool->GetSROMData(), ndiTool->GetSROMDataLength());
    if (returnvalue != NDIOKAY)
      return false;
    returnvalue = m_DeviceProtocol->PINIT(&portHandle);
    if (returnvalue != NDIOKAY)
      return false;
    returnvalue = m_DeviceProtocol->PENA(&portHandle, ndiTool->GetTrackingPriority()); // Enable tool
    if (returnvalue != NDIOKAY)
      return false;
 
    return true;
  }
  else
  {
    return false;
  }
}
 
void mitk::NDITrackingDevice::SetRotationMode(RotationMode r)
{
  m_RotationMode = r;
}
 
mitk::NDITrackingDevice::~NDITrackingDevice()
{
  /* stop tracking and disconnect from tracking device */
  if (GetState() == Tracking)
  {
    this->StopTracking();
  }
  if (GetState() == Ready)
  {
    this->CloseConnection();
  }
  /* cleanup tracking thread */
  if (m_Thread.joinable())
    m_Thread.join();
 
  /* free serial communication interface */
  if (m_SerialCommunication.IsNotNull())
  {
    m_SerialCommunication->ClearReceiveBuffer();
    m_SerialCommunication->ClearSendBuffer();
    m_SerialCommunication->CloseConnection();
    m_SerialCommunication = nullptr;
  }
}
 
void mitk::NDITrackingDevice::SetPortNumber(const PortNumber _arg)
{
  if (this->GetState() != Setup)
    return;
  itkDebugMacro("setting PortNumber to " << _arg);
  if (this->m_PortNumber != _arg)
  {
    this->m_PortNumber = _arg;
    this->Modified();
  }
}
 
void mitk::NDITrackingDevice::SetDeviceName(std::string _arg)
{
  if (this->GetState() != Setup)
    return;
  itkDebugMacro("setting eviceName to " << _arg);
  if (this->m_DeviceName != _arg)
  {
    this->m_DeviceName = _arg;
    this->Modified();
  }
}
 
void mitk::NDITrackingDevice::SetBaudRate(const BaudRate _arg)
{
  if (this->GetState() != Setup)
    return;
  itkDebugMacro("setting BaudRate to " << _arg);
  if (this->m_BaudRate != _arg)
  {
    this->m_BaudRate = _arg;
    this->Modified();
  }
}
 
void mitk::NDITrackingDevice::SetDataBits(const DataBits _arg)
{
  if (this->GetState() != Setup)
    return;
  itkDebugMacro("setting DataBits to " << _arg);
  if (this->m_DataBits != _arg)
  {
    this->m_DataBits = _arg;
    this->Modified();
  }
}
 
void mitk::NDITrackingDevice::SetParity(const Parity _arg)
{
  if (this->GetState() != Setup)
    return;
  itkDebugMacro("setting Parity to " << _arg);
  if (this->m_Parity != _arg)
  {
    this->m_Parity = _arg;
    this->Modified();
  }
}
 
void mitk::NDITrackingDevice::SetStopBits(const StopBits _arg)
{
  if (this->GetState() != Setup)
    return;
  itkDebugMacro("setting StopBits to " << _arg);
  if (this->m_StopBits != _arg)
  {
    this->m_StopBits = _arg;
    this->Modified();
  }
}
 
void mitk::NDITrackingDevice::SetHardwareHandshake(const HardwareHandshake _arg)
{
  if (this->GetState() != Setup)
    return;
  itkDebugMacro("setting HardwareHandshake to " << _arg);
  if (this->m_HardwareHandshake != _arg)
  {
    this->m_HardwareHandshake = _arg;
    this->Modified();
  }
}
 
void mitk::NDITrackingDevice::SetIlluminationActivationRate(const IlluminationActivationRate _arg)
{
  if (this->GetState() == Tracking)
    return;
  itkDebugMacro("setting IlluminationActivationRate to " << _arg);
  if (this->m_IlluminationActivationRate != _arg)
  {
    this->m_IlluminationActivationRate = _arg;
    this->Modified();
    if (this->GetState() == Ready)   // if the connection to the tracking system is established, send the new rate to the tracking device too
      m_DeviceProtocol->IRATE(this->m_IlluminationActivationRate);
  }
}
 
void mitk::NDITrackingDevice::SetDataTransferMode(const DataTransferMode _arg)
{
  itkDebugMacro("setting DataTransferMode to " << _arg);
  if (this->m_DataTransferMode != _arg)
  {
    this->m_DataTransferMode = _arg;
    this->Modified();
  }
}
 
mitk::NDIErrorCode mitk::NDITrackingDevice::Send(const std::string* input, bool addCRC)
{
  if (input == nullptr)
    return SERIALSENDERROR;
 
  std::string message;
 
  if (addCRC == true)
    message = *input + CalcCRC(input) + std::string(1, CR);
  else
    message = *input + std::string(1, CR);
 
  //unsigned int messageLength = message.length() + 1; // +1 for CR
 
  // Clear send buffer
  this->ClearSendBuffer();
  // Send the date to the device
  std::lock_guard<std::mutex> lock(m_SerialCommunicationMutex);
  long returnvalue = m_SerialCommunication->Send(message);
 
  if (returnvalue == 0)
    return SERIALSENDERROR;
  else
    return NDIOKAY;
}
 
mitk::NDIErrorCode mitk::NDITrackingDevice::Receive(std::string* answer, unsigned int numberOfBytes)
{
  if (answer == nullptr)
    return SERIALRECEIVEERROR;
 
  std::lock_guard<std::mutex> lock(m_SerialCommunicationMutex);
  long returnvalue = m_SerialCommunication->Receive(*answer, numberOfBytes);  // never read more bytes than the device has send, the function will block until enough bytes are send...
 
  if (returnvalue == 0)
    return SERIALRECEIVEERROR;
  else
    return NDIOKAY;
}
 
mitk::NDIErrorCode mitk::NDITrackingDevice::ReceiveByte(char* answer)
{
  if (answer == nullptr)
    return SERIALRECEIVEERROR;
 
  std::string m;
 
  std::lock_guard<std::mutex> lock(m_SerialCommunicationMutex);
 
  long returnvalue = m_SerialCommunication->Receive(m, 1);
 
  if ((returnvalue == 0) || (m.size() != 1))
    return SERIALRECEIVEERROR;
 
  *answer = m.at(0);
  return NDIOKAY;
}
 
mitk::NDIErrorCode mitk::NDITrackingDevice::ReceiveLine(std::string* answer)
{
  if (answer == nullptr)
    return SERIALRECEIVEERROR;
 
  std::string m;
 
  std::lock_guard<std::mutex> lock(m_SerialCommunicationMutex);
 
  do
  {
    long returnvalue = m_SerialCommunication->Receive(m, 1);
    if ((returnvalue == 0) || (m.size() != 1))
      return SERIALRECEIVEERROR;
    *answer += m;
  } while (m.at(0) != LF);
  return NDIOKAY;
}
 
void mitk::NDITrackingDevice::ClearSendBuffer()
{
  std::lock_guard<std::mutex> lock(m_SerialCommunicationMutex);
  m_SerialCommunication->ClearSendBuffer();
}
 
void mitk::NDITrackingDevice::ClearReceiveBuffer()
{
  std::lock_guard<std::mutex> lock(m_SerialCommunicationMutex);
  m_SerialCommunication->ClearReceiveBuffer();
}
 
const std::string mitk::NDITrackingDevice::CalcCRC(const std::string* input)
{
  if (input == nullptr)
    return "";
  /* the crc16 calculation code is taken from the NDI API guide example code section */
  static int oddparity[16] = { 0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0 };
  unsigned int data;  // copy of the input string's current character
  unsigned int crcValue = 0;  // the crc value is stored here
  unsigned int* puCRC16 = &crcValue;  // the algorithm uses a pointer to crcValue, so it's easier to provide that than to change the algorithm
  for (unsigned int i = 0; i < input->length(); i++)
  {
    data = (*input)[i];
    data = (data ^ (*(puCRC16)& 0xff)) & 0xff;
    *puCRC16 >>= 8;
    if (oddparity[data & 0x0f] ^ oddparity[data >> 4])
    {
      *(puCRC16) ^= 0xc001;
    }
    data <<= 6;
    *puCRC16 ^= data;
    data <<= 1;
    *puCRC16 ^= data;
  }
  // crcValue contains now the CRC16 value. Convert it to a string and return it
  char returnvalue[13];
  sprintf(returnvalue, "%04X", crcValue);  // 4 hexadecimal digit with uppercase format
  return std::string(returnvalue);
}
 
bool mitk::NDITrackingDevice::OpenConnection()
{
  //this->m_ModeMutex.lock();
  if (this->GetState() != Setup)
  {
    mitkThrowException(mitk::IGTException) << "Can only try to open the connection if in setup mode";
  }
 
  m_SerialCommunication = mitk::SerialCommunication::New();
 
  /* init local com port to standard com settings for a NDI tracking device:
  9600 baud, 8 data bits, no parity, 1 stop bit, no hardware handshake */
  if (m_DeviceName.empty())
    m_SerialCommunication->SetPortNumber(m_PortNumber);
  else
    m_SerialCommunication->SetDeviceName(m_DeviceName);
  m_SerialCommunication->SetBaudRate(mitk::SerialCommunication::BaudRate9600);
  m_SerialCommunication->SetDataBits(mitk::SerialCommunication::DataBits8);
  m_SerialCommunication->SetParity(mitk::SerialCommunication::None);
  m_SerialCommunication->SetStopBits(mitk::SerialCommunication::StopBits1);
  m_SerialCommunication->SetSendTimeout(5000);
  m_SerialCommunication->SetReceiveTimeout(5000);
  if (m_SerialCommunication->OpenConnection() == 0) // 0 == ERROR_VALUE
  {
    m_SerialCommunication->CloseConnection();
    m_SerialCommunication = nullptr;
    mitkThrowException(mitk::IGTHardwareException) << "Can not open serial port";
  }
 
  /* Reset Tracking device by sending a serial break for 500ms */
  m_SerialCommunication->SendBreak(400);
 
  /* Read answer from tracking device (RESETBE6F) */
  static const std::string reset("RESETBE6F\r");
  std::string answer = "";
  this->Receive(&answer, reset.length());  // read answer (should be RESETBE6F)
  this->ClearReceiveBuffer();     // flush the receive buffer of all remaining data (carriage return, strings other than reset
  if (reset.compare(answer) != 0)  // check for RESETBE6F
  {
    if (m_SerialCommunication.IsNotNull())
    {
      m_SerialCommunication->CloseConnection();
      m_SerialCommunication = nullptr;
    }
    mitkThrowException(mitk::IGTHardwareException) << "Hardware Reset of tracking device did not work";
  }
 
  /* Now the tracking device isSetData reset, start initialization */
  NDIErrorCode returnvalue;
 
  /* set device com settings to new values and wait for the device to change them */
  returnvalue = m_DeviceProtocol->COMM(m_BaudRate, m_DataBits, m_Parity, m_StopBits, m_HardwareHandshake);
 
  if (returnvalue != NDIOKAY)
  {
    mitkThrowException(mitk::IGTHardwareException) << "Could not set comm settings in trackingdevice";
  }
 
  //after changing COMM wait at least 100ms according to NDI Api documentation page 31
  itksys::SystemTools::Delay(500);
 
  /* now change local com settings accordingly */
  m_SerialCommunication->CloseConnection();
  m_SerialCommunication->SetBaudRate(m_BaudRate);
  m_SerialCommunication->SetDataBits(m_DataBits);
  m_SerialCommunication->SetParity(m_Parity);
  m_SerialCommunication->SetStopBits(m_StopBits);
  m_SerialCommunication->SetHardwareHandshake(m_HardwareHandshake);
  m_SerialCommunication->SetSendTimeout(5000);
  m_SerialCommunication->SetReceiveTimeout(5000);
  m_SerialCommunication->OpenConnection();
 
  /* initialize the tracking device */
  returnvalue = m_DeviceProtocol->INIT();
  if (returnvalue != NDIOKAY)
  {
    mitkThrowException(mitk::IGTHardwareException) << "Could not initialize the tracking device";
  }
 
  if (this->GetType() == mitk::UnspecifiedTrackingTypeInformation::GetTrackingDeviceName())  // if the type of tracking device is not specified, try to query the connected device
  {
    mitk::TrackingDeviceType deviceType;
    returnvalue = m_DeviceProtocol->VER(deviceType);
    if ((returnvalue != NDIOKAY) || (deviceType == mitk::UnspecifiedTrackingTypeInformation::GetTrackingDeviceName()))
    {
      mitkThrowException(mitk::IGTHardwareException) << "Could not determine tracking device type. Please set manually and try again.";
    }
    this->SetType(deviceType);
  }
 
  /****  Optional Polaris specific code, Work in progress
  // start diagnostic mode
  returnvalue = m_DeviceProtocol->DSTART();
  if (returnvalue != NDIOKAY)
  {
  this->SetErrorMessage("Could not start diagnostic mode");
  return false;
  }
  else    // we are in diagnostic mode
  {
  // initialize extensive IR checking
  returnvalue = m_DeviceProtocol->IRINIT();
  if (returnvalue != NDIOKAY)
  {
  this->SetErrorMessage("Could not initialize intense infrared light checking");
  return false;
  }
  bool intenseIR = false;
  returnvalue = m_DeviceProtocol->IRCHK(&intenseIR);
  if (returnvalue != NDIOKAY)
  {
  this->SetErrorMessage("Could not execute intense infrared light checking");
  return false;
  }
  if (intenseIR == true)
  // do something - warn the user, raise exception, write to protocol or similar
  std::cout << "Warning: Intense infrared light detected. Accurate tracking will probably not be possible.\n";
 
  // stop diagnictic mode
  returnvalue = m_DeviceProtocol->DSTOP();
  if (returnvalue != NDIOKAY)
  {
  this->SetErrorMessage("Could not stop diagnostic mode");
  return false;
  }
  }
  *** end of optional polaris code ***/
 
  /* First, check if the tracking device has port handles that need to be freed and free them */
  returnvalue = FreePortHandles();
  // non-critical, therefore no error handling
 
  {
    std::lock_guard<std::mutex> lock(m_ToolsMutex);
    std::string portHandle;
    auto endIt = m_6DTools.end();
    for (auto it = m_6DTools.begin(); it != endIt; ++it)
    {
      /* get a port handle for the tool */
      returnvalue = m_DeviceProtocol->PHRQ(&portHandle);
      if (returnvalue == NDIOKAY)
      {
        (*it)->SetPortHandle(portHandle.c_str());
        /* now write the SROM file of the tool to the tracking system using PVWR */
        if (this->m_Data.Line == mitk::NDIPolarisTypeInformation::GetTrackingDeviceName())
        {
          returnvalue = m_DeviceProtocol->PVWR(&portHandle, (*it)->GetSROMData(), (*it)->GetSROMDataLength());
          if (returnvalue != NDIOKAY)
          {
            mitkThrowException(mitk::IGTHardwareException) << (std::string("Could not write SROM file for tool '") + (*it)->GetToolName() + std::string("' to tracking device")).c_str();
          }
 
          returnvalue = m_DeviceProtocol->PINIT(&portHandle);
          if (returnvalue != NDIOKAY)
          {
            mitkThrowException(mitk::IGTHardwareException) << (std::string("Could not initialize tool '") + (*it)->GetToolName()).c_str();
          }
 
          if ((*it)->IsEnabled() == true)
          {
            returnvalue = m_DeviceProtocol->PENA(&portHandle, (*it)->GetTrackingPriority()); // Enable tool
            if (returnvalue != NDIOKAY)
            {
              mitkThrowException(mitk::IGTHardwareException) << (std::string("Could not enable port '") + portHandle +
                std::string("' for tool '") + (*it)->GetToolName() + std::string("'")).c_str();
            }
          }
        }
      }
    }
  } // end of toolsmutexlockholder scope
 
  /* check for wired tools and add them too */
  if (this->DiscoverWiredTools() == false)  // query the tracking device for wired tools and add them to our tool list
    return false; // \TODO: could we continue anyways?
 
  /*POLARIS: set the illuminator activation rate */
  if (this->m_Data.Line == mitk::NDIPolarisTypeInformation::GetTrackingDeviceName())
  {
    returnvalue = m_DeviceProtocol->IRATE(this->m_IlluminationActivationRate);
    if (returnvalue != NDIOKAY)
    {
      mitkThrowException(mitk::IGTHardwareException) << "Could not set the illuminator activation rate";
    }
  }
  /* finish  - now all tools should be added, initialized and enabled, so that tracking can be started */
  this->SetState(Ready);
  try
  {
    SetVolume(this->m_Data);
  }
  catch (const mitk::IGTHardwareException& e)
  {
    MITK_WARN << e.GetDescription();
  }
 
  return true;
}
 
bool mitk::NDITrackingDevice::InitializeWiredTools()
{
  NDIErrorCode returnvalue;
  std::string portHandle;
  returnvalue = m_DeviceProtocol->PHSR(OCCUPIED, &portHandle);
 
  if (returnvalue != NDIOKAY)
  {
    mitkThrowException(mitk::IGTHardwareException) << "Could not obtain a list of port handles that are connected";
  }
 
  /* if there are port handles that need to be initialized, initialize them. Furthermore instantiate tools for each handle that has no tool yet. */
  std::string ph;
 
  for (unsigned int i = 0; i < portHandle.size(); i += 2)
  {
    ph = portHandle.substr(i, 2);
    mitk::NDIPassiveTool* pt = this->GetInternalTool(ph);
    if (pt == nullptr) // if we don't have a tool, something is wrong. Tools should be discovered first by calling DiscoverWiredTools()
      continue;
 
    if (pt->GetSROMData() == nullptr)
      continue;
 
    returnvalue = m_DeviceProtocol->PVWR(&ph, pt->GetSROMData(), pt->GetSROMDataLength());
    if (returnvalue != NDIOKAY)
    {
      mitkThrowException(mitk::IGTHardwareException) << (std::string("Could not write SROM file for tool '") + pt->GetToolName() + std::string("' to tracking device")).c_str();
    }
 
    returnvalue = m_DeviceProtocol->PINIT(&ph);
    if (returnvalue != NDIOKAY)
    {
      mitkThrowException(mitk::IGTHardwareException) << (std::string("Could not initialize tool '") + pt->GetToolName()).c_str();
    }
 
    if (pt->IsEnabled() == true)
    {
      returnvalue = m_DeviceProtocol->PENA(&ph, pt->GetTrackingPriority()); // Enable tool
      if (returnvalue != NDIOKAY)
      {
        mitkThrowException(mitk::IGTHardwareException) << (std::string("Could not enable port '") + portHandle +
          std::string("' for tool '") + pt->GetToolName() + std::string("'")).c_str();
      }
    }
  }
  return true;
}
 
mitk::TrackingDeviceType mitk::NDITrackingDevice::TestConnection()
{
  if (this->GetState() != Setup)
  {
    return mitk::UnspecifiedTrackingTypeInformation::GetTrackingDeviceName();
  }
 
  m_SerialCommunication = mitk::SerialCommunication::New();
  //m_DeviceProtocol =  mitk::NDIProtocol::New();
  //m_DeviceProtocol->SetTrackingDevice(this);
  //m_DeviceProtocol->UseCRCOn();
  /* init local com port to standard com settings for a NDI tracking device:
  9600 baud, 8 data bits, no parity, 1 stop bit, no hardware handshake
  */
  if (m_DeviceName.empty())
    m_SerialCommunication->SetPortNumber(m_PortNumber);
  else
    m_SerialCommunication->SetDeviceName(m_DeviceName);
 
  m_SerialCommunication->SetBaudRate(mitk::SerialCommunication::BaudRate9600);
  m_SerialCommunication->SetDataBits(mitk::SerialCommunication::DataBits8);
  m_SerialCommunication->SetParity(mitk::SerialCommunication::None);
  m_SerialCommunication->SetStopBits(mitk::SerialCommunication::StopBits1);
  m_SerialCommunication->SetSendTimeout(5000);
  m_SerialCommunication->SetReceiveTimeout(5000);
  if (m_SerialCommunication->OpenConnection() == 0) // error
  {
    m_SerialCommunication = nullptr;
    return mitk::UnspecifiedTrackingTypeInformation::GetTrackingDeviceName();
  }
 
  /* Reset Tracking device by sending a serial break for 500ms */
  m_SerialCommunication->SendBreak(400);
 
  /* Read answer from tracking device (RESETBE6F) */
  static const std::string reset("RESETBE6F\r");
  std::string answer = "";
  this->Receive(&answer, reset.length());  // read answer (should be RESETBE6F)
  this->ClearReceiveBuffer();     // flush the receive buffer of all remaining data (carriage return, strings other than reset
  if (reset.compare(answer) != 0)  // check for RESETBE6F
  {
    m_SerialCommunication->CloseConnection();
    m_SerialCommunication = nullptr;
    mitkThrowException(mitk::IGTHardwareException) << "Hardware Reset of tracking device did not work";
  }
 
  /* Now the tracking device is reset, start initialization */
  NDIErrorCode returnvalue;
 
  /* initialize the tracking device */
  //returnvalue = m_DeviceProtocol->INIT();
  //if (returnvalue != NDIOKAY)
  //{
  //  this->SetErrorMessage("Could not initialize the tracking device");
  //  return mitk::TrackingSystemNotSpecified;
  //}
 
  mitk::TrackingDeviceType deviceType;
  returnvalue = m_DeviceProtocol->VER(deviceType);
  if ((returnvalue != NDIOKAY) || (deviceType == mitk::UnspecifiedTrackingTypeInformation::GetTrackingDeviceName()))
  {
    m_SerialCommunication = nullptr;
    return mitk::UnspecifiedTrackingTypeInformation::GetTrackingDeviceName();
  }
  m_SerialCommunication = nullptr;
  return deviceType;
}
 
bool mitk::NDITrackingDevice::CloseConnection()
{
  if (this->GetState() != Setup)
  {
    //init before closing to force the field generator from aurora to switch itself off
    m_DeviceProtocol->INIT();
    /* close the serial connection */
    m_SerialCommunication->CloseConnection();
    /* invalidate all tools */
    this->InvalidateAll();
    /* return to setup mode */
    this->SetState(Setup);
    m_SerialCommunication = nullptr;
  }
  return true;
}
 
void mitk::NDITrackingDevice::ThreadStartTracking()
{
  if (this->GetOperationMode() == ToolTracking6D)
    this->TrackTools();             // call TrackTools() from the original object
  else if (this->GetOperationMode() == MarkerTracking3D)
    this->TrackMarkerPositions();   // call TrackMarkerPositions() from the original object
  else if (this->GetOperationMode() == ToolTracking5D)
    this->TrackMarkerPositions(); // call TrackMarkerPositions() from the original object
  else if (this->GetOperationMode() == HybridTracking)
  {
    this->TrackToolsAndMarkers();
  }
}
 
bool mitk::NDITrackingDevice::StartTracking()
{
  if (this->GetState() != Ready)
    return false;
 
  this->SetState(Tracking);      // go to mode Tracking
  this->m_StopTrackingMutex.lock();  // update the local copy of m_StopTracking
  this->m_StopTracking = false;
  this->m_StopTrackingMutex.unlock();
 
  m_Thread = std::thread(&NDITrackingDevice::ThreadStartTracking, this);    // start a new thread that executes the TrackTools() method
  mitk::IGTTimeStamp::GetInstance()->Start(this);
  return true;
}
 
void mitk::NDITrackingDevice::TrackTools()
{
  /* lock the TrackingFinishedMutex to signal that the execution rights are now transfered to the tracking thread */
  std::lock_guard<std::mutex> lock(m_TrackingFinishedMutex);
 
  if (this->GetState() != Tracking)
    return;
 
  NDIErrorCode returnvalue;
  returnvalue = m_DeviceProtocol->TSTART();
  if (returnvalue != NDIOKAY)
    return;
 
  bool localStopTracking;       // Because m_StopTracking is used by two threads, access has to be guarded by a mutex. To minimize thread locking, a local copy is used here
  this->m_StopTrackingMutex.lock();  // update the local copy of m_StopTracking
  localStopTracking = this->m_StopTracking;
  this->m_StopTrackingMutex.unlock();
  while ((this->GetState() == Tracking) && (localStopTracking == false))
  {
    if (this->m_DataTransferMode == TX)
    {
      returnvalue = this->m_DeviceProtocol->TX();
      if (!((returnvalue == NDIOKAY) || (returnvalue == NDICRCERROR) || (returnvalue == NDICRCDOESNOTMATCH))) // right now, do not stop on crc errors
        break;
    }
    else
    {
      returnvalue = this->m_DeviceProtocol->BX();
      if (returnvalue != NDIOKAY)
        break;
    }
    /* Update the local copy of m_StopTracking */
    this->m_StopTrackingMutex.lock();
    localStopTracking = m_StopTracking;
    this->m_StopTrackingMutex.unlock();
  }
  /* StopTracking was called, thus the mode should be changed back to Ready now that the tracking loop has ended. */
 
  returnvalue = m_DeviceProtocol->TSTOP();
  if (returnvalue != NDIOKAY)
  {
    mitkThrowException(mitk::IGTHardwareException) << "An error occured while tracking tools.";
  }
 
  return;       // returning from this function (and ThreadStartTracking()) this will end the thread and transfer control back to main thread by releasing trackingFinishedLockHolder
}
 
void mitk::NDITrackingDevice::TrackMarkerPositions()
{
  std::lock_guard<std::mutex> lock(m_TrackingFinishedMutex);
 
  if (m_OperationMode == ToolTracking6D)
    return;
 
  if (this->GetState() != Tracking)
    return;
 
  NDIErrorCode returnvalue;
 
  returnvalue = m_DeviceProtocol->DSTART();   // Start Diagnostic Mode
  if (returnvalue != NDIOKAY)
    return;
 
  bool localStopTracking;       // Because m_StopTracking is used by two threads, access has to be guarded by a mutex. To minimize thread locking, a local copy is used here
  this->m_StopTrackingMutex.lock();  // update the local copy of m_StopTracking
  localStopTracking = this->m_StopTracking;
  this->m_StopTrackingMutex.unlock();
  while ((this->GetState() == Tracking) && (localStopTracking == false))
  {
    m_MarkerPointsMutex.lock();                                    // lock points data structure
    returnvalue = this->m_DeviceProtocol->POS3D(&m_MarkerPoints); // update points data structure with new position data from tracking device
    m_MarkerPointsMutex.unlock();
    if (!((returnvalue == NDIOKAY) || (returnvalue == NDICRCERROR) || (returnvalue == NDICRCDOESNOTMATCH))) // right now, do not stop on crc errors
    {
      std::cout << "Error in POS3D: could not read data. Possibly no markers present." << std::endl;
    }
    /* Update the local copy of m_StopTracking */
    this->m_StopTrackingMutex.lock();
    localStopTracking = m_StopTracking;
    this->m_StopTrackingMutex.unlock();
 
    itksys::SystemTools::Delay(1);
  }
  /* StopTracking was called, thus the mode should be changed back to Ready now that the tracking loop has ended. */
  returnvalue = m_DeviceProtocol->DSTOP();
  if (returnvalue != NDIOKAY)
    return;     // how can this thread tell the application, that an error has occured?
 
  this->SetState(Ready);
  return;       // returning from this function (and ThreadStartTracking()) this will end the thread
}
 
void mitk::NDITrackingDevice::TrackToolsAndMarkers()
{
  std::lock_guard<std::mutex> lock(m_TrackingFinishedMutex);
  if (m_OperationMode != HybridTracking)
    return;
 
  NDIErrorCode returnvalue;
 
  returnvalue = m_DeviceProtocol->TSTART();   // Start Diagnostic Mode
  if (returnvalue != NDIOKAY)
    return;
 
  bool localStopTracking;       // Because m_StopTracking is used by two threads, access has to be guarded by a mutex. To minimize thread locking, a local copy is used here
  this->m_StopTrackingMutex.lock();  // update the local copy of m_StopTracking
  localStopTracking = this->m_StopTracking;
  this->m_StopTrackingMutex.unlock();
  while ((this->GetState() == Tracking) && (localStopTracking == false))
  {
    m_MarkerPointsMutex.lock();                                     // lock points data structure
    returnvalue = this->m_DeviceProtocol->TX(true, &m_MarkerPoints); // update points data structure with new position data from tracking device
    m_MarkerPointsMutex.unlock();
    if (!((returnvalue == NDIOKAY) || (returnvalue == NDICRCERROR) || (returnvalue == NDICRCDOESNOTMATCH))) // right now, do not stop on crc errors
    {
      std::cout << "Error in TX: could not read data. Possibly no markers present." << std::endl;
    }
    /* Update the local copy of m_StopTracking */
    this->m_StopTrackingMutex.lock();
    localStopTracking = m_StopTracking;
    this->m_StopTrackingMutex.unlock();
  }
  /* StopTracking was called, thus the mode should be changed back to Ready now that the tracking loop has ended. */
 
  returnvalue = m_DeviceProtocol->TSTOP();
  if (returnvalue != NDIOKAY)
    return;     // how can this thread tell the application, that an error has occurred?
 
  this->SetState(Ready);
  return;       // returning from this function (and ThreadStartTracking()) this will end the thread
}
 
mitk::TrackingTool* mitk::NDITrackingDevice::GetTool(unsigned int toolNumber) const
{
  std::lock_guard<std::mutex> lock(m_ToolsMutex);
  if (toolNumber < m_6DTools.size())
    return m_6DTools.at(toolNumber);
  return nullptr;
}
 
mitk::TrackingTool* mitk::NDITrackingDevice::GetToolByName(std::string name) const
{
  std::lock_guard<std::mutex> lock(m_ToolsMutex);
  auto end = m_6DTools.end();
  for (auto iterator = m_6DTools.begin(); iterator != end; ++iterator)
    if (name.compare((*iterator)->GetToolName()) == 0)
      return *iterator;
  return nullptr;
}
 
mitk::NDIPassiveTool* mitk::NDITrackingDevice::GetInternalTool(std::string portHandle)
{
  std::lock_guard<std::mutex> lock(m_ToolsMutex);
  auto end = m_6DTools.end();
  for (auto iterator = m_6DTools.begin(); iterator != end; ++iterator)
    if (portHandle.compare((*iterator)->GetPortHandle()) == 0)
      return *iterator;
  return nullptr;
}
 
unsigned int mitk::NDITrackingDevice::GetToolCount() const
{
  std::lock_guard<std::mutex> lock(m_ToolsMutex);
  return m_6DTools.size();
}
 
bool mitk::NDITrackingDevice::Beep(unsigned char count)
{
  if (this->GetState() != Setup)
  {
    return (m_DeviceProtocol->BEEP(count) == NDIOKAY);
  }
  else
  {
    return false;
  }
}
 
mitk::TrackingTool* mitk::NDITrackingDevice::AddTool(const char* toolName, const char* fileName, TrackingPriority p /*= NDIPassiveTool::Dynamic*/)
{
  mitk::NDIPassiveTool::Pointer t = mitk::NDIPassiveTool::New();
  if (t->LoadSROMFile(fileName) == false)
    return nullptr;
  t->SetToolName(toolName);
  t->SetTrackingPriority(p);
  if (this->InternalAddTool(t) == false)
    return nullptr;
  return t.GetPointer();
}
 
bool mitk::NDITrackingDevice::InternalAddTool(mitk::NDIPassiveTool* tool)
{
  if (tool == nullptr)
    return false;
  NDIPassiveTool::Pointer p = tool;
  /* if the connection to the tracking device is already established, add the new tool to the device now */
  if (this->GetState() == Ready)
  {
    /* get a port handle for the tool */
    std::string newPortHandle;
    NDIErrorCode returnvalue;
    returnvalue = m_DeviceProtocol->PHRQ(&newPortHandle);
    if (returnvalue == NDIOKAY)
    {
      p->SetPortHandle(newPortHandle.c_str());
      /* now write the SROM file of the tool to the tracking system using PVWR */
      returnvalue = m_DeviceProtocol->PVWR(&newPortHandle, p->GetSROMData(), p->GetSROMDataLength());
      if (returnvalue != NDIOKAY)
      {
        mitkThrowException(mitk::IGTHardwareException) << (std::string("Could not write SROM file for tool '") + p->GetToolName() + std::string("' to tracking device")).c_str();
      }
      /* initialize the port handle */
      returnvalue = m_DeviceProtocol->PINIT(&newPortHandle);
      if (returnvalue != NDIOKAY)
      {
        mitkThrowException(mitk::IGTHardwareException) << (std::string("Could not initialize port '") + newPortHandle +
          std::string("' for tool '") + p->GetToolName() + std::string("'")).c_str();
      }
      /* enable the port handle */
      if (p->IsEnabled() == true)
      {
        returnvalue = m_DeviceProtocol->PENA(&newPortHandle, p->GetTrackingPriority()); // Enable tool
        if (returnvalue != NDIOKAY)
        {
          mitkThrowException(mitk::IGTHardwareException) << (std::string("Could not enable port '") + newPortHandle +
            std::string("' for tool '") + p->GetToolName() + std::string("'")).c_str();
        }
      }
    }
    /* now that the tool is added to the device, add it to list too */
    m_ToolsMutex.lock();
    this->m_6DTools.push_back(p);
    m_ToolsMutex.unlock();
    this->Modified();
    return true;
  }
  else if (this->GetState() == Setup)
  {
    /* In Setup mode, we only add it to the list, so that OpenConnection() can add it later */
    m_ToolsMutex.lock();
    this->m_6DTools.push_back(p);
    m_ToolsMutex.unlock();
    this->Modified();
    return true;
  }
  else  // in Tracking mode, no tools can be added
    return false;
}
 
bool mitk::NDITrackingDevice::RemoveTool(mitk::TrackingTool* tool)
{
  mitk::NDIPassiveTool* ndiTool = dynamic_cast<mitk::NDIPassiveTool*>(tool);
  if (ndiTool == nullptr)
    return false;
 
  std::string portHandle = ndiTool->GetPortHandle();
  /* a valid portHandle has length 2. If a valid handle exists, the tool is already added to the tracking device, so we have to remove it there
  if the connection to the tracking device has already been established.
  */
  if ((portHandle.length() == 2) && (this->GetState() == Ready))  // do not remove a tool in tracking mode
  {
    NDIErrorCode returnvalue;
    returnvalue = m_DeviceProtocol->PHF(&portHandle);
    if (returnvalue != NDIOKAY)
      return false;
    /* Now that the tool is removed from the tracking device, remove it from our tool list too */
    std::lock_guard<std::mutex> lock(m_ToolsMutex);
    auto end = m_6DTools.end();
    for (auto iterator = m_6DTools.begin(); iterator != end; ++iterator)
    {
      if (iterator->GetPointer() == ndiTool)
      {
        m_6DTools.erase(iterator);
        this->Modified();
        return true;
      }
    }
    return false;
  }
  else if (this->GetState() == Setup)  // in Setup Mode, we are not connected to the tracking device, so we can just remove the tool from the tool list
  {
    std::lock_guard<std::mutex> lock(m_ToolsMutex);
    auto end = m_6DTools.end();
    for (auto iterator = m_6DTools.begin(); iterator != end; ++iterator)
    {
      if ((*iterator).GetPointer() == ndiTool)
      {
        m_6DTools.erase(iterator);
        this->Modified();
        return true;
      }
    }
    return false;
  }
  return false;
}
 
void mitk::NDITrackingDevice::InvalidateAll()
{
  std::lock_guard<std::mutex> lock(m_ToolsMutex);
  auto end = m_6DTools.end();
  for (auto iterator = m_6DTools.begin(); iterator != end; ++iterator)
    (*iterator)->SetDataValid(false);
}
 
bool mitk::NDITrackingDevice::SetOperationMode(OperationMode mode)
{
  if (GetState() == Tracking)
    return false;
 
  m_OperationMode = mode;
  return true;
}
 
mitk::OperationMode mitk::NDITrackingDevice::GetOperationMode()
{
  return m_OperationMode;
}
 
bool mitk::NDITrackingDevice::GetMarkerPositions(MarkerPointContainerType* markerpositions)
{
  m_MarkerPointsMutex.lock();
  *markerpositions = m_MarkerPoints;  // copy the internal vector to the one provided
  m_MarkerPointsMutex.unlock();
  return (markerpositions->size() != 0);
}
 
bool mitk::NDITrackingDevice::DiscoverWiredTools()
{
  /* First, check for disconnected tools and remove them */
  this->FreePortHandles();
 
  //NDI handling (PHSR 02, PINIT, PHSR 02, PHSR 00) => all initialized and all handles available
  //creation of MITK tools
  //NDI enable all tools (PENA)
  //NDI get all serial numbers (PHINF)
 
  /* check for occupied port handles on channel 0 */
  std::string portHandle;
  NDIErrorCode returnvalue = m_DeviceProtocol->PHSR(OCCUPIED, &portHandle);
 
  if (returnvalue != NDIOKAY)
  {
      mitkThrowException(mitk::IGTHardwareException) << "Could not obtain a list of port handles that are connected on channel 0.";
  }
 
  /* Initialize all port handles on channel 0 */
  for (unsigned int i = 0; i < portHandle.size(); i += 2)
  {
     std::string ph = portHandle.substr(i, 2);
     returnvalue = m_DeviceProtocol->PINIT(&ph);
 
     if (returnvalue != NDIOKAY)
     {
        mitkThrowException(mitk::IGTHardwareException) << (std::string("Could not initialize port '") + ph + std::string("."));
     }
  }
 
  /* check for occupied port handles on channel 1 (initialize automatically, portHandle is empty although additional tools were detected) */
  //For a split port on a dual 5DOF tool, the first PHSR sent will report only one port handle. After the port handle is
  //initialized, it is assigned to channel 0. You must then use PHSR again to assign a port handle to channel 1. The
  //port handle for channel 1 is initialized automatically.
  returnvalue = m_DeviceProtocol->PHSR(OCCUPIED, &portHandle);
 
  if (returnvalue != NDIOKAY)
  {
     mitkThrowException(mitk::IGTHardwareException) << "Could not obtain a list of port handles that are connected on channel 1.";
  }
 
  /* read all port handles */
  returnvalue = m_DeviceProtocol->PHSR(ALL, &portHandle);
 
  if (returnvalue != NDIOKAY)
  {
     mitkThrowException(mitk::IGTHardwareException) << "Could not obtain a list of port handles that are connected on all channels.";
  }
 
  for (unsigned int i = 0; i < portHandle.size(); i += 2)
  {
     std::string ph = portHandle.substr(i, 2);
     if (this->GetInternalTool(ph) != nullptr) // if we already have a tool with this handle
        continue;                              // then skip the initialization
 
     //define tracking priority
     auto trackingPriority = mitk::NDIPassiveTool::Dynamic;
 
     //instantiate an object for each tool that is connected
     mitk::NDIPassiveTool::Pointer newTool = mitk::NDIPassiveTool::New();
     newTool->SetPortHandle(ph.c_str());
     newTool->SetTrackingPriority(trackingPriority);
 
     //set a name for identification
     newTool->SetToolName((std::string("Port ") + ph).c_str());
 
     /* enable the port handle */
     returnvalue = m_DeviceProtocol->PENA(&ph, trackingPriority); // Enable tool
 
     if (returnvalue != NDIOKAY)
     {
        mitkThrowException(mitk::IGTHardwareException) << (std::string("Could not enable port '") + ph +
           std::string("' for tool '") + newTool->GetToolName() + std::string("'")).c_str();
     }
 
     //we have to temporarily unlock m_ModeMutex here to avoid a deadlock with another lock inside InternalAddTool()
     if (this->InternalAddTool(newTool) == false)
     {
        mitkThrowException(mitk::IGTException) << "Error while adding new tool";
     }
  }
 
  // after initialization readout serial numbers of automatically detected tools
  for (unsigned int i = 0; i < portHandle.size(); i += 2)
  {
     std::string ph = portHandle.substr(i, 2);
 
     std::string portInfo;
     NDIErrorCode returnvaluePort = m_DeviceProtocol->PHINF(ph, &portInfo);
     if ((returnvaluePort == NDIOKAY) && (portInfo.size() > 31))
        dynamic_cast<mitk::NDIPassiveTool*>(this->GetInternalTool(ph))->SetSerialNumber(portInfo.substr(23, 8));
     MITK_INFO << "portInfo: " << portInfo;
     itksys::SystemTools::Delay(10);
  }
 
  return true;
}
 
mitk::NDIErrorCode mitk::NDITrackingDevice::FreePortHandles()
{
  /*  first search for port handles that need to be freed: e.g. because of a reset of the tracking system */
  NDIErrorCode returnvalue = NDIOKAY;
  std::string portHandle;
  returnvalue = m_DeviceProtocol->PHSR(FREED, &portHandle);
  if (returnvalue != NDIOKAY)
  {
    mitkThrowException(mitk::IGTHardwareException) << "Could not obtain a list of port handles that need to be freed";
  }
 
  /* if there are port handles that need to be freed, free them */
  if (portHandle.empty() == true)
    return returnvalue;
 
  std::string ph;
  for (unsigned int i = 0; i < portHandle.size(); i += 2)
  {
    ph = portHandle.substr(i, 2);
 
    mitk::NDIPassiveTool* t = this->GetInternalTool(ph);
    if (t != nullptr)  // if we have a tool for the port handle that needs to be freed
    {
      if (this->RemoveTool(t) == false)  // remove it (this will free the port too)
        returnvalue = NDIERROR;
    }
    else  // we don't have a tool, the port handle exists only in the tracking device
    {
      returnvalue = m_DeviceProtocol->PHF(&ph);  // free it there
      // What to do if port handle could not be freed? This seems to be a non critical error
      if (returnvalue != NDIOKAY)
      {
        mitkThrowException(mitk::IGTHardwareException) << "Could not free all port handles";
      }
    }
  }
  return returnvalue;
}
 
int mitk::NDITrackingDevice::GetMajorFirmwareRevisionNumber()
{
  std::string revision;
  if (m_DeviceProtocol->APIREV(&revision) != mitk::NDIOKAY || revision.empty() || (revision.size() != 9))
  {
    MITK_ERROR << "Could not receive firmware revision number!";
    return 0;
  }
 
  const std::string majrevno = revision.substr(2, 3); //cut out "004" from "D.004.001"
 
  return std::atoi(majrevno.c_str());
}
 
const char* mitk::NDITrackingDevice::GetFirmwareRevisionNumber()
{
  static std::string revision;
  if (m_DeviceProtocol->APIREV(&revision) != mitk::NDIOKAY || revision.empty() || (revision.size() != 9))
  {
    MITK_ERROR << "Could not receive firmware revision number!";
    revision = "";
    return revision.c_str();
  }
  return revision.c_str();
}
 
bool mitk::NDITrackingDevice::AutoDetectToolsAvailable()
{
  if (this->GetType() == mitk::NDIAuroraTypeInformation::GetTrackingDeviceName()) { return true; }
  else { return false; }
}
 
bool mitk::NDITrackingDevice::AddSingleToolIsAvailable()
{
  //For Aurora, only AutoDetecion or loading of toolStorage should be used. It is not possible to add a single tool.
  if (this->GetType() == mitk::NDIAuroraTypeInformation::GetTrackingDeviceName()) { return false; }
  //For Polaris, a single tool can be added, there is no autoDetection.
  else { return true; }
}
 
mitk::NavigationToolStorage::Pointer mitk::NDITrackingDevice::AutoDetectTools()
{
  mitk::NavigationToolStorage::Pointer autoDetectedStorage = mitk::NavigationToolStorage::New();
  if (this->GetType() == mitk::NDIAuroraTypeInformation::GetTrackingDeviceName())
  {
    try
    {
      this->OpenConnection();
      this->StartTracking();
    }
    catch (mitk::Exception& e)
    {
      MITK_WARN << "Warning, can not auto-detect tools! (" << e.GetDescription() << ")";
      return autoDetectedStorage;
    }
 
    for (unsigned int i = 0; i < this->GetToolCount(); i++)
    {
      //create a navigation tool with sphere as surface
      std::stringstream toolname;
      toolname << "AutoDetectedTool" << i;
      mitk::NavigationTool::Pointer newTool = mitk::NavigationTool::New();
      newTool->SetSerialNumber(dynamic_cast<mitk::NDIPassiveTool*>(this->GetTool(i))->GetSerialNumber());
      newTool->SetIdentifier(toolname.str());
      newTool->SetTrackingDeviceType(mitk::NDIAuroraTypeInformation::GetTrackingDeviceName());
      newTool->GetDataNode()->SetName(toolname.str());
      autoDetectedStorage->AddTool(newTool);
    }
    this->StopTracking();
    this->CloseConnection();
  }
  return autoDetectedStorage;
}
 
bool mitk::NDITrackingDevice::GetSupportedVolumes(unsigned int* numberOfVolumes, mitk::NDITrackingDevice::NDITrackingVolumeContainerType* volumes, mitk::NDITrackingDevice::TrackingVolumeDimensionType* volumesDimensions)
{
  if (numberOfVolumes == nullptr || volumes == nullptr || volumesDimensions == nullptr)
    return false;
 
  static std::string info;
  if (m_DeviceProtocol->SFLIST(&info) != mitk::NDIOKAY || info.empty())
  {
    MITK_ERROR << "Could not receive tracking volume information of tracking system!";
    return false;
  }
 
  /*info contains the following:
  <HEX:number of volumes> (+n times:) <HEX:shape type> <shape parameters D1-D10> <HEX:reserved / number of wavelength supported> <metal resistant / supported wavelength>
  */
  (*numberOfVolumes) = (unsigned int)std::atoi(info.substr(0, 1).c_str());
 
  for (unsigned int i = 0; i < (*numberOfVolumes); i++)
  {
    //e.g. for cube:  "9-025000+025000-025000+025000-055000-005000+000000+000000+000000+00000011"
    //for dome:       "A+005000+048000+005000+066000+000000+000000+000000+000000+000000+00000011"
 
    std::string::size_type offset, end;
    offset = (i * 73) + 1;
    end = 73 + (i * 73);
    std::string currentVolume = info.substr(offset, end);//i=0: from 1 to 73 characters; i=1: from 75 to 148 char;
    // if i>0 then we have a return statement <LF> infront
    if (i > 0)
      currentVolume = currentVolume.substr(1, currentVolume.size());
    if (currentVolume.compare(0, 1, NDIPolarisTypeInformation::GetDeviceDataPolarisOldModel().HardwareCode) == 0)
      volumes->push_back(NDIPolarisTypeInformation::GetDeviceDataPolarisOldModel().Model);
    if (currentVolume.compare(0, 3, NDIPolarisTypeInformation::GetDeviceDataPolarisSpectra().HardwareCode) == 0)
      volumes->push_back(NDIPolarisTypeInformation::GetDeviceDataPolarisSpectra().Model);
    if (currentVolume.compare(1, 3, NDIPolarisTypeInformation::GetDeviceDataSpectraExtendedPyramid().HardwareCode) == 0)
    {
      currentVolume = currentVolume.substr(1, currentVolume.size());
      volumes->push_back(NDIPolarisTypeInformation::GetDeviceDataSpectraExtendedPyramid().Model);
    }
    if (currentVolume.compare(0, 1, NDIPolarisTypeInformation::GetDeviceDataPolarisVicra().HardwareCode) == 0)
      volumes->push_back(NDIPolarisTypeInformation::GetDeviceDataPolarisVicra().Model);
    else if (currentVolume.compare(0, 1, mitk::NDIAuroraTypeInformation::GetDeviceDataAuroraPlanarCube().HardwareCode) == 0)
      volumes->push_back(mitk::NDIAuroraTypeInformation::GetDeviceDataAuroraPlanarCube().Model);//alias cube
    else if (currentVolume.compare(0, 1, mitk::NDIAuroraTypeInformation::GetDeviceDataAuroraPlanarDome().HardwareCode) == 0)
      volumes->push_back(mitk::NDIAuroraTypeInformation::GetDeviceDataAuroraPlanarDome().Model);
 
    //fill volumesDimensions
    for (unsigned int index = 0; index < 10; index++)
    {
      std::string::size_type offD, endD;
      offD = 1 + (index * 7); //7 digits per dimension and the first is the type of volume
      endD = offD + 7;
      int dimension = std::atoi(currentVolume.substr(offD, endD).c_str());
      dimension /= 100; //given in mm. 7 digits are xxxx.xx according to NDI //strange, the last two digits (11) also for the metal flag get read also...
      volumesDimensions->push_back(dimension);
    }
  }
 
  return true;
}
 
bool mitk::NDITrackingDevice::SetVolume(mitk::TrackingDeviceData volume)
{
  if (m_DeviceProtocol->VSEL(volume) != mitk::NDIOKAY)
  {
    mitkThrowException(mitk::IGTHardwareException) << "Could not set volume!";
  }
  return true;
}