PlusLib  2.9.0
Software library for tracked ultrasound image acquisition, calibration, and processing.
vtkPlusBrachyTracker.cxx
Go to the documentation of this file.
1 /*=Plus=header=begin======================================================
2 Program: Plus
3 Copyright (c) Laboratory for Percutaneous Surgery. All rights reserved.
4 See License.txt for details.
5 =========================================================Plus=header=end*/
6 
7 #include "PlusConfigure.h"
8 
9 #include "PlusBrachyStepper.h"
10 #include "PlusCivcoBrachyStepper.h"
11 #include "PlusCmsBrachyStepper.h"
12 #include "igsioTrackedFrame.h"
13 #include "vtkIGSIOAccurateTimer.h"
14 #include "vtkPlusBrachyTracker.h"
15 #include "vtkMath.h"
16 #include "vtkMatrix4x4.h"
17 #include "vtkObjectFactory.h"
18 #include "vtkPlusChannel.h"
19 #include "vtkPlusDataSource.h"
20 #include "vtkIGSIOTrackedFrameList.h"
21 #include "vtkTransform.h"
22 #include "vtkXMLDataElement.h"
23 #include "vtksys/SystemTools.hxx"
24 #include <sstream>
25 
27 
28 //----------------------------------------------------------------------------
30 {
31  this->Device = NULL;
32  this->ModelVersion = NULL;
33  this->ModelNumber = NULL;
34  this->ModelSerialNumber = NULL;
35  this->CalibrationAlgorithmVersion = NULL;
36  this->CalibrationDate = NULL;
37 
38  this->SetSerialPort(1);
39  this->BaudRate = 19200;
40 
41  this->SetToolReferenceFrameName("StepperHome");
42 
43  // Add tools to the tracker
44  vtkSmartPointer<vtkPlusDataSource> probeTool = vtkSmartPointer<vtkPlusDataSource>::New();
45  probeTool->SetId("Probe");
46  std::ostringstream probePortName;
47  probePortName << PROBEHOME_TO_PROBE_TRANSFORM;
48  probeTool->SetPortName(probePortName.str().c_str());
49  this->AddTool(probeTool);
50 
51  vtkSmartPointer<vtkPlusDataSource> templateTool = vtkSmartPointer<vtkPlusDataSource>::New();
52  templateTool->SetId("Template");
53  std::ostringstream templatePortName;
54  templatePortName << TEMPLATEHOME_TO_TEMPLATE_TRANSFORM;
55  templateTool->SetPortName(templatePortName.str().c_str());
56  this->AddTool(templateTool);
57 
58  vtkSmartPointer<vtkPlusDataSource> encoderTool = vtkSmartPointer<vtkPlusDataSource>::New();
59  encoderTool->SetId("StepperEncoderValues");
60  std::ostringstream encoderPortName;
61  encoderPortName << RAW_ENCODER_VALUES;
62  encoderTool->SetPortName(encoderPortName.str().c_str());
63  this->AddTool(encoderTool);
64 
66 
67  // Stepper calibration parameters
68  this->CompensationEnabledOn();
71  this->SetProbeRotationAxisOrientation(0, 0, 1);
73 
75 
76  // No callback function provided by the device, so the data capture thread will be used to poll the hardware and add new items to the buffer
77  this->StartThreadForInternalUpdates = true;
78  this->AcquisitionRate = 20;
79 }
80 
81 //----------------------------------------------------------------------------
83 {
84  if (this->Recording)
85  {
86  this->StopRecording();
87  }
88 
89  if (this->Device != NULL)
90  {
91  delete this->Device;
92  this->Device = NULL;
93  }
94 
95  this->SetModelVersion(NULL);
96  this->SetModelNumber(NULL);
97  this->SetModelSerialNumber(NULL);
99  this->SetCalibrationDate(NULL);
100 
101 }
102 
103 //----------------------------------------------------------------------------
104 void vtkPlusBrachyTracker::PrintSelf(ostream& os, vtkIndent indent)
105 {
106  Superclass::PrintSelf(os, indent);
107 }
108 
109 //-----------------------------------------------------------------------------
111 {
112  if (this->Device == NULL)
113  {
114  LOG_ERROR("Failed to connect to brachy tracker - BrachyStepperType not selected, device is NULL!");
115  return PLUS_FAIL;
116  }
117 
118  return this->Device->Connect();
119 }
120 
121 //-----------------------------------------------------------------------------
123 {
124  this->Device->Disconnect();
125  return this->StopRecording();
126 }
127 
128 //----------------------------------------------------------------------------
130 {
131  if (this->Recording)
132  {
133  return PLUS_SUCCESS;
134  }
135 
136  if (!this->Connect())
137  {
138  LOG_ERROR("Unable to connect to stepper on port: " << this->GetSerialPort());
139  return PLUS_FAIL;
140  }
141 
142  this->Disconnect();
143 
144  return PLUS_SUCCESS;
145 }
146 
147 //----------------------------------------------------------------------------
149 {
150  if (this->IsRecording())
151  {
152  return PLUS_SUCCESS;
153  }
154 
155  if (this->InitBrachyTracker() != PLUS_SUCCESS)
156  {
157  LOG_ERROR("Couldn't initialize brachy stepper.");
158  return PLUS_FAIL;
159  }
160 
161  return PLUS_SUCCESS;
162 }
163 
164 //----------------------------------------------------------------------------
166 {
167  return PLUS_SUCCESS;
168 }
169 
170 //----------------------------------------------------------------------------
172 {
173  std::ostringstream toolPortName;
174  toolPortName << tool;
175  vtkPlusDataSource* trackerTool = NULL;
176  if (this->GetToolByPortName(toolPortName.str(), trackerTool) != PLUS_SUCCESS)
177  {
178  LOG_ERROR("Failed to get tool source ID by port: " << toolPortName.str());
179  return "";
180  }
181  std::string sourceId = trackerTool->GetId();
182  return sourceId;
183 }
184 
185 //----------------------------------------------------------------------------
187 {
188  ToolStatus status = TOOL_OK;
189 
190  if (!this->Recording)
191  {
192  LOG_ERROR("called Update() when Brachy stepper was not tracking");
193  return PLUS_FAIL;
194  }
195 
196  // get the transforms from stepper
197  double dProbePosition(0), dTemplatePosition(0), dProbeRotation(0);
198  unsigned long frameNum(0);
199  if (!this->Device->GetEncoderValues(dProbePosition, dTemplatePosition, dProbeRotation, frameNum))
200  {
201  LOG_DEBUG("Tracker request timeout...");
202  // Unable to get tracking information from tracker
203  status = TOOL_REQ_TIMEOUT;
204  }
205  LOG_TRACE("Encoder values: "
206  << "(Probe position) " << dProbePosition << ", "
207  << "(Probe rotation) " << dProbeRotation << ", "
208  << "(Template position) " << dTemplatePosition << ", "
209  << "(Frame number) " << frameNum);
210 
211  const double unfilteredTimestamp = vtkIGSIOAccurateTimer::GetSystemTime();
212 
213  // Save probe position to the matrix (0,3) element
214  // Save probe rotation to the matrix (1,3) element
215  // Save grid position to the matrix (2,3) element
216  vtkSmartPointer<vtkMatrix4x4> probePosition = vtkSmartPointer<vtkMatrix4x4>::New();
217  probePosition->SetElement(ROW_PROBE_POSITION, 3, dProbePosition);
218  probePosition->SetElement(ROW_PROBE_ROTATION, 3, dProbeRotation);
219  probePosition->SetElement(ROW_TEMPLATE_POSITION, 3, dTemplatePosition);
220 
221  // Update encoder values tool
222  if (this->ToolTimeStampedUpdate(this->GetBrachyToolSourceId(RAW_ENCODER_VALUES).c_str(), probePosition, status, frameNum, unfilteredTimestamp) != PLUS_SUCCESS)
223  {
224  LOG_ERROR("Failed to update tool: " << this->GetBrachyToolSourceId(RAW_ENCODER_VALUES));
225  return PLUS_FAIL;
226  }
227 
228  if (!this->CompensationEnabled)
229  {
230 
231  // Update template transform tool
232  vtkSmartPointer<vtkTransform> tTemplateHomeToTemplate = vtkSmartPointer<vtkTransform>::New();
233  tTemplateHomeToTemplate->Translate(0, 0, dTemplatePosition);
234  if (this->ToolTimeStampedUpdate(this->GetBrachyToolSourceId(TEMPLATEHOME_TO_TEMPLATE_TRANSFORM).c_str(), tTemplateHomeToTemplate->GetMatrix(), status, frameNum, unfilteredTimestamp) != PLUS_SUCCESS)
235  {
236  LOG_ERROR("Failed to update tool: " << this->GetBrachyToolSourceId(TEMPLATEHOME_TO_TEMPLATE_TRANSFORM));
237  return PLUS_FAIL;
238  }
239 
240  // Update probe transform tool
241  vtkSmartPointer<vtkTransform> tProbeHomeToProbe = vtkSmartPointer<vtkTransform>::New();
242  tProbeHomeToProbe->Translate(0, 0, dProbePosition);
243  tProbeHomeToProbe->RotateZ(dProbeRotation);
244  if (this->ToolTimeStampedUpdate(this->GetBrachyToolSourceId(PROBEHOME_TO_PROBE_TRANSFORM).c_str(), tProbeHomeToProbe->GetMatrix(), status, frameNum, unfilteredTimestamp) != PLUS_SUCCESS)
245  {
246  LOG_ERROR("Failed to update tool: " << this->GetBrachyToolSourceId(PROBEHOME_TO_PROBE_TRANSFORM));
247  return PLUS_FAIL;
248  }
249  return PLUS_SUCCESS;
250  }
251 
252  // Save template home to template transform
253  vtkSmartPointer<vtkTransform> tTemplateHomeToTemplate = vtkSmartPointer<vtkTransform>::New();
254  double templateTranslationAxisVector[3];
255  this->GetTemplateTranslationAxisOrientation(templateTranslationAxisVector);
256  vtkMath::MultiplyScalar(templateTranslationAxisVector, dTemplatePosition);
257  tTemplateHomeToTemplate->Translate(templateTranslationAxisVector);
258  // send the transformation matrix and status to the tool
259  if (this->ToolTimeStampedUpdate(this->GetBrachyToolSourceId(TEMPLATEHOME_TO_TEMPLATE_TRANSFORM).c_str(), tTemplateHomeToTemplate->GetMatrix(), status, frameNum, unfilteredTimestamp) != PLUS_SUCCESS)
260  {
261  LOG_ERROR("Failed to update tool: " << this->GetBrachyToolSourceId(TEMPLATEHOME_TO_TEMPLATE_TRANSFORM));
262  return PLUS_FAIL;
263  }
264 
265  // Save probehome to probe transform
266  vtkSmartPointer<vtkTransform> tProbeHomeToProbe = vtkSmartPointer<vtkTransform>::New();
267  // Translate the probe to the desired position
268  double probeTranslationVector[3];
269  this->GetProbeTranslationAxisOrientation(probeTranslationVector);
270  vtkMath::MultiplyScalar(probeTranslationVector, dProbePosition);
271  tProbeHomeToProbe->Translate(probeTranslationVector);
272 
273  // Translate the probe to the compensated rotation axis before the rotation
274  double probeRotationVector[3];
275  this->GetProbeRotationAxisOrientation(probeRotationVector);
276  vtkMath::MultiplyScalar(probeRotationVector, dProbePosition);
277  tProbeHomeToProbe->Translate(probeRotationVector);
278  const double compensatedProbeRotation = this->ProbeRotationEncoderScale * dProbeRotation;
279  tProbeHomeToProbe->RotateZ(compensatedProbeRotation);
280  // Translate back the probe to the original position
281  tProbeHomeToProbe->Translate(-probeRotationVector[0], -probeRotationVector[1], -probeRotationVector[2]);
282  // send the transformation matrix and status to the tool
283  if (this->ToolTimeStampedUpdate(this->GetBrachyToolSourceId(PROBEHOME_TO_PROBE_TRANSFORM).c_str(), tProbeHomeToProbe->GetMatrix(), status, frameNum, unfilteredTimestamp) != PLUS_SUCCESS)
284  {
285  LOG_ERROR("Failed to update tool: " << this->GetBrachyToolSourceId(PROBEHOME_TO_PROBE_TRANSFORM));
286  return PLUS_FAIL;
287  }
288 
289  return PLUS_SUCCESS;
290 }
291 
292 //----------------------------------------------------------------------------
294 {
295  // Connect to device
296  if (!this->Connect())
297  {
298  LOG_ERROR("Unable to connect to stepper on port: " << this->GetSerialPort());
299  return PLUS_FAIL;
300  }
301 
302  std::string version;
303  std::string model;
304  std::string serial;
305  if (this->Device->GetDeviceModelInfo(version, model, serial) != PLUS_SUCCESS)
306  {
307  LOG_ERROR("Couldn't get version info from stepper.");
308  return PLUS_FAIL;
309  }
310 
311  this->SetModelVersion(version.c_str());
312  this->SetModelNumber(model.c_str());
313  this->SetModelSerialNumber(serial.c_str());
314 
315  return PLUS_SUCCESS;
316 }
317 
318 //----------------------------------------------------------------------------
319 PlusStatus vtkPlusBrachyTracker::ReadConfiguration(vtkXMLDataElement* rootConfigElement)
320 {
321  XML_FIND_DEVICE_ELEMENT_REQUIRED_FOR_READING(deviceConfig, rootConfigElement);
322 
323  XML_READ_SCALAR_ATTRIBUTE_OPTIONAL(unsigned long, SerialPort, deviceConfig);
324  XML_READ_SCALAR_ATTRIBUTE_OPTIONAL(unsigned long, BaudRate, deviceConfig);
325 
326  if (!this->IsRecording())
327  {
328  const char* brachyStepperType = deviceConfig->GetAttribute("BrachyStepperType");
329  if (brachyStepperType == NULL)
330  {
331  LOG_ERROR("Unable to find BrachyStepperType attribute in configuration file");
332  return PLUS_FAIL;
333  }
334 
335  // Delete device before we change it
336  if (this->Device != NULL)
337  {
338  delete this->Device;
339  }
340 
342  {
343  this->Device = new PlusCmsBrachyStepper(this->GetSerialPort(), this->GetBaudRate());
346 
347  }
349  {
350  this->Device = new PlusCmsBrachyStepper(this->GetSerialPort(), this->GetBaudRate());
353  }
354  else if (STRCASECMP(PlusBrachyStepper::GetBrachyStepperTypeInString(PlusBrachyStepper::CMS_ACCUSEED_DS300).c_str(), brachyStepperType) == 0)
355  {
356  this->Device = new PlusCmsBrachyStepper(this->GetSerialPort(), this->GetBaudRate());
359  }
360  else if (STRCASECMP(PlusBrachyStepper::GetBrachyStepperTypeInString(PlusBrachyStepper::CIVCO_STEPPER).c_str(), brachyStepperType) == 0)
361  {
362  this->Device = new PlusCivcoBrachyStepper(this->GetSerialPort(), this->GetBaudRate());
365  }
366  else
367  {
368  LOG_ERROR("Unable to recognize brachy stepper type: " << brachyStepperType);
369  return PLUS_FAIL;
370  }
371 
372  XML_READ_CSTRING_ATTRIBUTE_OPTIONAL(ModelNumber, deviceConfig);
373  XML_READ_CSTRING_ATTRIBUTE_OPTIONAL(ModelVersion, deviceConfig);
374  XML_READ_CSTRING_ATTRIBUTE_OPTIONAL(ModelSerialNumber, deviceConfig);
375  }
376 
377  vtkXMLDataElement* calibration = deviceConfig->FindNestedElementWithName("StepperCalibrationResult");
378  if (calibration != NULL)
379  {
380  const char* calibrationAlgorithmVersion = calibration->GetAttribute("AlgorithmVersion");
381  if (calibrationAlgorithmVersion != NULL)
382  {
383  this->SetCalibrationAlgorithmVersion(calibrationAlgorithmVersion);
384  }
385  else
386  {
387  LOG_WARNING("Failed to read stepper calibration algorithm version from config file!");
388  this->SetCalibrationAlgorithmVersion("Unknown");
389  }
390 
391  const char* calibrationDate = calibration->GetAttribute("Date");
392  if (calibrationDate != NULL)
393  {
394  this->SetCalibrationDate(calibrationDate);
395  }
396  else
397  {
398  LOG_WARNING("Failed to read stepper calibration date from config file!");
399  this->SetCalibrationDate("Unknown");
400  }
401 
402  XML_READ_VECTOR_ATTRIBUTE_OPTIONAL(double, 3, ProbeTranslationAxisOrientation, calibration);
403  XML_READ_VECTOR_ATTRIBUTE_OPTIONAL(double, 3, TemplateTranslationAxisOrientation, calibration);
404  XML_READ_VECTOR_ATTRIBUTE_OPTIONAL(double, 3, ProbeRotationAxisOrientation, calibration);
405  XML_READ_SCALAR_ATTRIBUTE_OPTIONAL(double, ProbeRotationEncoderScale, calibration);
406 
407  }
408 
409  return PLUS_SUCCESS;
410 }
411 
412 //----------------------------------------------------------------------------
413 PlusStatus vtkPlusBrachyTracker::WriteConfiguration(vtkXMLDataElement* rootConfigElement)
414 {
415  XML_FIND_DEVICE_ELEMENT_REQUIRED_FOR_WRITING(trackerConfig, rootConfigElement);
416 
417  if (this->Device != NULL)
418  {
420  std::string strStepperType = PlusBrachyStepper::GetBrachyStepperTypeInString(stepperType);
421  trackerConfig->SetAttribute("BrachyStepperType", strStepperType.c_str());
422  }
423 
424  trackerConfig->SetUnsignedLongAttribute("SerialPort", this->GetSerialPort());
425  trackerConfig->SetDoubleAttribute("BaudRate", this->GetBaudRate());
426  trackerConfig->SetAttribute("ModelVersion", this->GetModelVersion());
427  trackerConfig->SetAttribute("ModelNumber", this->GetModelNumber());
428  trackerConfig->SetAttribute("ModelSerialNumber", this->GetModelSerialNumber());
429 
430  // Save stepper calibration results to file
431  vtkXMLDataElement* calibration = trackerConfig->FindNestedElementWithName("StepperCalibrationResult");
432  if (calibration == NULL)
433  {
434  // create new element and add to trackerTool
435  vtkSmartPointer<vtkXMLDataElement> newCalibration = vtkSmartPointer<vtkXMLDataElement>::New();
436  newCalibration->SetName("StepperCalibrationResult");
437  newCalibration->SetParent(trackerConfig);
438  trackerConfig->AddNestedElement(newCalibration);
439  calibration = newCalibration;
440  }
441 
442  calibration->SetAttribute("Date", this->GetCalibrationDate());
443  calibration->SetAttribute("AlgorithmVersion", this->GetCalibrationAlgorithmVersion());
444  calibration->SetVectorAttribute("ProbeRotationAxisOrientation", 3, this->GetProbeRotationAxisOrientation());
445  calibration->SetDoubleAttribute("ProbeRotationEncoderScale", this->GetProbeRotationEncoderScale());
446  calibration->SetVectorAttribute("ProbeTranslationAxisOrientation", 3, this->GetProbeTranslationAxisOrientation());
447  calibration->SetVectorAttribute("TemplateTranslationAxisOrientation", 3, this->GetTemplateTranslationAxisOrientation());
448 
449  return PLUS_SUCCESS;
450 }
451 
452 
453 //----------------------------------------------------------------------------
455 {
456  return this->Device->ResetStepper();
457 }
458 
459 //----------------------------------------------------------------------------
461 {
462  return this->Device->InitializeStepper(calibMsg);
463 }
464 
465 //----------------------------------------------------------------------------
466 PlusStatus vtkPlusBrachyTracker::GetTrackedFrame(double timestamp, igsioTrackedFrame* aTrackedFrame)
467 {
468  if (!aTrackedFrame)
469  {
470  return PLUS_FAIL;
471  }
472 
473  // PROBEHOME_TO_PROBE_TRANSFORM
474  ToolStatus probehome2probeStatus = TOOL_OK;
475  vtkSmartPointer<vtkMatrix4x4> probehome2probeMatrix = vtkSmartPointer<vtkMatrix4x4>::New();
476  if (this->GetProbeHomeToProbeTransform(timestamp, probehome2probeMatrix, probehome2probeStatus) != PLUS_SUCCESS)
477  {
478  LOG_ERROR("Failed to get probe home to probe transform from buffer!");
479  return PLUS_FAIL;
480  }
481 
482  igsioTransformName probeToReferenceTransformName(this->GetBrachyToolSourceId(PROBEHOME_TO_PROBE_TRANSFORM), this->ToolReferenceFrameName);
483  if (!probeToReferenceTransformName.IsValid())
484  {
485  LOG_ERROR("Invalid probe to reference tranform name!");
486  return PLUS_FAIL;
487  }
488 
489  if (aTrackedFrame->SetFrameTransform(probeToReferenceTransformName, probehome2probeMatrix) != PLUS_SUCCESS)
490  {
491  LOG_ERROR("Failed to set transform for tool " << this->GetBrachyToolSourceId(PROBEHOME_TO_PROBE_TRANSFORM));
492  return PLUS_FAIL;
493  }
494 
495  // Convert
496  if (aTrackedFrame->SetFrameTransformStatus(probeToReferenceTransformName, probehome2probeStatus) != PLUS_SUCCESS)
497  {
498  LOG_ERROR("Failed to set transform status for tool " << this->GetBrachyToolSourceId(PROBEHOME_TO_PROBE_TRANSFORM));
499  return PLUS_FAIL;
500  }
501 
502 
503  // TEMPLATEHOME_TO_TEMPLATE_TRANSFORM
504  ToolStatus templhome2templStatus = TOOL_OK;
505  vtkSmartPointer<vtkMatrix4x4> templhome2templMatrix = vtkSmartPointer<vtkMatrix4x4>::New();
506  if (this->GetTemplateHomeToTemplateTransform(timestamp, templhome2templMatrix, templhome2templStatus) != PLUS_SUCCESS)
507  {
508  LOG_ERROR("Failed to get template home to template transform from buffer!");
509  return PLUS_FAIL;
510  }
511 
512  igsioTransformName templateToReferenceTransformName(this->GetBrachyToolSourceId(TEMPLATEHOME_TO_TEMPLATE_TRANSFORM), this->ToolReferenceFrameName);
513  if (!templateToReferenceTransformName.IsValid())
514  {
515  LOG_ERROR("Invalid template to reference tranform name!");
516  return PLUS_FAIL;
517  }
518 
519  if (aTrackedFrame->SetFrameTransform(templateToReferenceTransformName, templhome2templMatrix) != PLUS_SUCCESS)
520  {
521  LOG_ERROR("Failed to set transform for tool " << this->GetBrachyToolSourceId(TEMPLATEHOME_TO_TEMPLATE_TRANSFORM));
522  return PLUS_FAIL;
523  }
524 
525  if (aTrackedFrame->SetFrameTransformStatus(templateToReferenceTransformName, templhome2templStatus) != PLUS_SUCCESS)
526  {
527  LOG_ERROR("Failed to set transform status for tool " << this->GetBrachyToolSourceId(TEMPLATEHOME_TO_TEMPLATE_TRANSFORM));
528  return PLUS_FAIL;
529  }
530 
531 
532  // RAW_ENCODER_VALUES
533  ToolStatus rawEncoderValuesStatus = TOOL_OK;
534  vtkSmartPointer<vtkMatrix4x4> rawEncoderValuesMatrix = vtkSmartPointer<vtkMatrix4x4>::New();
535  if (this->GetRawEncoderValuesTransform(timestamp, rawEncoderValuesMatrix, rawEncoderValuesStatus) != PLUS_SUCCESS)
536  {
537  LOG_ERROR("Failed to get raw encoder values from buffer!");
538  return PLUS_FAIL;
539  }
540 
541  igsioTransformName encoderToReferenceTransformName(this->GetBrachyToolSourceId(RAW_ENCODER_VALUES), this->ToolReferenceFrameName);
542  if (!encoderToReferenceTransformName.IsValid())
543  {
544  LOG_ERROR("Invalid encoder to reference tranform name!");
545  return PLUS_FAIL;
546  }
547 
548  if (aTrackedFrame->SetFrameTransform(encoderToReferenceTransformName, rawEncoderValuesMatrix) != PLUS_SUCCESS)
549  {
550  LOG_ERROR("Failed to set transform for tool " << this->GetBrachyToolSourceId(RAW_ENCODER_VALUES));
551  return PLUS_FAIL;
552  }
553 
554  if (aTrackedFrame->SetFrameTransformStatus(encoderToReferenceTransformName, rawEncoderValuesStatus) != PLUS_SUCCESS)
555  {
556  LOG_ERROR("Failed to set transform status for tool " << this->GetBrachyToolSourceId(RAW_ENCODER_VALUES));
557  return PLUS_FAIL;
558  }
559 
560  // Get value for PROBE_POSITION, PROBE_ROTATION, TEMPLATE_POSITION tools
561  ToolStatus encoderStatus = TOOL_OK;
562  double probePos(0), probeRot(0), templatePos(0);
563  if (vtkPlusBrachyTracker::GetStepperEncoderValues(timestamp, probePos, probeRot, templatePos, encoderStatus) != PLUS_SUCCESS)
564  {
565  LOG_ERROR("Failed to get stepper encoder values!");
566  return PLUS_FAIL;
567  }
568 
569  // PROBE_POSITION
570  std::ostringstream strProbePos;
571  strProbePos << probePos;
572  aTrackedFrame->SetFrameField("ProbePosition", strProbePos.str());
573 
574  // PROBE_ROTATION
575  std::ostringstream strProbeRot;
576  strProbeRot << probeRot;
577  aTrackedFrame->SetFrameField("ProbeRotation", strProbeRot.str());
578 
579  // TEMPLATE_POSITION
580  std::ostringstream strTemplatePos;
581  strTemplatePos << templatePos;
582  aTrackedFrame->SetFrameField("TemplatePosition", strTemplatePos.str());
583 
584  return PLUS_SUCCESS;
585 }
586 
587 //----------------------------------------------------------------------------
588 PlusStatus vtkPlusBrachyTracker::GetLatestStepperEncoderValues(double& probePosition, double& probeRotation, double& templatePosition, ToolStatus& status)
589 {
590  std::string encoderToolSourceId = this->GetBrachyToolSourceId(RAW_ENCODER_VALUES);
591  if (encoderToolSourceId.empty())
592  {
593  LOG_ERROR("Failed to get encoder values tool name!");
594  return PLUS_FAIL;
595  }
596 
597  vtkPlusDataSource* encoderTool = NULL;
598  if (this->GetTool(encoderToolSourceId.c_str(), encoderTool) != PLUS_SUCCESS)
599  {
600  LOG_ERROR("Failed to get tool: " << encoderToolSourceId);
601  return PLUS_FAIL;
602  }
603 
604  if (encoderTool->GetNumberOfItems() < 1)
605  {
606  LOG_DEBUG("The buffer is empty"); // do not report as an error, it may be normal after a buffer clear
607  probePosition = 0.0;
608  probeRotation = 0.0;
609  templatePosition = 0.0;
610  status = TOOL_MISSING;
611  return PLUS_SUCCESS;
612  }
613  BufferItemUidType latestUid = encoderTool->GetLatestItemUidInBuffer();
614 
615  return vtkPlusBrachyTracker::GetStepperEncoderValues(latestUid, probePosition, probeRotation, templatePosition, status);
616 }
617 
618 //----------------------------------------------------------------------------
619 PlusStatus vtkPlusBrachyTracker::GetStepperEncoderValues(BufferItemUidType uid, double& probePosition, double& probeRotation, double& templatePosition, ToolStatus& status)
620 {
621  std::string encoderToolSourceId = this->GetBrachyToolSourceId(RAW_ENCODER_VALUES);
622  if (encoderToolSourceId.empty())
623  {
624  LOG_ERROR("Failed to get encoder values tool name!");
625  return PLUS_FAIL;
626  }
627 
628  vtkPlusDataSource* encoderTool = NULL;
629  if (this->GetTool(encoderToolSourceId.c_str(), encoderTool) != PLUS_SUCCESS)
630  {
631  LOG_ERROR("Failed to get tool: " << encoderToolSourceId);
632  return PLUS_FAIL;
633  }
634 
635  StreamBufferItem bufferItem;
636  if (encoderTool->GetStreamBufferItem(uid, &bufferItem) != ITEM_OK)
637  {
638  LOG_ERROR("Failed to get stepper encoder values from buffer by UID: " << uid);
639  return PLUS_FAIL;
640  }
641 
642  vtkSmartPointer<vtkMatrix4x4> mx = vtkSmartPointer<vtkMatrix4x4>::New();
643  if (bufferItem.GetMatrix(mx) != PLUS_SUCCESS)
644  {
645  LOG_ERROR("Failed to get bufferitem matrix by UID: " << uid);
646  return PLUS_FAIL;
647  }
648  probePosition = mx->GetElement(ROW_PROBE_POSITION, 3);
649  probeRotation = mx->GetElement(ROW_PROBE_ROTATION, 3);
650  templatePosition = mx->GetElement(ROW_TEMPLATE_POSITION, 3);
651  status = bufferItem.GetStatus();
652 
653  return PLUS_SUCCESS;
654 }
655 
656 
657 //----------------------------------------------------------------------------
658 PlusStatus vtkPlusBrachyTracker::GetStepperEncoderValues(double timestamp, double& probePosition, double& probeRotation, double& templatePosition, ToolStatus& status)
659 {
660  std::string encoderToolSourceId = this->GetBrachyToolSourceId(RAW_ENCODER_VALUES);
661  if (encoderToolSourceId.empty())
662  {
663  LOG_ERROR("Failed to get encoder values tool name!");
664  return PLUS_FAIL;
665  }
666 
667  vtkPlusDataSource* encoderTool = NULL;
668  if (this->GetTool(encoderToolSourceId.c_str(), encoderTool) != PLUS_SUCCESS)
669  {
670  LOG_ERROR("Failed to get tool: " << encoderToolSourceId);
671  return PLUS_FAIL;
672  }
673 
674  BufferItemUidType uid(0);
675  if (encoderTool->GetItemUidFromTime(timestamp, uid) != ITEM_OK)
676  {
677  LOG_ERROR("Failed to get stepper encoder values from buffer by time: " << std::fixed << timestamp);
678  return PLUS_FAIL;
679  }
680 
681  return vtkPlusBrachyTracker::GetStepperEncoderValues(uid, probePosition, probeRotation, templatePosition, status);
682 }
683 
684 //----------------------------------------------------------------------------
685 PlusStatus vtkPlusBrachyTracker::GetProbeHomeToProbeTransform(BufferItemUidType uid, vtkMatrix4x4* probeHomeToProbeMatrix, ToolStatus& status)
686 {
687  if (probeHomeToProbeMatrix == NULL)
688  {
689  LOG_ERROR("Failed to get probe home to probe transform - input transform is NULL!");
690  return PLUS_FAIL;
691  }
692 
693  std::string probeToolSourceId = this->GetBrachyToolSourceId(PROBEHOME_TO_PROBE_TRANSFORM);
694  if (probeToolSourceId.empty())
695  {
696  LOG_ERROR("Failed to get probe tool name!");
697  return PLUS_FAIL;
698  }
699 
700  vtkPlusDataSource* probeTool = NULL;
701  if (this->GetTool(probeToolSourceId.c_str(), probeTool) != PLUS_SUCCESS)
702  {
703  LOG_ERROR("Failed to get tool: " << probeToolSourceId);
704  return PLUS_FAIL;
705  }
706 
707  StreamBufferItem bufferItem;
708  if (probeTool->GetStreamBufferItem(uid, &bufferItem) != ITEM_OK)
709  {
710  LOG_ERROR("Failed to get probe home to probe transform by UID: " << uid);
711  return PLUS_FAIL;
712  }
713 
714  status = bufferItem.GetStatus();
715  if (bufferItem.GetMatrix(probeHomeToProbeMatrix) != PLUS_SUCCESS)
716  {
717  LOG_ERROR("Failed to get probeHomeToProbeMatrix");
718  return PLUS_FAIL;
719  }
720 
721  return PLUS_SUCCESS;
722 }
723 
724 //----------------------------------------------------------------------------
725 PlusStatus vtkPlusBrachyTracker::GetProbeHomeToProbeTransform(double timestamp, vtkMatrix4x4* probeHomeToProbeMatrix, ToolStatus& status)
726 {
727  std::string probeToolSourceId = this->GetBrachyToolSourceId(PROBEHOME_TO_PROBE_TRANSFORM);
728  if (probeToolSourceId.empty())
729  {
730  LOG_ERROR("Failed to get probe tool name!");
731  return PLUS_FAIL;
732  }
733 
734  vtkPlusDataSource* probeTool = NULL;
735  if (this->GetTool(probeToolSourceId.c_str(), probeTool) != PLUS_SUCCESS)
736  {
737  LOG_ERROR("Failed to get tool: " << probeToolSourceId);
738  return PLUS_FAIL;
739  }
740 
741  BufferItemUidType uid(0);
742  if (probeTool->GetItemUidFromTime(timestamp, uid) != ITEM_OK)
743  {
744  LOG_ERROR("Failed to get probe home to probe transform by timestamp: " << std::fixed << timestamp);
745  PLUS_FAIL;
746  }
747 
748  return this->GetProbeHomeToProbeTransform(uid, probeHomeToProbeMatrix, status);
749 }
750 
751 //----------------------------------------------------------------------------
752 PlusStatus vtkPlusBrachyTracker::GetTemplateHomeToTemplateTransform(BufferItemUidType uid, vtkMatrix4x4* templateHomeToTemplateMatrix, ToolStatus& status)
753 {
754  if (templateHomeToTemplateMatrix == NULL)
755  {
756  LOG_ERROR("Failed to get template home to template transform - input transform is NULL!");
757  return PLUS_FAIL;
758  }
759 
760  std::string templateToolSourceId = this->GetBrachyToolSourceId(TEMPLATEHOME_TO_TEMPLATE_TRANSFORM);
761  if (templateToolSourceId.empty())
762  {
763  LOG_ERROR("Failed to get template tool name!");
764  return PLUS_FAIL;
765  }
766 
767  vtkPlusDataSource* templateTool = NULL;
768  if (this->GetTool(templateToolSourceId.c_str(), templateTool) != PLUS_SUCCESS)
769  {
770  LOG_ERROR("Failed to get tool: " << templateToolSourceId);
771  return PLUS_FAIL;
772  }
773 
774  StreamBufferItem bufferItem;
775  if (templateTool->GetStreamBufferItem(uid, &bufferItem) != ITEM_OK)
776  {
777  LOG_ERROR("Failed to get template home to template transform by UID: " << uid);
778  return PLUS_FAIL;
779  }
780 
781  status = bufferItem.GetStatus();
782  if (bufferItem.GetMatrix(templateHomeToTemplateMatrix) != PLUS_SUCCESS)
783  {
784  LOG_ERROR("Failed to get templateHomeToTemplateMatrix");
785  return PLUS_FAIL;
786  }
787 
788  return PLUS_SUCCESS;
789 }
790 
791 //----------------------------------------------------------------------------
792 PlusStatus vtkPlusBrachyTracker::GetTemplateHomeToTemplateTransform(double timestamp, vtkMatrix4x4* templateHomeToTemplateMatrix, ToolStatus& status)
793 {
794  std::string templateToolSourceId = this->GetBrachyToolSourceId(TEMPLATEHOME_TO_TEMPLATE_TRANSFORM);
795  if (templateToolSourceId.empty())
796  {
797  LOG_ERROR("Failed to get template tool name!");
798  return PLUS_FAIL;
799  }
800 
801  vtkPlusDataSource* templateTool = NULL;
802  if (this->GetTool(templateToolSourceId.c_str(), templateTool) != PLUS_SUCCESS)
803  {
804  LOG_ERROR("Failed to get tool: " << templateToolSourceId);
805  return PLUS_FAIL;
806  }
807 
808  BufferItemUidType uid(0);
809  if (templateTool->GetItemUidFromTime(timestamp, uid) != ITEM_OK)
810  {
811  LOG_ERROR("Failed to get template home to template transform by timestamp: " << std::fixed << timestamp);
812  return PLUS_FAIL;
813  }
814 
815  return this->GetTemplateHomeToTemplateTransform(uid, templateHomeToTemplateMatrix, status);
816 }
817 
818 //----------------------------------------------------------------------------
819 PlusStatus vtkPlusBrachyTracker::GetRawEncoderValuesTransform(BufferItemUidType uid, vtkMatrix4x4* rawEncoderValuesTransform, ToolStatus& status)
820 {
821  if (rawEncoderValuesTransform == NULL)
822  {
823  LOG_ERROR("Failed to get raw encoder values transform from buffer - input transform NULL!");
824  return PLUS_FAIL;
825  }
826 
827  std::string encoderToolSourceId = this->GetBrachyToolSourceId(RAW_ENCODER_VALUES);
828  if (encoderToolSourceId.empty())
829  {
830  LOG_ERROR("Failed to get encoder values tool name!");
831  return PLUS_FAIL;
832  }
833 
834  vtkPlusDataSource* encoderTool = NULL;
835  if (this->GetTool(encoderToolSourceId.c_str(), encoderTool) != PLUS_SUCCESS)
836  {
837  LOG_ERROR("Failed to get tool: " << encoderToolSourceId);
838  return PLUS_FAIL;
839  }
840 
841  StreamBufferItem bufferItem;
842  if (encoderTool->GetStreamBufferItem(uid, &bufferItem) != ITEM_OK)
843  {
844  LOG_ERROR("Failed to get raw encoder values transform from buffer by UID: " << uid);
845  return PLUS_FAIL;
846  }
847 
848  if (bufferItem.GetMatrix(rawEncoderValuesTransform) != PLUS_SUCCESS)
849  {
850  LOG_ERROR("Failed to get rawEncoderValuesTransform");
851  return PLUS_FAIL;
852  }
853 
854  status = bufferItem.GetStatus();
855 
856  return PLUS_SUCCESS;
857 }
858 
859 //----------------------------------------------------------------------------
860 PlusStatus vtkPlusBrachyTracker::GetRawEncoderValuesTransform(double timestamp, vtkMatrix4x4* rawEncoderValuesTransform, ToolStatus& status)
861 {
862  std::string encoderToolSourceId = this->GetBrachyToolSourceId(RAW_ENCODER_VALUES);
863  if (encoderToolSourceId.empty())
864  {
865  LOG_ERROR("Failed to get encoder values tool name!");
866  return PLUS_FAIL;
867  }
868 
869  vtkPlusDataSource* encoderTool = NULL;
870  if (this->GetTool(encoderToolSourceId.c_str(), encoderTool) != PLUS_SUCCESS)
871  {
872  LOG_ERROR("Failed to get tool: " << encoderToolSourceId);
873  return PLUS_FAIL;
874  }
875 
876  BufferItemUidType uid(0);
877  if (encoderTool->GetItemUidFromTime(timestamp, uid) != ITEM_OK)
878  {
879  LOG_ERROR("Failed to get raw encoder values transform by timestamp: " << std::fixed << timestamp);
880  return PLUS_FAIL;
881  }
882 
883  return this->GetRawEncoderValuesTransform(uid, rawEncoderValuesTransform, status);
884 }
885 
886 //----------------------------------------------------------------------------
888 {
889  if (this->OutputChannels.size() > 1)
890  {
891  LOG_WARNING("vtkPlusBrachyTracker is expecting one output channel and there are " << this->OutputChannels.size() << " channels. First output channel will be used.");
892  return PLUS_FAIL;
893  }
894 
895  if (this->OutputChannels.empty())
896  {
897  LOG_ERROR("No output channels defined for vtkPlusBrachyTracker. Cannot proceed.");
898  this->SetCorrectlyConfigured(false);
899  return PLUS_FAIL;
900  }
901 
902  vtkPlusChannel* outputChannel = this->OutputChannels[0];
903 
904  outputChannel->Clear();
905  for (DataSourceContainerIterator it = this->Tools.begin(); it != this->Tools.end(); ++it)
906  {
907  outputChannel->AddTool(it->second);
908  }
909 
910  return PLUS_SUCCESS;
911 }
virtual BufferItemUidType GetLatestItemUidInBuffer()
PlusStatus GetLatestStepperEncoderValues(double &probePosition, double &probeRotation, double &templatePosition, ToolStatus &status)
std::string ToolReferenceFrameName
virtual void PrintSelf(ostream &os, vtkIndent indent) VTK_OVERRIDE
ToolStatus GetStatus() const
PlusStatus InitializeStepper(std::string &calibMsg)
virtual PlusStatus WriteConfiguration(vtkXMLDataElement *config)
PlusBrachyStepper::BRACHY_STEPPER_TYPE BrachyStepperType
double * timestamp
Definition: phidget22.h:3432
virtual char * GetCalibrationDate()
virtual char * GetModelSerialNumber()
virtual ItemStatus GetStreamBufferItem(BufferItemUidType uid, StreamBufferItem *bufferItem)
#define XML_FIND_DEVICE_ELEMENT_REQUIRED_FOR_WRITING(deviceConfig, rootConfigElement)
virtual void SetProbeRotationEncoderScale(double)
virtual void CompensationEnabledOn()
virtual PlusStatus InternalDisconnect()
virtual unsigned long GetBaudRate()
vtkStandardNewMacro(vtkPlusBrachyTracker)
igsioStatus PlusStatus
Definition: PlusCommon.h:40
PlusStatus AddTool(vtkPlusDataSource *tool, bool requireUniquePortName=true)
virtual PlusStatus ToolTimeStampedUpdate(const std::string &aToolSourceId, vtkMatrix4x4 *matrix, ToolStatus status, unsigned long frameNumber, double unfilteredtimestamp, const igsioFieldMapType *customFields=NULL)
bool RequirePortNameInDeviceSetConfiguration
virtual PlusStatus Clear()
virtual double GetProbeRotationEncoderScale()
virtual char * GetModelVersion()
BRACHY_STEPPER_TYPE GetBrachyStepperType() const
double AcquisitionRate
PlusStatus AddTool(vtkPlusDataSource *aTool)
#define PLUS_FAIL
Definition: PlusCommon.h:43
DataSourceContainer Tools
virtual PlusStatus NotifyConfigured()
virtual void PrintSelf(ostream &os, vtkIndent indent) VTK_OVERRIDE
virtual void SetProbeRotationAxisOrientation(double, double, double)
PlusStatus GetTemplateHomeToTemplateTransform(BufferItemUidType uid, vtkMatrix4x4 *templateHomeToTemplateMatrix, ToolStatus &status)
double TemplateTranslationAxisOrientation[3]
virtual PlusStatus Disconnect()
virtual void SetCorrectlyConfigured(bool)
static std::string GetBrachyStepperTypeInString(BRACHY_STEPPER_TYPE stepperType)
virtual void SetCalibrationAlgorithmVersion(const char *)
#define PLUS_SUCCESS
Definition: PlusCommon.h:44
Interface to brachytherapy steppers.
PlusStatus GetToolByPortName(const char *aPortName, vtkPlusDataSource *&aSource)
virtual PlusStatus Connect()
virtual void SetModelNumber(const char *)
virtual PlusStatus StopRecording()
PlusStatus GetMatrix(vtkMatrix4x4 *outputMatrix)
PlusStatus GetStepperEncoderValues(BufferItemUidType uid, double &probePosition, double &probeRotation, double &templatePosition, ToolStatus &status)
std::string GetBrachyToolSourceId(BRACHY_STEPPER_TOOL tool)
#define XML_FIND_DEVICE_ELEMENT_REQUIRED_FOR_READING(deviceConfig, rootConfigElement)
virtual void SetTemplateTranslationAxisOrientation(double, double, double)
DataSourceContainer::iterator DataSourceContainerIterator
virtual PlusStatus ResetStepper()=0
virtual PlusStatus GetTrackedFrame(double timestamp, igsioTrackedFrame *aTrackedFrame)
double ProbeTranslationAxisOrientation[3]
virtual bool IsRecording() const
PlusStatus GetProbeHomeToProbeTransform(BufferItemUidType uid, vtkMatrix4x4 *probeHomeToProbeMatrix, ToolStatus &status)
virtual char * GetCalibrationAlgorithmVersion()
bool StartThreadForInternalUpdates
virtual PlusStatus Disconnect()=0
virtual char * GetModelNumber()
virtual double * GetProbeRotationAxisOrientation()
virtual PlusStatus GetDeviceModelInfo(std::string &version, std::string &model, std::string &serial)=0
virtual double * GetProbeTranslationAxisOrientation()
Contains an optional timestamped circular buffer containing the video images and a number of timestam...
virtual unsigned long GetSerialPort()
virtual PlusStatus InitializeStepper(std::string &CalibMsg)=0
ChannelContainer OutputChannels
virtual void SetCalibrationDate(const char *)
virtual void SetModelVersion(const char *)
virtual double * GetTemplateTranslationAxisOrientation()
virtual ItemStatus GetItemUidFromTime(double time, BufferItemUidType &uid)
void SetToolReferenceFrameName(const std::string &frameName)
PlusBrachyStepper * Device
virtual PlusStatus Connect()=0
PlusStatus GetTool(const char *aToolSourceId, vtkPlusDataSource *&aTool) const
unsigned long long BufferItemUidType
virtual void SetModelSerialNumber(const char *)
virtual PlusStatus ReadConfiguration(vtkXMLDataElement *config)
virtual void SetSerialPort(unsigned long)
virtual void SetProbeTranslationAxisOrientation(double, double, double)
void SetBrachyStepperType(BRACHY_STEPPER_TYPE type)
virtual int GetNumberOfItems()
PlusStatus GetRawEncoderValuesTransform(BufferItemUidType uid, vtkMatrix4x4 *rawEncoderValuesTransform, ToolStatus &status)
virtual PlusStatus GetEncoderValues(double &PPosition, double &GPosition, double &RPosition, unsigned long &PositionRequestNumber)=0
Interface to a 3D positioning tool, video source, or generalized data stream.