CustusX  18.04-rc1
An IGT application
cxOpenIGTLinkTool.cpp
Go to the documentation of this file.
1 /*=========================================================================
2 This file is part of CustusX, an Image Guided Therapy Application.
3 
4 Copyright (c) SINTEF Department of Medical Technology.
5 All rights reserved.
6 
7 CustusX is released under a BSD 3-Clause license.
8 
9 See Lisence.txt (https://github.com/SINTEFMedtek/CustusX/blob/master/License.txt) for details.
10 =========================================================================*/
11 
12 #include "cxOpenIGTLinkTool.h"
13 
14 #include <QDateTime>
15 
17 #include "cxLogger.h"
18 #include "cxProbeImpl.h"
19 
20 namespace cx
21 {
23  ToolImpl(toolFileToolStructure->mUid, toolFileToolStructure->mUid),
24  mTimestamp(0),
25  mVisible(false),
26  mLastReceivedPositionTime(0),
27  mConfigFileToolStructure(configFileToolStructure),
28  mToolFileToolStructure(toolFileToolStructure),
29  mPrintedWarningAboutTimeStampMismatch(false)
30 
31 {
32 // CX_LOG_DEBUG() << "OpenIGTLinkTool constr mInstrumentId: " << mToolFileToolStructure.mInstrumentId << " mInstrumentScannerId: " << mToolFileToolStructure.mInstrumentScannerId;
33 // CX_LOG_DEBUG() << "OpenIGTLinkTool constr mOpenIGTLinkTransformId: " << mConfigFileToolStructure.mOpenIGTLinkTransformId << " mOpenIGTLinkImageId: " << mConfigFileToolStructure.mOpenIGTLinkImageId;
34  connect(&mTpsTimer, &QTimer::timeout, this, &OpenIGTLinkTool::calculateTpsSlot);
35  connect(&mTpsTimer, &QTimer::timeout, this, &OpenIGTLinkTool::calculateVisible);//Use tps timer to calculate visibility
36 
37  if(toolFileToolStructure->mIsProbe)
38  {
39 // CX_LOG_DEBUG() << "OpenIGTLinkTool is probe mInstrumentId: " << mToolFileToolStructure.mInstrumentId << " mInstrumentScannerId: " << mToolFileToolStructure.mInstrumentScannerId;
40 // CX_LOG_DEBUG() << "OpenIGTLinkTool is probe";
41 // mProbe = ProbeImpl::New(mConfigFileToolStructure.mOpenIGTLinkTransformId, mConfigFileToolStructure.mOpenIGTLinkImageId);
42 // mProbe = ProbeImpl::New(mToolFileToolStructure.mInstrumentId, mToolFileToolStructure.mInstrumentScannerId);
43  // See ProbeCalibsConfigs.xml
44  // PlusDeviceSet_OpenIGTLinkCommandsTest - needs to be the same as <USScanner><Name>
45  // ProbeToReference - needs to be the same as <USProbe><Name>
46  mProbe = ProbeImpl::New("ProbeToReference", "PlusDeviceSet_OpenIGTLinkCommandsTest");
47  connect(mProbe.get(), SIGNAL(sectorChanged()), this, SIGNAL(toolProbeSector()));
48  }
49 
50  this->createToolGraphic();
51  this->toolVisibleSlot(true);
52 }
53 
54 bool OpenIGTLinkTool::doIdCorrespondToTool(QString openIGTLinkId)
55 {
56  bool retval = false;
57  if(openIGTLinkId.compare(this->mConfigFileToolStructure.mOpenIGTLinkTransformId, Qt::CaseInsensitive) == 0)
58  retval = true;
59  else if(openIGTLinkId.compare(this->mConfigFileToolStructure.mOpenIGTLinkImageId, Qt::CaseInsensitive) == 0)
60  retval = true;
61  return retval;
62 }
63 
65 {
66 }
67 
69 {
70  return mProbe;
71 }
72 
74 {
75  return mTimestamp;
76 }
77 
79 {
80  return mVisible;
81 }
82 
84 {
85  //TODO when is a tool initialized? when it is connected to the tracker?
86  return true;
87 }
88 
89 QString OpenIGTLinkTool::getUid() const
90 {
91  return Tool::mUid;
92 }
93 
94 QString OpenIGTLinkTool::getName() const
95 {
96  return Tool::mName;
97 }
98 
100 {
101  if(this->getProbe())
102  return this->getProbe()->getProbeDefinition().getDepthStart();
104 }
105 
107 {
108  if(this->getProbe())
109  return;
111 }
112 
114 {
115  Transform3D identity = Transform3D::Identity();
116 
117  Transform3D sMt = this->getCalibration_sMt();
118  bool calibrated = !similar(sMt, identity);
119  CX_LOG_DEBUG() << "Checking if openiglink tool is calibratated: " << calibrated;
120  return calibrated;
121 }
122 
124 {
125  return mToolFileToolStructure->getCalibrationAsSSC();
126 }
127 
129 {
130  CX_LOG_INFO() << mName << " got an updated calibration";
131  CX_LOG_WARNING() << "OpenIGTLinkTool::setCalibration_sMt() Receiving calibration. Should file be updated, or should it be discarded and use calibration from file instead?";
132  CX_LOG_WARNING() << "Current implementation discards this received calibration. sMt: " << sMt;
133 // mToolFileToolStructure.mCalibration = sMt;
134  //write to file
135 // mInternalStructure.saveCalibrationToFile();
136 }
137 
138 void OpenIGTLinkTool::toolTransformAndTimestampSlot(Transform3D prMs, double timestamp)
139 {
140  // TODO: Fix use of OpenIGTLink timestamp. Task CX-334
141  // OpenIGTLink timestamp should be in seconds: https://github.com/openigtlink/OpenIGTLink/blob/master/Documents/Protocol/timestamp.md
142  // The below line should be (this needs to be tested/verified):
143  //mTimestamp = timestamp / 1000
144  mTimestamp = timestamp * 1000;
145  this->checkTimestampMismatch();
146 
147  //TODO: Make sure this is the way we want to handle this
148  //Current implementation is to get transforms between tool frame and ref frame
149  //Another solution is to get all transforms between tool frame and tracking system.
150 
151  //Reference is getting transform from reference tool to tracking system.
152  //Only use received transform to verify that reference tool is visible.
153  if(isReference())
154  return;
155 
156  Transform3D prMt = prMs * this->getCalibration_sMt();
157  Transform3D prMt_filtered = prMt;
158 
160  {
161  mTrackingPositionFilter->addPosition(prMt, mTimestamp);
162  prMt_filtered = mTrackingPositionFilter->getFilteredPosition();
163  }
164 
165  (*mPositionHistory)[mTimestamp] = prMt; // store original in history
166  m_prMt = prMt_filtered;
167  emit toolTransformAndTimestamp(m_prMt, mTimestamp);
168 }
169 
170 void OpenIGTLinkTool::checkTimestampMismatch()
171 {
172  mLastReceivedPositionTime = QDateTime::currentDateTime().toMSecsSinceEpoch();
173  double diff = mLastReceivedPositionTime - mTimestamp;
174 
175  //Use system time if time difference is more than a second
176  if(fabs(diff) > 1000)
177  {
178  this->printWarningAboutTimestampMismatch(diff);
179  mTimestamp = mLastReceivedPositionTime;
180  }
181 }
182 
183 void OpenIGTLinkTool::printWarningAboutTimestampMismatch(double diff)
184 {
185  if (mPrintedWarningAboutTimeStampMismatch) // Only print warning once to avoid spamming output
186  return;
187 
188  CX_LOG_WARNING() << "Difference between system time and received tool timestamp: " << diff
189  << " The reason for this may be incompatible timestamps. "
190  << " System time will be used instead of received timestamp.";
191  mPrintedWarningAboutTimeStampMismatch = true;
192 }
193 
194 void OpenIGTLinkTool::calculateTpsSlot()
195 {
196  int tpsNr = 0;
197  size_t numberOfTransformsToCheck = ((mPositionHistory->size() >= 10) ? 10 : mPositionHistory->size());
198  if (numberOfTransformsToCheck <= 1)
199  {
200  emit tps(0);
201  return;
202  }
203 
204  TimedTransformMap::reverse_iterator rit = mPositionHistory->rbegin();
205  double lastTransform = rit->first;
206  for (size_t i = 0; i < numberOfTransformsToCheck-1; ++i)
207  {
208  ++rit;
209  }
210  double firstTransform = rit->first;
211  double secondsPassed = (lastTransform - firstTransform) / 1000;
212 
213  if (!similar(secondsPassed, 0))
214  tpsNr = int(numberOfTransformsToCheck / secondsPassed);
215  emit tps(tpsNr);
216 }
217 
218 void OpenIGTLinkTool::calculateVisible()
219 {
220  //Compare only timestamps from this computer, and not the received timestamps
221  //(as this computer man not be in sync with the one creating the messages)
222  qint64 currentTime = QDateTime::currentDateTime().toMSecsSinceEpoch();
223  double diff = currentTime - mLastReceivedPositionTime;
224 // CX_LOG_DEBUG() << "diff: " << diff;
225  if(diff > 500)//Set visible to false if last position is more than 500 ms old
226  setVisible(false);
227  else
228  setVisible(true);
229 }
230 
231 void OpenIGTLinkTool::toolVisibleSlot(bool on)
232 {
233  if (on)
234  mTpsTimer.start(1000); //calculate tps every 1 seconds
235  else
236  mTpsTimer.stop();
237 }
238 
240 {
241  mVisible = vis;
242  emit toolVisible(vis);
243 }
244 
246 {
247  return mToolFileToolStructure;
248 }
249 
251 {
252  return getToolFileToolStructure()->mIsReference;
253 }
254 
255 
256 }//namespace cx
TimedTransformMapPtr mPositionHistory
Definition: cxToolImpl.h:59
boost::shared_ptr< ToolInternalStructure > ToolInternalStructurePtr
TrackingPositionFilterPtr mTrackingPositionFilter
Definition: cxToolImpl.h:61
Transform3D Transform3D
Transform3D is a representation of an affine 3D transform.
Common functionality for Tool subclasses.
Definition: cxToolImpl.h:30
#define CX_LOG_INFO
Definition: cxLogger.h:96
void toolProbeSector()
virtual double getTooltipOffset() const
get a virtual offset extending from the tool tip.
Definition: cxToolImpl.cpp:65
void toolTransformAndTimestamp(Transform3D matrix, double timestamp)
QString mUid
Definition: cxTool.h:147
boost::shared_ptr< Probe > ProbePtr
Definition: cxProbe.h:72
void toolVisible(bool visible)
QString mName
Definition: cxTool.h:148
void tps(int)
#define CX_LOG_DEBUG
Definition: cxLogger.h:95
static ProbeImplPtr New(QString instrumentUid, QString scannerUid, ProbeXmlConfigParserPtr xml=ProbeXmlConfigParserPtr())
Definition: cxProbeImpl.cpp:28
bool similar(const CameraInfo &lhs, const CameraInfo &rhs, double tol)
#define CX_LOG_WARNING
Definition: cxLogger.h:98
virtual void setTooltipOffset(double val)
set a virtual offset extending from the tool tip.
Definition: cxToolImpl.cpp:70
void createToolGraphic()
Definition: cxToolImpl.cpp:117
Transform3D m_prMt
the transform from the tool to the patient reference
Definition: cxToolImpl.h:60
Namespace for all CustusX production code.