diff --git a/Modules/IGT/TrackingDevices/mitkNDITrackingDevice.cpp b/Modules/IGT/TrackingDevices/mitkNDITrackingDevice.cpp
index 7e1bc082e2..59cb18e59a 100644
--- a/Modules/IGT/TrackingDevices/mitkNDITrackingDevice.cpp
+++ b/Modules/IGT/TrackingDevices/mitkNDITrackingDevice.cpp
@@ -1,1314 +1,1356 @@
/*===================================================================
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 "mitkNDITrackingDevice.h"
#include "mitkIGTTimeStamp.h"
#include "mitkIGTHardwareException.h"
#include <cstdio>
#include <itksys/SystemTools.hxx>
#include <itkMutexLockHolder.h>
#include <mitkUnspecifiedTrackingTypeInformation.h>
#include <mitkNDIPolarisTypeInformation.h>
#include <mitkNDIAuroraTypeInformation.h>
// vtk
#include <vtkSphereSource.h>
typedef itk::MutexLockHolder<itk::FastMutexLock> MutexLockHolder;
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_ToolsMutex(nullptr),
m_SerialCommunication(nullptr), m_SerialCommunicationMutex(nullptr), m_DeviceProtocol(nullptr),
m_MultiThreader(nullptr), m_ThreadID(0), m_OperationMode(ToolTracking6D), m_MarkerPointsMutex(nullptr), m_MarkerPoints()
{
m_Data = mitk::UnspecifiedTrackingTypeInformation::GetDeviceDataUnspecified();
m_6DTools.clear();
m_SerialCommunicationMutex = itk::FastMutexLock::New();
m_DeviceProtocol = NDIProtocol::New();
m_DeviceProtocol->SetTrackingDevice(this);
m_DeviceProtocol->UseCRCOn();
m_MultiThreader = itk::MultiThreader::New();
m_ToolsMutex = itk::FastMutexLock::New();
m_MarkerPointsMutex = itk::FastMutexLock::New();
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_ThreadID != 0) && (m_MultiThreader.IsNotNull()))
{
m_MultiThreader->TerminateThread(m_ThreadID);
}
m_MultiThreader = nullptr;
/* 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
MutexLockHolder lock(*m_SerialCommunicationMutex); // lock and unlock the mutex
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;
MutexLockHolder lock(*m_SerialCommunicationMutex); // lock and unlock the mutex
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;
MutexLockHolder lock(*m_SerialCommunicationMutex); // lock and unlock the mutex
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;
MutexLockHolder lock(*m_SerialCommunicationMutex); // lock and unlock the mutex
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()
{
MutexLockHolder lock(*m_SerialCommunicationMutex); // lock and unlock the mutex
m_SerialCommunication->ClearSendBuffer();
}
void mitk::NDITrackingDevice::ClearReceiveBuffer()
{
MutexLockHolder lock(*m_SerialCommunicationMutex); // lock and unlock the mutex
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 ***/
/**
* now add tools to the tracking system
**/
/* 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
/**
* POLARIS: initialize the tools that were added manually
**/
{
MutexLockHolder toolsMutexLockHolder(*m_ToolsMutex); // lock and unlock the mutex
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 (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;
}
ITK_THREAD_RETURN_TYPE mitk::NDITrackingDevice::ThreadStartTracking(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;
}
NDITrackingDevice *trackingDevice = (NDITrackingDevice*)pInfo->UserData;
if (trackingDevice != nullptr)
{
if (trackingDevice->GetOperationMode() == ToolTracking6D)
trackingDevice->TrackTools(); // call TrackTools() from the original object
else if (trackingDevice->GetOperationMode() == MarkerTracking3D)
trackingDevice->TrackMarkerPositions(); // call TrackMarkerPositions() from the original object
else if (trackingDevice->GetOperationMode() == ToolTracking5D)
trackingDevice->TrackMarkerPositions(); // call TrackMarkerPositions() from the original object
else if (trackingDevice->GetOperationMode() == HybridTracking)
{
trackingDevice->TrackToolsAndMarkers();
}
}
trackingDevice->m_ThreadID = 0; // erase thread id, now that this thread will end.
return ITK_THREAD_RETURN_VALUE;
}
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_ThreadID = m_MultiThreader->SpawnThread(this->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 */
MutexLockHolder trackingFinishedLockHolder(*m_TrackingFinishedMutex); // keep lock until end of scope
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()
{
MutexLockHolder trackingFinishedLockHolder(*m_TrackingFinishedMutex); // keep lock until end of scope
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()
{
MutexLockHolder trackingFinishedLockHolder(*m_TrackingFinishedMutex); // keep lock until end of scope
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
{
MutexLockHolder toolsMutexLockHolder(*m_ToolsMutex); // lock and unlock the mutex
if (toolNumber < m_6DTools.size())
return m_6DTools.at(toolNumber);
return nullptr;
}
mitk::TrackingTool* mitk::NDITrackingDevice::GetToolByName(std::string name) const
{
MutexLockHolder toolsMutexLockHolder(*m_ToolsMutex); // lock and unlock the mutex
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)
{
MutexLockHolder toolsMutexLockHolder(*m_ToolsMutex); // lock and unlock the mutex
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
{
MutexLockHolder toolsMutexLockHolder(*m_ToolsMutex); // lock and unlock the mutex
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 */
MutexLockHolder toolsMutexLockHolder(*m_ToolsMutex); // lock and unlock the mutex (scope is inside the if-block
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
{
MutexLockHolder toolsMutexLockHolder(*m_ToolsMutex); // lock and unlock the mutex
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()
{
MutexLockHolder toolsMutexLockHolder(*m_ToolsMutex); // lock and unlock the mutex
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();
- /* check for new tools, add and initialize them */
- NDIErrorCode returnvalue;
+ //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)
+
+ /**
+ NDI handling (PHSR 02, PINIT, PHSR 02, PHSR 00) => all initialized and all handles available
+ **/
+
+ /* 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";
+ mitkThrowException(mitk::IGTHardwareException) << "Could not obtain a list of port handles that are connected on channel 1.";
}
- /* 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;
-
- /* we need to remember the ports which are occupied to be able to readout the serial numbers of the connected tools later */
- std::vector<int> occupiedPorts = std::vector<int>();
- int numberOfToolsAtStart = this->GetToolCount(); //also remember the number of tools at start to identify the automatically detected tools later
+ /* 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.";
+ }
+
+ /**
+ 1. Create MITK tracking tool representations of NDI tools
+ 2. NDI enable all tools (PENA)
+ **/
+
+ //remember the number of tools at start to identify the automatically detected tools later
+ int numberOfToolsAtStart = this->GetToolCount();
for (unsigned int i = 0; i < portHandle.size(); i += 2)
{
- ph = portHandle.substr(i, 2);
- if (this->GetInternalTool(ph) != nullptr) // if we already have a tool with this handle
- continue; // then skip the initialization
-
- //instantiate an object for each tool that is connected
- mitk::NDIPassiveTool::Pointer newTool = mitk::NDIPassiveTool::New();
- newTool->SetPortHandle(ph.c_str());
- newTool->SetTrackingPriority(mitk::NDIPassiveTool::Dynamic);
-
- //set a name for identification
- newTool->SetToolName((std::string("Port ") + ph).c_str());
-
- returnvalue = m_DeviceProtocol->PINIT(&ph);
- if (returnvalue != NDIINITIALIZATIONFAILED) //if the initialization failed (AURORA) it can not be enabled. A srom file will have to be specified manually first. Still return true to be able to continue
- {
- if (returnvalue != NDIOKAY)
- {
- mitkThrowException(mitk::IGTHardwareException) << (std::string("Could not initialize port '") + ph +
- std::string("' for tool '") + newTool->GetToolName() + std::string("'")).c_str();
- }
- /* enable the port handle */
- returnvalue = m_DeviceProtocol->PENA(&ph, newTool->GetTrackingPriority()); // Enable tool
- if (returnvalue != NDIOKAY)
- {
+ 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";
- }
- else occupiedPorts.push_back(i);
+ 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";
+ }
}
+ /**
+ NDI get all serial numbers (PHINF)
+ **/
+
// after initialization readout serial numbers of automatically detected tools
- for (unsigned int i = 0; i < occupiedPorts.size(); i++)
+ for (unsigned int i = 0; i < portHandle.size(); i += 2)
{
- ph = portHandle.substr(occupiedPorts.at(i), 2);
- std::string portInfo;
- NDIErrorCode returnvaluePort = m_DeviceProtocol->PHINF(ph, &portInfo);
- if ((returnvaluePort == NDIOKAY) && (portInfo.size()>31)) dynamic_cast<mitk::NDIPassiveTool*>(this->GetTool(i + numberOfToolsAtStart))->SetSerialNumber(portInfo.substr(23, 8));
- itksys::SystemTools::Delay(10);
+ 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;
}
\ No newline at end of file