Medical Imaging Interaction Toolkit  2018.4.99-87d68d9f
Medical Imaging Interaction Toolkit
mitkPolhemusTrackingDevice.cpp
Go to the documentation of this file.
1 /*============================================================================
2 
3 The Medical Imaging Interaction Toolkit (MITK)
4 
5 Copyright (c) German Cancer Research Center (DKFZ)
6 All rights reserved.
7 
8 Use of this source code is governed by a 3-clause BSD license that can be
9 found in the LICENSE file.
10 
11 ============================================================================*/
12 
14 #include "mitkPolhemusTool.h"
15 #include "mitkIGTConfig.h"
16 #include "mitkIGTTimeStamp.h"
17 #include "mitkIGTHardwareException.h"
18 #include <itksys/SystemTools.hxx>
19 #include <iostream>
20 #include <itkMutexLockHolder.h>
22 
24 
26 {
27  //set the type of this tracking device
29 
30  this->m_MultiThreader = itk::MultiThreader::New();
31  m_ThreadID = 0;
32 
34 }
35 
37 {
38 }
39 
41 {
42  return true;
43 }
44 
45 mitk::TrackingTool* mitk::PolhemusTrackingDevice::AddTool(const char* toolName, int toolPort)
46 {
47  //Only add tool if port isn't already used.
48  for (auto _tool : m_AllTools)
49  {
50  if (_tool->GetToolPort() == toolPort)
51  {
52  MITK_DEBUG << "There is already a tool connected to this port. Returning existing tool";
53  return _tool;
54  }
55  }
56 
57  mitk::PolhemusTool::Pointer t = mitk::PolhemusTool::New();
58  t->SetToolName(toolName);
59  t->SetToolPort(toolPort);
60  if (this->InternalAddTool(t) == false)
61  return nullptr;
62  return t.GetPointer();
63 }
64 
65 bool mitk::PolhemusTrackingDevice::InternalAddTool(PolhemusTool::Pointer tool)
66 {
67  m_AllTools.push_back(tool);
68  return true;
69 }
70 
72 {
73  bool success = m_Device->StartTracking();
74  if (success)
75  {
77  this->SetState(Tracking);
78  this->m_StopTrackingMutex->Lock();
79  this->m_StopTracking = false;
80  this->m_StopTrackingMutex->Unlock();
81  m_ThreadID = m_MultiThreader->SpawnThread(this->ThreadStartTracking, this); // start a new thread that executes the TrackTools() method
82  return true;
83  }
84  else
85  {
86  this->SetState(Ready);
87  mitkThrowException(mitk::IGTHardwareException) << "Error while trying to start the device!";
88  }
89  return success;
90 }
91 
93 {
94  m_Device->StopTracking();
95  return Superclass::StopTracking();
96 }
97 
99 {
100  return (unsigned int)this->m_AllTools.size();
101 }
102 
104 {
105  if (toolNumber >= this->GetToolCount())
106  return nullptr;
107  else
108  return this->m_AllTools[toolNumber];
109 }
110 
112 {
113  //reset everything
114  if (m_Device.IsNull()) { m_Device = mitk::PolhemusInterface::New(); }
115  if (!m_Device->Connect()) //Connect the device, if it fails, throw an error.
116  {
117  MITK_ERROR << "Cannot connect Polhemus device!";
118  CloseConnection();
119  return false;
120  }
121 
122  //Ready must be set here, 'cause if tools don't match we need to be able to disconnect.
123  this->SetState(Ready);
124 
125  //check if connected ports of Polhemus matches the tools in the toolStorage.
126  std::vector<int> toolPorts = m_Device->GetToolPorts();
127 
128  //first, check size.
129  if (this->GetToolCount() != toolPorts.size())
130  {
131  MITK_ERROR << "Cannot connect device, number of tools in toolstorage doesn't match the number of tools connected to Polhemus device!";
132  CloseConnection();
133  return false;
134  }
135 
136  //second, check if toolStorage identifier is included in this port.
137  for (auto _tool : m_AllTools)
138  {
139  if (std::find(toolPorts.begin(), toolPorts.end(), _tool->GetToolPort()) == toolPorts.end())
140  {
141  MITK_ERROR << "Cannot connect device, tool " << _tool->GetToolPort() << " is not connected to its port.";
142  CloseConnection();
143  return false;
144  }
145  else
146  {
147  //erase this port to avoid that two tools want to connect to the same port.
148  toolPorts.erase(std::find(toolPorts.begin(), toolPorts.end(), _tool->GetToolPort()));
149  }
150  }
151 
152  m_Device->SetHemisphereTrackingEnabled(m_HemisphereTrackingEnabled);
153 
154  return true;
155 }
156 
158 {
159  bool returnValue = true;
160  if (this->GetState() == Setup)
161  return true;
162 
163  returnValue = m_Device->Disconnect();
164 
165  this->SetState(Setup);
166  return returnValue;
167 }
168 
170 {
171  return m_Device;
172 }
173 
174 std::vector<mitk::PolhemusTool::Pointer> mitk::PolhemusTrackingDevice::GetAllTools()
175 {
176  return this->m_AllTools;
177 }
178 
180 {
181  try
182  {
183  /* lock the TrackingFinishedMutex to signal that the execution rights are now transfered to the tracking thread */
184  MutexLockHolder trackingFinishedLockHolder(*m_TrackingFinishedMutex); // keep lock until end of scope
185 
186  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
187  this->m_StopTrackingMutex->Lock(); // update the local copy of m_StopTracking
188  localStopTracking = this->m_StopTracking;
189  this->m_StopTrackingMutex->Unlock();
190  Sleep(100);//Wait a bit until the tracker is ready...
191 
192  while ((this->GetState() == Tracking) && (localStopTracking == false))
193  {
194  std::vector<mitk::PolhemusInterface::trackingData> lastData = this->GetDevice()->GetLastFrame();
195 
196  if (lastData.size() != m_AllTools.size())
197  {
198  MITK_WARN << "Tool count is corrupt. Hardware gives " << lastData.size() << " tools, MITK expects " << m_AllTools.size() << " tools. Aborting!";
199  }
200  else
201  {
202  std::vector<mitk::PolhemusTool::Pointer> allTools = this->GetAllTools();
203  for (size_t i = 0; i < allTools.size(); i++)
204  {
205  mitk::PolhemusTool::Pointer currentTool = allTools.at(i);
206 
207  const int distortionLevel = lastData.at(i).distortionLevel;
208 
209  if (distortionLevel == 0)
210  {
211  currentTool->SetDataValid(true);
212  }
213  else
214  {
215  currentTool->SetDataValid(false);
216  }
217 
218  currentTool->SetDistortionLevel(distortionLevel);
219  currentTool->SetPosition(lastData.at(i).pos);
220  currentTool->SetOrientation(lastData.at(i).rot);
221  currentTool->SetIGTTimeStamp(mitk::IGTTimeStamp::GetInstance()->GetElapsed());
222  }
223  }
224  /* Update the local copy of m_StopTracking */
225  this->m_StopTrackingMutex->Lock();
226  localStopTracking = m_StopTracking;
227  this->m_StopTrackingMutex->Unlock();
228  }
229  }
230  catch (...)
231  {
232  this->StopTracking();
233  mitkThrowException(mitk::IGTHardwareException) << "Error while trying to track tools. Thread stopped.";
234  }
235 }
236 
237 ITK_THREAD_RETURN_TYPE mitk::PolhemusTrackingDevice::ThreadStartTracking(void* pInfoStruct)
238 {
239  /* extract this pointer from Thread Info structure */
240  struct itk::MultiThreader::ThreadInfoStruct * pInfo = (struct itk::MultiThreader::ThreadInfoStruct*)pInfoStruct;
241  if (pInfo == nullptr)
242  {
243  return ITK_THREAD_RETURN_VALUE;
244  }
245  if (pInfo->UserData == nullptr)
246  {
247  return ITK_THREAD_RETURN_VALUE;
248  }
249  PolhemusTrackingDevice *trackingDevice = (PolhemusTrackingDevice*)pInfo->UserData;
250 
251  if (trackingDevice != nullptr)
252  trackingDevice->TrackTools();
253 
254  return ITK_THREAD_RETURN_VALUE;
255 }
256 
258 {
259  return true;
260 }
261 
262 mitk::NavigationToolStorage::Pointer mitk::PolhemusTrackingDevice::AutoDetectTools()
263 {
264  std::vector<mitk::PolhemusInterface::trackingData> singeFrameData = this->m_Device->AutoDetectTools();
265  MITK_INFO << "Found " << singeFrameData.size() << " tools.";
266  mitk::NavigationToolStorage::Pointer returnValue = mitk::NavigationToolStorage::New();
267  for each (mitk::PolhemusInterface::trackingData t in singeFrameData)
268  {
269  mitk::NavigationTool::Pointer newTool = mitk::NavigationTool::New();
270 
271  std::stringstream name;
272  name << "Sensor-" << ((int)t.id);
273  newTool->GetDataNode()->SetName(name.str());
274 
275  //The identifier defines, which plug is used (e.g. "Sens 2" --> 2).
276  std::stringstream identifier;
277  identifier << ((int)t.id);
278  newTool->SetIdentifier(identifier.str());
279 
281  returnValue->AddTool(newTool);
282  }
283  return returnValue;
284 }
285 
287 {
288  //We need to remember if HemisphereTracking is switch on for this reason:
289  /* m_Device->SetHemi works only if the device is connected. However, GUI can also change if it is not connected.
290  In this case, we remember it in the m_HemisphereTrackingEnabled variable. And when connecting, we know, which
291  status is wanted from the user by GUI.
292  */
293  m_HemisphereTrackingEnabled = _HemisphereTrackingEnabled;
294  this->m_Device->SetHemisphereTrackingEnabled(_HemisphereTrackingEnabled);
295 }
296 
298 {
299  this->m_Device->ToggleHemisphere(_tool);
300 }
301 
303 {
304  //If you set a hemisphere vector which is unequal (0|0|0), this means, that there is no hemisphere tracking any more
305  //disable the option, so that it can be reactivated... Also if it is just a single tool.
306  if (_hemisphere.GetNorm() != 0)
308 
309  this->m_Device->SetHemisphere(_tool, _hemisphere);
310 }
311 
313 {
314  return this->m_Device->GetHemisphere(_tool);
315 }
316 
318 {
319  return this->m_Device->GetHemisphereTrackingEnabled(_tool);
320 }
321 
323 {
324  return this->m_Device->AdjustHemisphere(_tool);
325 }
An object of this class represents the interface to Polhemus trackers. All variables with the name "t...
An object of this class represents an exception of the MITK-IGT module which are releated to the hard...
Descibes a line.
Definition: mitkLine.h:28
Interface for all Tracking Tools.
An object of this class represents Polhemus tracking device. You can add tools to this device...
#define MITK_INFO
Definition: mitkLogMacros.h:18
static ITK_THREAD_RETURN_TYPE ThreadStartTracking(void *data)
itk::FastMutexLock::Pointer m_TrackingFinishedMutex
mutex to manage control flow of StopTracking()
mitk::TrackingTool * AddTool(const char *toolName, int toolPort)
Create a new Polhemus tool with toolName and add it to the list of tools.
#define MITK_ERROR
Definition: mitkLogMacros.h:20
void SetHemisphereTrackingEnabled(bool _HemisphereTrackingEnabled)
virtual bool StartTracking() override
Starts the tracking.
virtual bool OpenConnection() override
Opens the connection to the device. This have to be done before the tracking is started.
#define MITK_DEBUG
Definition: mitkLogMacros.h:22
DataCollection - Class to facilitate loading/accessing structured data.
PolhemusInterface::Pointer m_Device
represents the interface to the tracking hardware
bool InternalAddTool(PolhemusTool::Pointer tool)
Adds a tool to the tracking device.
void TrackTools()
This method tracks tools as long as the variable m_Mode is set to "Tracking". Tracking tools means gr...
TrackingDeviceState GetState() const
return current object state (Setup, Ready or Tracking)
static Pointer New()
static Pointer New()
itk::MutexLockHolder< itk::FastMutexLock > MutexLockHolder
std::vector< PolhemusTool::Pointer > m_AllTools
vector holding all tools
virtual bool CloseConnection() override
Closes the connection and clears all resources.
#define MITK_WARN
Definition: mitkLogMacros.h:19
virtual mitk::NavigationToolStorage::Pointer AutoDetectTools()
itk::MultiThreader::Pointer m_MultiThreader
virtual bool StopTracking() override
Stops the tracking.
bool m_StopTracking
signal stop to tracking thread
Interface for all Tracking Devices.
static IGTTimeStamp * GetInstance()
returns a pointer to the current instance of mitkTimeStamp
void Start(itk::Object::Pointer device)
starts the time-acquisition
static Pointer New()
Constructs a NavigationToolStorage without reference to a DataStorage. The Data Nodes of tools have t...
static Pointer New()
TrackingDeviceData m_Data
current device Data
#define mitkThrowException(classname)
itk::FastMutexLock::Pointer m_StopTrackingMutex
mutex to control access to m_StopTracking
void SetHemisphere(int _tool, mitk::Vector3D _hemisphere)
TrackingTool * GetTool(unsigned int toolNumber) const override
std::vector< trackingData > GetLastFrame()
std::vector< PolhemusTool::Pointer > GetAllTools()
void SetState(TrackingDeviceState state)
change object state
virtual unsigned int GetToolCount() const override