Medical Imaging Interaction Toolkit  2016.11.0
Medical Imaging Interaction Toolkit
mitkMicroBirdTrackingDevice.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,
6 Division of Medical and Biological Informatics.
7 All rights reserved.
8 
9 This software is distributed WITHOUT ANY WARRANTY; without
10 even the implied warranty of MERCHANTABILITY or FITNESS FOR
11 A PARTICULAR PURPOSE.
12 
13 See LICENSE.txt or http://www.mitk.org for details.
14 
15 ===================================================================*/
16 
17 #include "mitkMicrobirdTrackingDevice.h"
18 
19 #include <itksys/SystemTools.hxx>
20 #include <itkMutexLockHolder.h>
21 
23 
25 m_ErrorMessage(""),
26 m_ThreadID(0),
27 m_pl(50), // 50 Hz for Europe
28 m_metric(true),
29 m_agcModeBoth(true),
30 m_measurementRate(68.3), // 68.3 for mid-range transmitter, 40.5 for flat transmitter
31 m_TransmitterConfig(NULL),
32 m_SensorConfig(NULL)
33 {
34  // Flat transmitter needs measurement rate: 40.5
35  // Mid-range transmitter needs measurement rate: 68.3;
36 
37  // Set the tracker type
38  this->m_Data = mitk::DeviceDataMicroBird;
39 
40  // Clear tools vector
41  m_Tools.clear();
42 
43  // Create tools vector mutex
45 
46  // Prepare multi-threading
48 
49  // Pointer to record member variable
50  pRecord = &record;
51 }
52 
53 
55 {
56  if (m_MultiThreader)
57  m_MultiThreader->TerminateThread(m_ThreadID);
58  m_MultiThreader = NULL;
59  if (m_ToolsMutex)
60  m_ToolsMutex->Unlock();
61  m_ToolsMutex = NULL;
62 
63  this->StopTracking();
64  this->CloseConnection();
65 
66  if (m_TransmitterConfig != NULL)
67  delete [] m_TransmitterConfig;
68  if (m_SensorConfig != NULL)
69  delete [] m_SensorConfig;
70 
71  //\TODO: Do we need to clean up the pointers to PCIBird data like DOUBLE_POSITION_QUATERNION_TIME_Q_RECORD?
72 }
73 
74 
76 {
77  /* Check whether in setup mode */
78  if (this->GetState() != Setup)
79  {
80  this->SetErrorMessage("Can only try to open the connection if in setup mode");
81  return false;
82  }
83 
84  int errorCode; // Holds error code
85 
86  /* Initialize the PCIBIRD driver and DLL */
87  errorCode = InitializeBIRDSystem(); // this function can take a few seconds
88  if (!CompareError(errorCode, BIRD_ERROR_SUCCESS))
89  {
90  HandleError(errorCode);
91  return false;
92  }
94  // Serial numbers could be compared to known ones for some simple
95  // parameters sanity check (measurement frequency etc.)
96 
97  /* Get system configuration */
98  errorCode = GetBIRDSystemConfiguration(&m_SystemConfig);
99  if (!CompareError(errorCode, BIRD_ERROR_SUCCESS))
100  {
101  HandleError(errorCode);
102  return false;
103  }
104 
105  /* use metric measurements in mm */
106  errorCode = SetSystemParameter(METRIC, &m_metric, sizeof(m_metric));
107  if (!CompareError(errorCode, BIRD_ERROR_SUCCESS))
108  {
109  HandleError(errorCode);
110  return false;
111  }
112 
113  /* Set the measurement rate to m_measurementRate */
114  if ((m_measurementRate > 30) && (m_measurementRate < 80))
115  {
116  errorCode = SetSystemParameter(MEASUREMENT_RATE, &m_measurementRate, sizeof(m_measurementRate));
117  if (!CompareError(errorCode, BIRD_ERROR_SUCCESS))
118  {
119  HandleError(errorCode);
120  return false;
121  }
122  }
123 
124  /* Set power line frequency */
125  if ((m_pl >= 50) && (m_pl <= 60))
126  {
127  errorCode = SetSystemParameter(POWER_LINE_FREQUENCY, &m_pl, sizeof(m_pl));
128  if (!CompareError(errorCode, BIRD_ERROR_SUCCESS))
129  {
130  HandleError(errorCode);
131  return false;
132  }
133  }
134 
135  /* Set AGC mode */
136  m_agc = m_agcModeBoth ? TRANSMITTER_AND_SENSOR_AGC : SENSOR_AGC_ONLY;
137  errorCode = SetSystemParameter(AGC_MODE, &m_agc, sizeof(m_agc));
138  if (!CompareError(errorCode, BIRD_ERROR_SUCCESS))
139  {
140  HandleError(errorCode);
141  return false;
142  }
143 
144  /* Get system configuration */
145  errorCode = GetBIRDSystemConfiguration(&m_SystemConfig);
146  if (!CompareError(errorCode, BIRD_ERROR_SUCCESS))
147  {
148  HandleError(errorCode);
149  return false;
150  }
151 
152  /* Get sensor information */
153  m_SensorConfig = new SENSOR_CONFIGURATION[m_SystemConfig.numberSensors];
154  for (int i = 0; i < m_SystemConfig.numberSensors; i++)
155  {
156  errorCode = GetSensorConfiguration(i, &(m_SensorConfig[i]));
157  if (!CompareError(errorCode, BIRD_ERROR_SUCCESS))
158  {
159  HandleError(errorCode);
160  }
161 
162  /* Initialize the quality parameter structure */
163  QUALITY_PARAMETERS qualityParameters; // = { 164, 0, 32, 3072 };
164  GetSensorParameter(i, QUALITY, &qualityParameters, sizeof(qualityParameters));
165 
166  /* Set data format to matrix format */
167  //DATA_FORMAT_TYPE tempBuffer = DOUBLE_POSITION_MATRIX_TIME_Q;
168  /* Set data format to quaternion format */
169  DATA_FORMAT_TYPE tempBuffer = DOUBLE_POSITION_QUATERNION_TIME_Q;
170 
171  /* Set data format for sensor */
172  DATA_FORMAT_TYPE *pTempBuffer = &tempBuffer;
173  errorCode = SetSensorParameter(i, DATA_FORMAT, pTempBuffer, sizeof(tempBuffer));
174  if (!CompareError(errorCode, BIRD_ERROR_SUCCESS))
175  {
176  HandleError(errorCode);
177  }
178  }
179 
180  /* Initialize tools vector */
181  {
182  MutexLockHolder(*m_ToolsMutex);
183  for (int i = 0; i < m_SystemConfig.numberSensors; i++)
184  {
185  if (m_SensorConfig[i].attached)
186  m_Tools.push_back(ToolType::New());
187 
188  }
189  }
190 
191  /* Get transmitter configuration */
192  m_TransmitterConfig = new TRANSMITTER_CONFIGURATION[m_SystemConfig.numberTransmitters];
193  for (int i = 0; i < m_SystemConfig.numberTransmitters; i++)
194  {
195  errorCode = GetTransmitterConfiguration(i, &(m_TransmitterConfig[i]));
196  if (!CompareError(errorCode, BIRD_ERROR_SUCCESS))
197  {
198  HandleError(errorCode);
199  }
200  }
201 
202  /* Switch off transmitter */
203  SwitchTransmitter(true);
204  SwitchTransmitter(false);
205 
206  // @todo : set up error scaling?
207 
208  /* finish - now all tools should be added, initialized and enabled, so that tracking can be started */
209  this->SetState(Ready);
210  this->SetErrorMessage("");
211  return true; // Return success
212 }
213 
214 
216 {
217  if (switchOn)
218  {
219  /* Search for the first attached transmitter and turn it on */
220  for (short id = 0; id < m_SystemConfig.numberTransmitters; id++)
221  {
222  if (m_TransmitterConfig[id].attached)
223  {
224  // Transmitter selection is a system function.
225  // Using the SELECT_TRANSMITTER parameter we send the id of the
226  // transmitter that we want to run with the SetSystemParameter() call
227  int errorCode = SetSystemParameter(SELECT_TRANSMITTER, &id, sizeof(id));
228  if (!CompareError(errorCode, BIRD_ERROR_SUCCESS))
229  {
230  HandleError(errorCode);
231  return false;
232  }
233  else
234  return true; //break; // \TODO: Stop after the first attached transmitter was turned off?
235  }
236  }
237  }
238  else
239  {
240  /* Transmitter selection is a system function, Note: a selector of -1 switches off the current transmitter */
241  short TRANSMITTER_OFF = -1;
242  int errorCode = SetSystemParameter(SELECT_TRANSMITTER, &TRANSMITTER_OFF, sizeof(TRANSMITTER_OFF));
243  if (!CompareError(errorCode, BIRD_ERROR_SUCCESS))
244  {
245  HandleError(errorCode);
246  return false;
247  }
248  else
249  return true;
250  }
251  // Return success
252  return true;
253 }
254 
255 
257 {
258  SwitchTransmitter(false); // Switch off the transmitter
259 
260  int errorCode = CloseBIRDSystem(); // Close connection. This function can take a few seconds
261 
262  // Error checking
263  if (!CompareError(errorCode, BIRD_ERROR_SUCCESS))
264  HandleError(errorCode);
265 
266  // Delete configuration
267  if (m_TransmitterConfig != NULL)
268  {
269  delete [] m_TransmitterConfig;
270  m_TransmitterConfig = NULL;
271  }
272  if (m_SensorConfig != NULL)
273  {
274  delete [] m_SensorConfig;
275  m_SensorConfig = NULL;
276  }
277  // Change mode and release mutex
278  this->SetState(Setup);
279 
280  // Clear error message
281  this->SetErrorMessage("");
282 
283  return true;
284 }
285 
286 
287 ITK_THREAD_RETURN_TYPE mitk::MicroBirdTrackingDevice::ThreadStartTracking(void* pInfoStruct)
288 {
289  /* extract this pointer from Thread Info structure */
290  struct itk::MultiThreader::ThreadInfoStruct * pInfo = (struct itk::MultiThreader::ThreadInfoStruct*)pInfoStruct;
291  if ((pInfo == NULL) || (pInfo->UserData == NULL))
292  return ITK_THREAD_RETURN_VALUE;
293 
294  MicroBirdTrackingDevice *trackingDevice = (MicroBirdTrackingDevice*)pInfo->UserData;
295  if (trackingDevice != NULL)
296  trackingDevice->TrackTools(); // call TrackTools() from the original object
297 
298  return ITK_THREAD_RETURN_VALUE;
299 }
300 
301 
303 {
304  TrackingDevice::StopTracking(); // Call superclass method
305 
306  SwitchTransmitter(false); // Switch off transmitter
307  InvalidateAll(); // Invalidate all tools
308  return true; // \todo : think about return value
309 }
310 
311 
313 {
314  if (this->GetState() != Ready)
315  return false;
316 
317  this->SetState(Tracking);
318 
319  /* Switch on transmitter */
320  SwitchTransmitter(true);
321 
322  /* Update the local copy of m_StopTracking */
323  this->m_StopTrackingMutex->Lock();
324  this->m_StopTracking = false;
325  this->m_StopTrackingMutex->Unlock();
326 
327  m_TrackingFinishedMutex->Unlock(); // transfer the execution rights to tracking thread
328  m_ThreadID = m_MultiThreader->SpawnThread(this->ThreadStartTracking, this); // start a new thread that executes the TrackTools() method
329  mitk::TimeStamp::GetInstance()->Start(this);
330  return true;
331 }
332 
333 
335 {
336  if (this->GetState() != Tracking)
337  return;
338 
339  /* Frequency configuration */
340  double updateRate = 1000.0 / m_SystemConfig.measurementRate;
341  double measurementDuration = 0.0;
342 
343 
344  // lock the TrackingFinishedMutex to signal that the execution rights
345  // are now transfered to the tracking thread
346  MutexLockHolder trackingFinishedLockHolder(*m_TrackingFinishedMutex); // keep lock until end of scope
347 
348  // Because m_StopTracking is used by two threads, access has to be guarded
349  // by a mutex. To minimize thread locking, a local copy is used here
350  bool localStopTracking;
351 
352  /* update the local copy of m_StopTracking */
353  this->m_StopTrackingMutex->Lock();
354  localStopTracking = this->m_StopTracking;
355  this->m_StopTrackingMutex->Unlock();
356 
357  /* Tracking loop */
358  while ((this->GetState() == Tracking) && (localStopTracking == false))
359  {
360  int errorCode;
361  unsigned int nOfAttachedSensors = 0;
362  double timeStamp = 0.0;
363  int toolNumber = 0; // Numbers for attached sensors only
364 
365  for (int sensorID = 0; sensorID < m_SystemConfig.numberSensors; sensorID++) // for each sensor grep data
366  {
367  if (!m_SensorConfig[sensorID].attached)
368  continue;
369 
370  // sensor attached so get record
371  errorCode = GetAsynchronousRecord(sensorID, pRecord, sizeof(record));
372  if (CompareError(errorCode, BIRD_ERROR_SUCCESS))
373  { // On SUCCESS, parse sensor information
374  nOfAttachedSensors++;
375  timeStamp += record.time; // Get timestamp from record
376  ToolType* tool = GetMicroBirdTool(toolNumber);
377  if (tool != NULL)
378  {
379  tool->SetTrackingError(record.quality); // Set tracking error (quality) from record
380  mitk::Point3D position;
381  position[0] = record.x;
382  position[1] = record.y;
383  position[2] = record.z;
384  tool->SetPosition(position); // Set position
385  mitk::Quaternion orientation(record.q[1], record.q[2], record.q[3],record.q[0]);
386  tool->SetOrientation(orientation); // Set orientation as quaternion \todo : verify quaternion order q(r,x,y,z)
387  tool->SetDataValid(true); // Set data state to valid
388  }
389  toolNumber++; // Increment tool number
390  }
391  else
392  { // ERROR while reading sensor information
393  HandleError(errorCode);
394  }
395  }
396 
398  // Average timestamp: timeStamp/nOfAttachedSensors
399 
400  // Compute sleep time
401  double sleepTime = updateRate - measurementDuration;
402  // Sleep
403  if (sleepTime > 0.0 && sleepTime < 500.0)
404  {
405  // Note: we only have to approximately sleep one measurement cycle,
406  // since the tracker keeps track of the measurement rate itself
407  itksys::SystemTools::Delay(sleepTime)
408  //Sleep(static_cast<DWORD>(sleepTime));
409  }
410 
411  // Update the local copy of m_StopTracking
412  this->m_StopTrackingMutex->Lock();
413  localStopTracking = m_StopTracking;
414  this->m_StopTrackingMutex->Unlock();
415  }
416 
417  // @bug (#1813) : maybe we need to check for localStopTracking=true here?
418  // m_StopTracking should only ever be updated by StopTracking(), so
419  // maybe we should not unlock a mutex that nobody is waiting for?
420 
421  return; // returning from this function (and ThreadStartTracking()) this will end the thread
422 }
423 
424 
426 {
427  return static_cast<TrackingTool*>(GetMicroBirdTool(toolNumber));
428 }
429 
430 
432 {
433  ToolType* t = NULL;
434 
435  MutexLockHolder toolsMutexLockHolder(*m_ToolsMutex); // lock and unlock the mutex
436  if (toolNumber < m_Tools.size())
437  {
438  t = m_Tools.at(toolNumber);
439  }
440  return t;
441 }
442 
443 
445 {
446  MutexLockHolder toolsMutexLockHolder(*m_ToolsMutex); // lock and unlock the mutex
447  return m_Tools.size();
448 }
449 
450 
451 bool mitk::MicroBirdTrackingDevice::CompareError(int errorCode, int errorConstant)
452 {
453  return ((errorCode & 0xffff) == errorConstant);
454 }
455 
456 
458 {
459  char buffer[1024];
460  char* pBuffer = &buffer[0];
461 
462  while(!CompareError(errorCode, BIRD_ERROR_SUCCESS))
463  {
464  // Print error number on screen
465  //cout << "MicroBIRD Error Code: " << errorCode << endl;
466  // Print error message on screen
467  errorCode = GetErrorText(errorCode, pBuffer, sizeof(buffer), SIMPLE_MESSAGE);
469  this->SetErrorMessage(buffer);
470  }
471 }
472 
473 
475 {
476  MutexLockHolder toolsMutexLockHolder(*m_ToolsMutex); // lock and unlock the mutex
477  for (ToolContainerType::iterator iterator = m_Tools.begin(); iterator != m_Tools.end(); ++iterator)
478  (*iterator)->SetDataValid(false);
479 }
Interface for all Tracking Tools.
virtual bool CloseConnection()
Closes the connection.
virtual void SetDataValid(bool _arg)
sets if the tracking data (position & Orientation) is valid
superclass for specific MIRCOBIRD tracking Devices
virtual TrackingTool * GetTool(unsigned int toolNumber)
returns a tracking tool that contains positional information about one of the sensors ...
virtual void InvalidateAll()
invalidates all tools (on stoptracking, closeconnection)
bool CompareError(int errorCode, int errorConstant)
itk::FastMutexLock::Pointer m_ToolsMutex
virtual bool OpenConnection()
Builds up the connection (loads tools, initializes and enables them)
virtual unsigned int GetToolCount() const
returns a the number of attached sensors
itk::MultiThreader::Pointer m_MultiThreader
itk::MutexLockHolder< itk::FastMutexLock > MutexLockHolder
virtual void SetPosition(Point3D position)
sets the position
implements TrackingTool interface
virtual bool StartTracking()
Start the tracking.
static ITK_THREAD_RETURN_TYPE ThreadStartTracking(void *data)
Helper function, because the itk::MultiThreader can only start a new thread with a static member func...
virtual bool StopTracking()
stop retrieving tracking data from the device. stop retrieving tracking data from the device...
DOUBLE_POSITION_QUATERNION_TIME_Q_RECORD * pRecord
One tracking data record (quaternion orientation format)
bool SwitchTransmitter(bool switchOn)
Switches the transmitter on resp. off.
Interface for all Tracking Devices.
vnl_quaternion< ScalarType > Quaternion
virtual void SetTrackingError(float error)
sets the tracking error
virtual bool StopTracking()
here we use the superclass method.
TrackingDeviceData m_Data
current device Data
virtual void SetOrientation(Quaternion orientation)
sets the orientation as a quaternion
virtual void TrackTools()
tracks the position and orientation of all tools until StopTracking() is called.
static void Setup()
DOUBLE_POSITION_QUATERNION_TIME_Q_RECORD record
ToolType * GetMicroBirdTool(unsigned int toolNumber)
returns a tracking tool that contains positional information about one of the sensors ...
static itkEventMacro(BoundingShapeInteractionEvent, itk::AnyEvent) class MITKBOUNDINGSHAPE_EXPORT BoundingShapeInteractor Pointer New()
Basic interaction methods for mitk::GeometryData.