CustusX  2020.11.07-dev+develop.4ce4a
An IGT application
cxIgstkToolManager.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 "cxIgstkToolManager.h"
13 
14 #include "cxLogger.h"
15 #include "cxTypeConversions.h"
16 
17 namespace cx
18 {
19 
20 void sampleInfo2xml(const igstk::NDITracker::TrackingSampleInfo& info, QDomElement& node)
21 {
22  node.setAttribute("timestamp", QString("%1").arg(info.m_TimeStamp, 0, 'f', 0));
23  node.setAttribute("error", QString("%1").arg(info.m_Error, 0, 'f', 3));
24  node.setAttribute("frame", QString("%1").arg(info.m_FrameNumber));
25  node.setAttribute("portstatus", QString("0b%1").arg(info.m_PortStatus, 16, 2, QChar('0')));
26  node.setAttribute("toolinformation", QString("0b%1").arg(info.m_ToolInformation, 8, 2, QChar('0')));
27  QString markers;
28  for (unsigned i=0; i<info.m_MarkerInformation.size(); ++i)
29  markers += QString::number(info.m_MarkerInformation[i]);
30  node.setAttribute("markers", markers);
31 }
32 
33 
35  std::vector<ToolFileParser::ToolInternalStructurePtr> toolStructures,
36  ToolFileParser::ToolInternalStructurePtr referenceToolStructure) :
37  mInitAnsweres(0), mInternalInitialized(false)
38 {
39  mTimer = 0;
40 
41  this->createTracker(trackerStructure);
42  this->createTools(toolStructures, referenceToolStructure);
43  this->setReferenceAndTrackerOnTools();
44 
45  connect(mTracker.get(), SIGNAL(tracking(bool)), this, SIGNAL(tracking(bool)));
46  connect(mTracker.get(), SIGNAL(error()), this, SIGNAL(error()));
47 
48  connect(mTracker.get(), SIGNAL(initialized(bool)), this, SLOT(deviceInitializedSlot(bool)));
49  connect(mTracker.get(), SIGNAL(tracking(bool)), this, SLOT(trackerTrackingSlot(bool)));
50 
51  mTimer = new QTimer();
52  connect(mTimer, SIGNAL(timeout()), this, SLOT(checkTimeoutsAndRequestTransformSlot()));
53 
54  igstk::RealTimeClock::Initialize();
55 }
56 
58 {
59  this->trackSlot(false);
60  this->initializeSlot(false);
61 }
62 
63 std::map<QString, IgstkToolPtr> IgstkToolManager::getTools()
64 {
65  QMutexLocker sentry(&mToolMutex);
66  return mTools;
67 }
68 
70 {
71  QMutexLocker sentry(&mReferenceMutex);
72  return mReferenceTool;
73 }
74 
75 void IgstkToolManager::setReferenceAndTrackerOnTools()
76 {
77  if (!mReferenceTool)
78  {
79  reportWarning("Tracking is configured without a reference tool.");
80  }
81 
82  std::map<QString, IgstkToolPtr>::iterator it;
83  for (it = mTools.begin(); it != mTools.end(); ++it)
84  {
85  if (mReferenceTool)
86  it->second->setReference(mReferenceTool);
87  if (mTracker)
88  it->second->setTracker(mTracker);
89  }
90 }
91 
92 void IgstkToolManager::createTracker(ToolFileParser::TrackerInternalStructure trackerStructure)
93 {
94  TrackerPtr tracker(new IgstkTracker(trackerStructure));
95  if (tracker->isValid())
96  mTracker = tracker;
97  else
98  reportWarning("Invalid tracker.");
99 }
100 
101 void IgstkToolManager::createTools(std::vector<ToolFileParser::ToolInternalStructurePtr> toolStructures,
102  ToolFileParser::ToolInternalStructurePtr referenceToolStructure)
103 {
104  for (unsigned i = 0; i < toolStructures.size(); ++i)
105  {
106  this->addIgstkTools(toolStructures[i]);
107  }
108  if (referenceToolStructure && !referenceToolStructure->mUid.isEmpty())
109  {
110  IgstkToolPtr refTool = this->addIgstkTools(referenceToolStructure);
111  if (refTool->isValid())
112  mReferenceTool = refTool;
113  }
114 }
115 
116 IgstkToolPtr IgstkToolManager::addIgstkTools(ToolFileParser::ToolInternalStructurePtr toolStructure)
117 {
118  IgstkToolPtr igstkTool(new IgstkTool(toolStructure));
119  if (igstkTool->isValid())
120  {
121  QMutexLocker sentry(&mToolMutex);
122  mTools[igstkTool->getUid()] = igstkTool;
123  connect(igstkTool.get(), SIGNAL(attachedToTracker(bool)), this, SLOT(deviceInitializedSlot(bool)));
124  }
125  else
126  {
127  reportWarning(toolStructure->mUid + " is not valid.");
128  }
129  return igstkTool;
130 }
131 
132 void IgstkToolManager::trackerTrackingSlot(bool isTracking)
133 {
134  int igstkPulsingDriveRate = 10;
135  if (isTracking)
136  mTimer->start(igstkPulsingDriveRate);
137  else
138  mTimer->stop();
139 }
140 
142 {
143  if (on)
144  {
145  connect(mTracker.get(), SIGNAL(initialized(bool)), this, SLOT(attachToolsWhenTrackerIsInitializedSlot(bool)));
146  if (!mTracker->isOpen())
147  mTracker->open();
148  }
149  else
150  {
151  mTracker->detachTools(mTools); //not sure we have to detach all tools before we close, read NDI manual
152  if (mTracker->isOpen())
153  mTracker->close();
154  }
155 }
156 
158 {
159  if (on && !mTracker->isTracking())
160  mTracker->startTracking();
161  else if (!on && mTracker->isTracking())
162  mTracker->stopTracking();
163 }
164 
165 void IgstkToolManager::checkTimeoutsAndRequestTransformSlot()
166 {
167  igstk::PulseGenerator::CheckTimeouts();
168 
169  std::map<QString, IgstkToolPtr>::iterator it = mTools.begin();
170  for (; it != mTools.end(); ++it)
171  {
172  if (!it->second)
173  continue;
174 
175  if (mReferenceTool)
176  it->second->getPointer()->RequestComputeTransformTo(mReferenceTool->getPointer());
177  else
178  it->second->getPointer()->RequestComputeTransformTo(mTracker->getPointer());
179  }
180 }
181 
182 void IgstkToolManager::deviceInitializedSlot(bool deviceInit)
183 {
184  size_t numberOfDevices = mTools.size() + 1; //+1 is the tracker
185 
186  if (deviceInit)
187  {
188  mInitAnsweres++;
189 
190  if (mInitAnsweres == numberOfDevices)
191  {
192  mInternalInitialized = true;
193  emit initialized(true);
194  }
195  }
196  else
197  {
198  mInitAnsweres--;
199 
200  if (mInitAnsweres < numberOfDevices)
201  {
202  if (mInternalInitialized)
203  {
204  mInitAnsweres = 0;
205  mInternalInitialized = false;
206  emit initialized(false);
207  }
208  }
209  }
210 }
211 
212 void IgstkToolManager::attachToolsWhenTrackerIsInitializedSlot(bool open)
213 {
214  if (!open)
215  return;
216 
217  disconnect(mTracker.get(), SIGNAL(initialized(bool)), this, SLOT(attachToolsWhenTrackerIsInitializedSlot(bool)));
218  mTracker->attachTools(mTools);
219 }
220 
221 void IgstkToolManager::printStatus()
222 {
223  std::cout << "mInternalInitialized " << mInternalInitialized << std::endl;
224  std::cout << "mInitAnsweres " << mInitAnsweres << std::endl;
225  std::cout << "mTracker->isValid() " << mTracker->isValid() << std::endl;
226  std::cout << "mTracker->isOpen() " << mTracker->isOpen() << std::endl;
227  std::cout << "mTracker->isInitialized() " << mTracker->isInitialized() << std::endl;
228  std::cout << "mTracker->isTracking() " << mTracker->isTracking() << std::endl;
229  std::cout << "mTools.size() " << string_cast(mTools.size()) << std::endl;
230 }
231 }
std::map< QString, IgstkToolPtr > getTools()
ThreadSafe.
boost::shared_ptr< ToolInternalStructure > ToolInternalStructurePtr
Class representing the navigation system.
boost::shared_ptr< class IgstkTool > IgstkToolPtr
void trackSlot(bool on)
tracking on or off
std::string string_cast(const T &val)
void tracking(bool on)
void initializeSlot(bool on)
connects to the hardware
IgstkToolManager(ToolFileParser::TrackerInternalStructure trackerStructure, std::vector< ToolFileParser::ToolInternalStructurePtr > toolStructures, ToolFileParser::ToolInternalStructurePtr referenceToolStructure)
void reportWarning(QString msg)
Definition: cxLogger.cpp:70
void sampleInfo2xml(const igstk::NDITracker::TrackingSampleInfo &info, QDomElement &node)
Class for controlling the igstk tracking (hardware) interface.
Definition: cxIgstkTool.h:52
void initialized(bool on)
when all trackers and tools are initialized == true, else false
boost::shared_ptr< IgstkTracker > TrackerPtr
IgstkToolPtr getRefereceTool()
ThreadSafe.
Namespace for all CustusX production code.