diff --git a/Modules/IGT/TrackingDevices/mitkPolhemusInterface.cpp b/Modules/IGT/TrackingDevices/mitkPolhemusInterface.cpp
index 5cda02a3d0..8649c42477 100644
--- a/Modules/IGT/TrackingDevices/mitkPolhemusInterface.cpp
+++ b/Modules/IGT/TrackingDevices/mitkPolhemusInterface.cpp
@@ -1,444 +1,469 @@
/*===================================================================
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 <mitkPolhemusInterface.h>
#define _USE_MATH_DEFINES
#include <math.h>
#include <PDI.h>
BYTE MotionBuf[0x1FA400];
mitk::PolhemusInterface::PolhemusInterface() : m_continousTracking(false)
{
m_pdiDev = new CPDIdev();
m_numberOfTools = 0;
}
mitk::PolhemusInterface::~PolhemusInterface()
{
delete m_pdiDev;
}
bool mitk::PolhemusInterface::InitializeDevice()
{
m_pdiDev->ResetTracker();
m_pdiDev->ResetSAlignment(-1);
m_pdiDev->Trace(TRUE, 7);
m_continousTracking = false;
return true;
}
bool mitk::PolhemusInterface::SetupDevice()
{
m_pdiDev->SetPnoBuffer(MotionBuf, 0x1FA400);
m_pdiDev->SetMetric(true); //use cm instead of inches
m_pdiDev->StartPipeExport();
CPDImdat pdiMDat;
pdiMDat.Empty();
pdiMDat.Append(PDI_MODATA_FRAMECOUNT);
pdiMDat.Append(PDI_MODATA_POS);
pdiMDat.Append(PDI_MODATA_ORI);
m_pdiDev->SetSDataList(-1, pdiMDat);
CPDIbiterr cBE;
m_pdiDev->GetBITErrs(cBE);
if (!(cBE.IsClear())) { m_pdiDev->ClearBITErrs(); }
return true;
}
bool mitk::PolhemusInterface::StartTracking()
{
m_continousTracking = true;
return m_pdiDev->StartContPno(0);
}
bool mitk::PolhemusInterface::StopTracking()
{
m_continousTracking = false;
m_pdiDev->StopContPno();
return true;
}
-bool mitk::PolhemusInterface::Connect()
+bool mitk::PolhemusInterface::OpenConnection()
{
bool returnValue;
//Initialize, and if it is not successful, return false.
if (!InitializeDevice())
{
returnValue = false;
}
//Connect
else if (m_pdiDev->CnxReady())
{
returnValue = true;
}
//If it is not successful, search for connections.
else
{
CPDIser pdiSer;
m_pdiDev->SetSerialIF(&pdiSer);
ePiCommType eType = m_pdiDev->DiscoverCnx();
switch (eType)
{
case PI_CNX_USB:
MITK_INFO << "USB Connection: " << m_pdiDev->GetLastResultStr();
break;
case PI_CNX_SERIAL:
MITK_INFO << "Serial Connection: " << m_pdiDev->GetLastResultStr();
break;
default:
MITK_INFO << "DiscoverCnx result: " << m_pdiDev->GetLastResultStr();
break;
}
//Setup device
if (!SetupDevice())
{
returnValue = false;
}
else
{
returnValue = m_pdiDev->CnxReady();
}
}
+ return returnValue;
+}
+
+bool mitk::PolhemusInterface::Connect()
+{
+ bool returnValue = OpenConnection();
- if (returnValue)
+ if (!returnValue)
{
- m_numberOfTools = this->GetNumberOfTools();
+ return returnValue;
}
+ m_numberOfTools = this->GetNumberOfTools();
+
//Get the tracking data to find out which tools are available.
std::vector<mitk::PolhemusInterface::trackingData> _trackingData = GetFrame();
- //if we had tool before, check if they are still the same.
+ //if we have more/less tools than before, reset all data.
+ //check with toolStorage changes is nto enough, 'cause a sensor could just have been unplugged.
+ if (m_ToolPorts.size() != _trackingData.size())
+ {
+ m_ToolPorts.clear();
+ m_Hemispheres.clear();
+ m_HemisphereTracking.clear();
+ }
+
+ //if we have the same number of tools as before, check if they are still the same.
if (m_ToolPorts.size() == _trackingData.size())
{
for (int i = 0; i < _trackingData.size(); ++i)
{
//if they are not the same, clear hemispheres and toolNames and break.
if (m_ToolPorts[i] != _trackingData.at(i).id)
{
m_ToolPorts.clear();
m_Hemispheres.clear();
m_HemisphereTracking.clear();
break;
}
}
}
//if we don't have old tool names or if the old ones don't match any more, assign them again.
if (m_ToolPorts.size() == 0)
{
for (int i = 0; i < _trackingData.size(); ++i)
{
m_ToolPorts.push_back(_trackingData.at(i).id);
}
//and reset the hemisphere parameters
m_Hemispheres.clear();
m_HemisphereTracking.clear();
mitk::Vector3D temp;
mitk::FillVector3D(temp, 1, 0, 0);
m_Hemispheres.assign(m_numberOfTools, temp);
m_HemisphereTracking.assign(m_numberOfTools, false);
}
return returnValue;
}
bool mitk::PolhemusInterface::Disconnect()
{
bool returnValue = true;
//If Tracking is running, stop tracking first
if (m_continousTracking)
{
this->StopTracking();
}
returnValue = m_pdiDev->Disconnect();
MITK_INFO << "Disconnect: " << m_pdiDev->GetLastResultStr();
return returnValue;
}
+std::vector<mitk::PolhemusInterface::trackingData> mitk::PolhemusInterface::AutoDetectTools()
+{
+ OpenConnection();
+ std::vector<mitk::PolhemusInterface::trackingData> frame = GetSingleFrame();
+ m_pdiDev->Disconnect();
+ return frame;
+}
+
unsigned int mitk::PolhemusInterface::GetNumberOfTools()
{
std::vector<mitk::PolhemusInterface::trackingData> _trackingData = GetFrame();
return _trackingData.size();
}
std::vector<mitk::PolhemusInterface::trackingData> mitk::PolhemusInterface::GetFrame()
{
if (m_continousTracking)
return this->GetLastFrame();
else
return this->GetSingleFrame();
}
std::vector<mitk::PolhemusInterface::trackingData> mitk::PolhemusInterface::GetLastFrame()
{
PBYTE pBuf;
DWORD dwSize;
//read one frame
if (!m_pdiDev->LastPnoPtr(pBuf, dwSize)) { MITK_WARN << m_pdiDev->GetLastResultStr(); }
std::vector<mitk::PolhemusInterface::trackingData> returnValue = ParsePolhemusRawData(pBuf, dwSize);
if (returnValue.empty())
{
MITK_WARN << "Cannot parse data / no tools present";
}
return returnValue;
}
std::vector<mitk::PolhemusInterface::trackingData> mitk::PolhemusInterface::GetSingleFrame()
{
if (m_continousTracking)
{
MITK_WARN << "Cannot get a single frame when continuous tracking is on!";
return std::vector<mitk::PolhemusInterface::trackingData>();
}
PBYTE pBuf;
DWORD dwSize;
//read one frame
if (!m_pdiDev->ReadSinglePnoBuf(pBuf, dwSize)) {
MITK_WARN << m_pdiDev->GetLastResultStr();
return std::vector<mitk::PolhemusInterface::trackingData>();
}
return ParsePolhemusRawData(pBuf, dwSize);
}
std::vector<mitk::PolhemusInterface::trackingData> mitk::PolhemusInterface::ParsePolhemusRawData(PBYTE pBuf, DWORD dwSize)
{
std::vector<mitk::PolhemusInterface::trackingData> returnValue;
DWORD i = 0;
while (i < dwSize)
{
BYTE ucSensor = pBuf[i + 2];
SHORT shSize = pBuf[i + 6];
// skip rest of header
i += 8;
PDWORD pFC = (PDWORD)(&pBuf[i]);
PFLOAT pPno = (PFLOAT)(&pBuf[i + 4]);
mitk::PolhemusInterface::trackingData currentTrackingData;
currentTrackingData.id = ucSensor;
currentTrackingData.pos[0] = pPno[0] * 10; //from cm to mm
currentTrackingData.pos[1] = pPno[1] * 10;
currentTrackingData.pos[2] = pPno[2] * 10;
double azimuthAngle = pPno[3] / 180 * M_PI; //from degree to rad
double elevationAngle = pPno[4] / 180 * M_PI;
double rollAngle = pPno[5] / 180 * M_PI;
vnl_quaternion<double> eulerQuat(rollAngle, elevationAngle, azimuthAngle);
currentTrackingData.rot = eulerQuat;
returnValue.push_back(currentTrackingData);
i += shSize;
}
return returnValue;
}
void mitk::PolhemusInterface::SetHemisphereTrackingEnabled(bool _HeisphereTrackingEnabeled, int _tool)
{
//only if connection is ready!
if (!this->m_pdiDev->CnxReady())
return;
if (m_Hemispheres.empty())
{
MITK_ERROR << "No Hemispheres. This should never happen when connected. Check your code!";
}
//HemisphereTracking is switched on by SetSHemiTrack(-1). "-1" means for all sensors.
//To switch heisphere tracking of, you need to set a hemisphere vector e.g. by calling SetSHemisphere(-1, { (float)1,0,0 })
if (_HeisphereTrackingEnabeled)
{
m_pdiDev->SetSHemiTrack(_tool);
if (_tool != -1)
{
m_HemisphereTracking.at(GetToolIndex(_tool)) = true;
}
else
{
m_HemisphereTracking.assign(m_numberOfTools, true);
}
}
//switch HemiTracking OFF
else
{
//Get Tool Position. ToDo, this should not be the tool tip but the sensor position. Any chance, to get that from Polhemus interface?!
std::vector<mitk::PolhemusInterface::trackingData> _position = GetFrame();
for (int index : GetToolIterator(_tool))
{
//Scalar product between mitk::point and mitk::vector
double _scalarProduct = _position.at(index).pos.GetVectorFromOrigin() * m_Hemispheres.at(index);
//if scalar product is negative, then the tool is in the opposite sphere then when we started to track.
//Hence, we have to set the inverted hemisphere.
//For default (1|0|0) this means, if x is negative, we have to set (-1|0|0). But we want to keep it generic if user sets different hemisphere...
if (_scalarProduct < 0)
{
m_Hemispheres.at(index) = -1. * m_Hemispheres.at(index);
}
else if (_scalarProduct == 0)
MITK_ERROR << "Something went wrong. Hemisphere or Position should not be zero.";
SetHemisphere(m_ToolPorts[index], m_Hemispheres.at(index));
}
}
}
void mitk::PolhemusInterface::ToggleHemisphere(int _tool)
{
//only if connection is ready!
if (!this->m_pdiDev->CnxReady())
return;
//toggle.
for (int index : GetToolIterator(_tool))
{
if (m_HemisphereTracking.at(index))
{
SetHemisphereTrackingEnabled(false, m_ToolPorts[index]);
this->SetHemisphere(m_ToolPorts[index], -1.*m_Hemispheres.at(index));
SetHemisphereTrackingEnabled(true, m_ToolPorts[index]);
}
else
{
this->SetHemisphere(m_ToolPorts[index], -1.*m_Hemispheres.at(index));
}
}
}
void mitk::PolhemusInterface::AdjustHemisphere(int _tool)
{
//only if connection is ready!
if (!this->m_pdiDev->CnxReady())
return;
mitk::Vector3D _hemisphere;
mitk::FillVector3D(_hemisphere, 1, 0, 0);
for (int index : GetToolIterator(_tool))
{
if (m_HemisphereTracking.at(index))
{
SetHemisphereTrackingEnabled(false, m_ToolPorts[index]);
this->SetHemisphere(m_ToolPorts[index], _hemisphere);
SetHemisphereTrackingEnabled(true, m_ToolPorts[index]);
}
else
{
this->SetHemisphere(m_ToolPorts[index], _hemisphere);
}
}
}
void mitk::PolhemusInterface::SetHemisphere(int _tool, mitk::Vector3D _hemisphere)
{
//only if connection is ready!
if (!this->m_pdiDev->CnxReady())
return;
m_pdiDev->SetSHemisphere(_tool, { (float)_hemisphere[0], (float)_hemisphere[1], (float)_hemisphere[2] });
for (int index : GetToolIterator(_tool))
{
if (_hemisphere.GetNorm() != 0)
{
m_HemisphereTracking.at(index) = false;
m_Hemispheres.at(index) = _hemisphere;
}
else
{
m_HemisphereTracking.at(index) = true;
//don't set the Hemisphere to (0|0|0), as we want to remember the old one.
}
}
}
mitk::Vector3D mitk::PolhemusInterface::GetHemisphere(int _tool)
{
if (_tool == -1)
{
MITK_WARN << "Can't return hemisphere for all tools. Returning Hemisphere of first tool " << m_ToolPorts[0];
return m_Hemispheres.at(0);
}
return m_Hemispheres.at(GetToolIndex(_tool));
}
bool mitk::PolhemusInterface::GetHemisphereTrackingEnabled(int _tool)
{
//if tool is -1, this means "All Tools". We return true if HemiTracking is enabled for all tools, and false if it is off for at least one tool.
if (_tool == -1)
{
bool _returnValue = true;
for (bool currentValue : m_HemisphereTracking)
_returnValue = _returnValue && currentValue;
return _returnValue;
}
else
return m_HemisphereTracking.at(GetToolIndex(_tool));
}
std::vector<int> mitk::PolhemusInterface::GetToolPorts()
{
return m_ToolPorts;
}
int mitk::PolhemusInterface::GetToolIndex(int _tool)
{
if (_tool == -1)
return -1;
else
return std::find(m_ToolPorts.begin(), m_ToolPorts.end(), _tool) - m_ToolPorts.begin();
}
std::vector<int> mitk::PolhemusInterface::GetToolIterator(int _tool)
{
std::vector<int> _iterator;
if (_tool == -1)
{
for (int i = 0; i < m_numberOfTools; ++i)
_iterator.push_back(i);
}
else
{
_iterator.push_back(GetToolIndex(_tool));
}
return _iterator;
}
void mitk::PolhemusInterface::PrintStatus()
{
MITK_INFO << "Polhemus status: " << this->m_pdiDev->CnxReady();
}
\ No newline at end of file
diff --git a/Modules/IGT/TrackingDevices/mitkPolhemusInterface.h b/Modules/IGT/TrackingDevices/mitkPolhemusInterface.h
index 3feb660e10..4123d66038 100644
--- a/Modules/IGT/TrackingDevices/mitkPolhemusInterface.h
+++ b/Modules/IGT/TrackingDevices/mitkPolhemusInterface.h
@@ -1,168 +1,174 @@
/*===================================================================
The Medical Imaging Interaction Toolkit (MITK)
Copyright (c) German Cancer Research Center,
Division of Medical and Biological Informatics.
All rights reserved.
This software is distributed WITHOUT ANY WARRANTY; without
even the implied warranty of MERCHANTABILITY or FITNESS FOR
A PARTICULAR PURPOSE.
See LICENSE.txt or http://www.mitk.org for details.
===================================================================*/
#ifndef MITKPolhemusINTERFACE_H_HEADER_INCLUDED_
#define MITKPolhemusINTERFACE_H_HEADER_INCLUDED_
#include <vector>
#include <string>
#include <MitkIGTExports.h>
#include "mitkCommon.h"
#include <itkObject.h>
#include <itkObjectFactory.h>
#include <windows.h>
#include <tchar.h>
#include <string>
#include <mitkNavigationData.h>
class CPDIdev;
namespace mitk
{
/** Documentation:
* \brief An object of this class represents the interface to Polhemus trackers.
* All variables with the name "tool" start with index 1, which is the station number of Polhemus.
* Make sure to call functions in this class with parameter "1" if you want to loop over all tools.
* If you need to access an array (e.g. m_Hemisphere), you need to use "_tool -1" and adapt your index for loops...
* \ingroup IGT
*/
class MITKIGT_EXPORT PolhemusInterface : public itk::Object
{
public:
mitkClassMacroItkParent(PolhemusInterface, itk::Object);
itkFactorylessNewMacro(Self);
itkCloneMacro(Self);
struct trackingData
{
mitk::Point3D pos;
mitk::Quaternion rot;
BYTE id;
};
/**
* \brief Opens the connection to the device and makes it ready to track tools.
* \return Returns true if there is a connection to the device and the device is ready to track tools, false if not.
*/
bool StartTracking();
/**
* \brief Clears all resources. After this method have been called the system isn't ready to track any longer.
* \return Returns true if the operation was succesful, false if not.
*/
bool StopTracking();
bool Connect();
bool Disconnect();
/** @return Returns the number of tools. Returns 0 if no information is avialable.*/
unsigned int GetNumberOfTools();
/** Enables/disables hemisphere tracking for all stations/tools. */
void SetHemisphereTrackingEnabled(bool _HeisphereTrackingEnabeled, int _tool = -1);
/** Toggles the current hemisphere. Parameter _tool describes, for which tool the hemisphere should change. Default -1 toggles all tools.
Index starts at "1" for the first tool (i.e. station number of Polhemus). Not 0!
*/
void ToggleHemisphere(int _tool = -1);
/** Convenient method to print the status of the tracking device (true/false) if connection is established. For debugging...*/
void PrintStatus();
/** Sets the Hemisphere of tool _tool to the vector _hemisphere. "-1" sets all tools.
Index starts at "1" for the first tool (i.e. station number of Polhemus). Not 0!
*/
void SetHemisphere(int _tool, mitk::Vector3D _hemisphere);
/** Get the Hemisphere for _tool as mitk vector. -1 ("all tools") returns hemisphere of first tool.
Index starts at "1" for the first tool (i.e. station number of Polhemus). Not 0!
*/
mitk::Vector3D GetHemisphere(int _tool);
/** Get the ports on which tools are connected. Returns empty vector if device is not connected!
*/
std::vector<int> GetToolPorts();
/** Is Hemisphere Tracking Enabled for this tool?
* if tool is -1, this means "All Tools". We return true if HemiTracking is enabled for all tools, and false if it is off for at least one tool.*/
bool GetHemisphereTrackingEnabled(int _tool);
/** Adjust the Hemisphere for this tool. User needs to make sure, that the tool is located in hemisphere (1|0|0) when calling this function.
In contrast to SetHemisphere(1,0,0), this method restores the original HemisphereTracking settings at the end. */
void AdjustHemisphere(int _tool);
/** @return Returns a single frame. Only works if the tracking device is not in continous tracking mode. Returns an empty vector in case of an error.*/
std::vector<trackingData> GetSingleFrame();
+ /** @return Returns a single frame with all tools, which could be auto detected.*/
+ std::vector<trackingData> AutoDetectTools();
+
/** @return Returns the last frame when the tracking device is in continous tracking mode. Returns an empty vector in case of an error.*/
std::vector<trackingData> GetLastFrame();
protected:
/**
* \brief standard constructor
*/
PolhemusInterface();
/**
* \brief standard destructor
*/
~PolhemusInterface();
/** Polhemus liberty/patriot tracker object*/
CPDIdev* m_pdiDev;
/** Parses polhemus raw data to a collection of tracking data of single tools. */
std::vector<mitk::PolhemusInterface::trackingData> ParsePolhemusRawData(PBYTE pBuf, DWORD dwSize);
bool InitializeDevice();
bool SetupDevice();
//returns the index in the arrays of tool _tool. Eg. sensor 3 (_tool = 3) is the second tool --> index 1 in m_Hemispheres etc.
int GetToolIndex(int _tool);
/** @brief Convenient method to get a frame from the tracking device.
* @return Returns a single OR the last frame depending on m_continuousTracking.
* @warning Don't use this function if you use different threads. You need to make sure, that you are still in the right mode! */
std::vector<trackingData> GetFrame();
private:
//returns vector with tool index as only element if tool != -1, else returns vector from 0 to numberOfTools
std::vector<int> GetToolIterator(int _tool);
+ //helper method to open connection
+ bool OpenConnection();
+
private:
//Stores the hemispheres for all sensors. Default is (1|0|0).
std::vector<mitk::Vector3D> m_Hemispheres;
//Stores, if hemisphereTracking is on for this Sensor.
std::vector<bool> m_HemisphereTracking;
//This vector stores the order of tools, which are available.
//E.g. only Sensor 1 and 3 are attached, then this vector maps the first tool (0) to Polhemus identifier 1 and the second tool (1) to Polhemus 3.
std::vector<int> m_ToolPorts;
unsigned int m_numberOfTools;
bool m_continousTracking;
};
}//mitk
#endif
diff --git a/Modules/IGT/TrackingDevices/mitkPolhemusTrackingDevice.cpp b/Modules/IGT/TrackingDevices/mitkPolhemusTrackingDevice.cpp
index d2a053f4e4..95ceb41457 100644
--- a/Modules/IGT/TrackingDevices/mitkPolhemusTrackingDevice.cpp
+++ b/Modules/IGT/TrackingDevices/mitkPolhemusTrackingDevice.cpp
@@ -1,348 +1,332 @@
/*===================================================================
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 "mitkPolhemusTrackingDevice.h"
#include "mitkPolhemusTool.h"
#include "mitkIGTConfig.h"
#include "mitkIGTTimeStamp.h"
#include "mitkIGTHardwareException.h"
#include <itksys/SystemTools.hxx>
#include <iostream>
#include <itkMutexLockHolder.h>
#include "mitkPolhemusTrackerTypeInformation.h"
#include <vtkConeSource.h>
-
-
typedef itk::MutexLockHolder<itk::FastMutexLock> MutexLockHolder;
-
-mitk::PolhemusTrackingDevice::PolhemusTrackingDevice(): mitk::TrackingDevice()
+mitk::PolhemusTrackingDevice::PolhemusTrackingDevice() : mitk::TrackingDevice()
{
//set the type of this tracking device
this->m_Data = mitk::PolhemusTrackerTypeInformation::GetDeviceDataPolhemusTrackerLiberty();
this->m_MultiThreader = itk::MultiThreader::New();
m_ThreadID = 0;
m_Device = mitk::PolhemusInterface::New();
-
}
mitk::PolhemusTrackingDevice::~PolhemusTrackingDevice()
{
}
bool mitk::PolhemusTrackingDevice::IsDeviceInstalled()
{
return true;
}
-
mitk::TrackingTool* mitk::PolhemusTrackingDevice::AddTool(const char* toolName, int toolPort)
{
//Only add tool if port isn't already used.
for (auto _tool : m_AllTools)
{
if (_tool->GetToolPort() == toolPort)
{
MITK_DEBUG << "There is already a tool connected to this port. Returning existing tool";
return _tool;
}
}
mitk::PolhemusTool::Pointer t = mitk::PolhemusTool::New();
t->SetToolName(toolName);
t->SetToolPort(toolPort);
if (this->InternalAddTool(t) == false)
return nullptr;
return t.GetPointer();
}
bool mitk::PolhemusTrackingDevice::InternalAddTool(PolhemusTool::Pointer tool)
{
m_AllTools.push_back(tool);
return true;
}
bool mitk::PolhemusTrackingDevice::StartTracking()
{
bool success = m_Device->StartTracking();
if (success)
{
mitk::IGTTimeStamp::GetInstance()->Start(this);
this->SetState(Tracking);
this->m_StopTrackingMutex->Lock();
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
return true;
}
else
{
this->SetState(Ready);
mitkThrowException(mitk::IGTHardwareException) << "Error while trying to start the device!";
}
return success;
}
bool mitk::PolhemusTrackingDevice::StopTracking()
{
m_Device->StopTracking();
return Superclass::StopTracking();
}
-
unsigned int mitk::PolhemusTrackingDevice::GetToolCount() const
{
return (unsigned int)this->m_AllTools.size();
}
-
mitk::TrackingTool* mitk::PolhemusTrackingDevice::GetTool(unsigned int toolNumber) const
{
- if ( toolNumber >= this->GetToolCount())
+ if (toolNumber >= this->GetToolCount())
return nullptr;
else
return this->m_AllTools[toolNumber];
}
-
bool mitk::PolhemusTrackingDevice::OpenConnection()
{
//reset everything
- if (m_Device.IsNull()) {m_Device = mitk::PolhemusInterface::New();}
+ if (m_Device.IsNull()) { m_Device = mitk::PolhemusInterface::New(); }
if (!m_Device->Connect()) //Connect the device, if it fails, throw an error.
{
MITK_ERROR << "Cannot connect Polhemus device!";
CloseConnection();
return false;
}
- m_Device->SetHemisphereTrackingEnabled(m_HemisphereTrackingEnabled);
+
+ //Ready must be set here, 'cause if tools don't match we need to be able to disconnect.
+ this->SetState(Ready);
//check if connected ports of Polhemus matches the tools in the toolStorage.
std::vector<int> toolPorts = m_Device->GetToolPorts();
//first, check size.
if (this->GetToolCount() != toolPorts.size())
{
- MITK_WARN << "Cannot connect device, number of tools in toolstorage doesn't match the number of tools connected to Polhemus device!";
+ MITK_ERROR << "Cannot connect device, number of tools in toolstorage doesn't match the number of tools connected to Polhemus device!";
CloseConnection();
return false;
}
//second, check if toolStorage identifier is included in this port.
for (auto _tool : m_AllTools)
{
if (std::find(toolPorts.begin(), toolPorts.end(), _tool->GetToolPort()) == toolPorts.end())
{
- MITK_WARN << "Cannot connect device, tool " << _tool->GetToolPort() << " is not connected to its port.";
+ MITK_ERROR << "Cannot connect device, tool " << _tool->GetToolPort() << " is not connected to its port.";
CloseConnection();
return false;
}
else
{
//erase this port to avoid that two tools want to connect to the same port.
toolPorts.erase(std::find(toolPorts.begin(), toolPorts.end(), _tool->GetToolPort()));
}
}
- this->SetState(Ready);
+ m_Device->SetHemisphereTrackingEnabled(m_HemisphereTrackingEnabled);
+
return true;
}
bool mitk::PolhemusTrackingDevice::CloseConnection()
{
bool returnValue = true;
if (this->GetState() == Setup)
return true;
returnValue = m_Device->Disconnect();
this->SetState(Setup);
return returnValue;
}
-
mitk::PolhemusInterface* mitk::PolhemusTrackingDevice::GetDevice()
{
return m_Device;
}
-
std::vector<mitk::PolhemusTool::Pointer> mitk::PolhemusTrackingDevice::GetAllTools()
{
return this->m_AllTools;
}
-
void mitk::PolhemusTrackingDevice::TrackTools()
{
try
{
/* 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
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();
Sleep(100);//Wait a bit until the tracker is ready...
-
-
while ((this->GetState() == Tracking) && (localStopTracking == false))
{
std::vector<mitk::PolhemusInterface::trackingData> lastData = this->GetDevice()->GetLastFrame();
if (lastData.size() != m_AllTools.size())
{
MITK_WARN << "Tool count is corrupt. Aborting!";
}
else
{
-
-
std::vector<mitk::PolhemusTool::Pointer> allTools = this->GetAllTools();
for (int i = 0; i < allTools.size(); i++)
{
mitk::PolhemusTool::Pointer currentTool = allTools.at(i);
currentTool->SetDataValid(true);
currentTool->SetPosition(lastData.at(i).pos);
currentTool->SetOrientation(lastData.at(i).rot);
currentTool->SetIGTTimeStamp(mitk::IGTTimeStamp::GetInstance()->GetElapsed());
}
}
/* Update the local copy of m_StopTracking */
this->m_StopTrackingMutex->Lock();
localStopTracking = m_StopTracking;
this->m_StopTrackingMutex->Unlock();
-
}
}
- catch(...)
+ catch (...)
{
this->StopTracking();
mitkThrowException(mitk::IGTHardwareException) << "Error while trying to track tools. Thread stopped.";
}
}
ITK_THREAD_RETURN_TYPE mitk::PolhemusTrackingDevice::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;
}
PolhemusTrackingDevice *trackingDevice = (PolhemusTrackingDevice*)pInfo->UserData;
if (trackingDevice != nullptr)
trackingDevice->TrackTools();
return ITK_THREAD_RETURN_VALUE;
}
bool mitk::PolhemusTrackingDevice::AutoDetectToolsAvailable()
{
return true;
}
mitk::NavigationToolStorage::Pointer mitk::PolhemusTrackingDevice::AutoDetectTools()
{
- this->OpenConnection();
- std::vector<mitk::PolhemusInterface::trackingData> singeFrameData = this->m_Device->GetSingleFrame();
+ std::vector<mitk::PolhemusInterface::trackingData> singeFrameData = this->m_Device->AutoDetectTools();
MITK_INFO << "Found " << singeFrameData.size() << " tools.";
- this->CloseConnection();
mitk::NavigationToolStorage::Pointer returnValue = mitk::NavigationToolStorage::New();
for each (mitk::PolhemusInterface::trackingData t in singeFrameData)
{
-
mitk::DataNode::Pointer newNode = mitk::DataNode::New();
std::stringstream name;
name << "Sensor-" << ((int)t.id);
newNode->SetName(name.str());
mitk::Surface::Pointer myCone = mitk::Surface::New();
vtkConeSource *vtkData = vtkConeSource::New();
vtkData->SetAngle(5.0);
vtkData->SetResolution(50);
vtkData->SetHeight(6.0f);
vtkData->SetRadius(2.0f);
vtkData->SetCenter(0.0, 0.0, 0.0);
vtkData->Update();
myCone->SetVtkPolyData(vtkData->GetOutput());
vtkData->Delete();
newNode->SetData(myCone);
mitk::NavigationTool::Pointer newTool = mitk::NavigationTool::New();
newTool->SetDataNode(newNode);
//The identifier defines, which plug is used (e.g. "Sens 2" --> 2).
std::stringstream identifier;
identifier << ((int)t.id);
newTool->SetIdentifier(identifier.str());
newTool->SetTrackingDeviceType(mitk::PolhemusTrackerTypeInformation::GetDeviceDataPolhemusTrackerLiberty().Line);
returnValue->AddTool(newTool);
}
return returnValue;
}
void mitk::PolhemusTrackingDevice::SetHemisphereTrackingEnabled(bool _HemisphereTrackingEnabled)
{
//We need to remember if HemisphereTracking is switch on for this reason:
/* m_Device->SetHemi works only if the device is connected. However, GUI can also change if it is not connected.
In this case, we remember it in the m_HemisphereTrackingEnabled variable. And when connecting, we know, which
status is wanted from the user by GUI.
- */
- m_HemisphereTrackingEnabled = _HemisphereTrackingEnabled;
- this->m_Device->SetHemisphereTrackingEnabled(_HemisphereTrackingEnabled);
+ */
+ m_HemisphereTrackingEnabled = _HemisphereTrackingEnabled;
+ this->m_Device->SetHemisphereTrackingEnabled(_HemisphereTrackingEnabled);
}
void mitk::PolhemusTrackingDevice::ToggleHemisphere(int _tool)
{
- this->m_Device->ToggleHemisphere(_tool);
+ this->m_Device->ToggleHemisphere(_tool);
}
void mitk::PolhemusTrackingDevice::SetHemisphere(int _tool, mitk::Vector3D _hemisphere)
{
//If you set a hemisphere vector which is unequal (0|0|0), this means, that there is no hemisphere tracking any more
//disable the option, so that it can be reactivated... Also if it is just a single tool.
if (_hemisphere.GetNorm() != 0)
m_HemisphereTrackingEnabled = false;
this->m_Device->SetHemisphere(_tool, _hemisphere);
}
mitk::Vector3D mitk::PolhemusTrackingDevice::GetHemisphere(int _tool)
{
return this->m_Device->GetHemisphere(_tool);
}
bool mitk::PolhemusTrackingDevice::GetHemisphereTrackingEnabled(int _tool)
{
- return this->m_Device->GetHemisphereTrackingEnabled(_tool);
+ return this->m_Device->GetHemisphereTrackingEnabled(_tool);
}
void mitk::PolhemusTrackingDevice::AdjustHemisphere(int _tool)
{
return this->m_Device->AdjustHemisphere(_tool);
}
\ No newline at end of file