NorMIT-nav  22.09
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  mTimer->stop();
60  disconnect(mTimer, SIGNAL(timeout()), this, SLOT(checkTimeoutsAndRequestTransformSlot()));
61  this->trackSlot(false);
62  this->initializeSlot(false);
63 }
64 
65 std::map<QString, IgstkToolPtr> IgstkToolManager::getTools()
66 {
67  QMutexLocker sentry(&mToolMutex);
68  return mTools;
69 }
70 
72 {
73  QMutexLocker sentry(&mReferenceMutex);
74  return mReferenceTool;
75 }
76 
77 void IgstkToolManager::setReferenceAndTrackerOnTools()
78 {
79  if (!mReferenceTool)
80  {
81  reportWarning("Tracking is configured without a reference tool.");
82  }
83 
84  std::map<QString, IgstkToolPtr>::iterator it;
85  for (it = mTools.begin(); it != mTools.end(); ++it)
86  {
87  if (mReferenceTool)
88  it->second->setReference(mReferenceTool);
89  if (mTracker)
90  it->second->setTracker(mTracker);
91  }
92 }
93 
94 void IgstkToolManager::createTracker(ToolFileParser::TrackerInternalStructure trackerStructure)
95 {
96  TrackerPtr tracker(new IgstkTracker(trackerStructure));
97  if (tracker->isValid())
98  mTracker = tracker;
99  else
100  reportWarning("Invalid tracker.");
101 }
102 
103 void IgstkToolManager::createTools(std::vector<ToolFileParser::ToolInternalStructurePtr> toolStructures,
104  ToolFileParser::ToolInternalStructurePtr referenceToolStructure)
105 {
106  for (unsigned i = 0; i < toolStructures.size(); ++i)
107  {
108  this->addIgstkTools(toolStructures[i]);
109  }
110  if (referenceToolStructure && !referenceToolStructure->mUid.isEmpty())
111  {
112  IgstkToolPtr refTool = this->addIgstkTools(referenceToolStructure);
113  if (refTool->isValid())
114  mReferenceTool = refTool;
115  }
116 }
117 
118 IgstkToolPtr IgstkToolManager::addIgstkTools(ToolFileParser::ToolInternalStructurePtr toolStructure)
119 {
120  IgstkToolPtr igstkTool(new IgstkTool(toolStructure));
121  if (igstkTool->isValid())
122  {
123  QMutexLocker sentry(&mToolMutex);
124  mTools[igstkTool->getUid()] = igstkTool;
125  connect(igstkTool.get(), SIGNAL(attachedToTracker(bool)), this, SLOT(deviceInitializedSlot(bool)));
126  }
127  else
128  {
129  reportWarning(toolStructure->mUid + " is not valid.");
130  }
131  return igstkTool;
132 }
133 
134 void IgstkToolManager::trackerTrackingSlot(bool isTracking)
135 {
136  int igstkPulsingDriveRate = 10;
137  if (isTracking)
138  mTimer->start(igstkPulsingDriveRate);
139  else
140  mTimer->stop();
141 }
142 
144 {
145  if (on)
146  {
147  connect(mTracker.get(), SIGNAL(initialized(bool)), this, SLOT(attachToolsWhenTrackerIsInitializedSlot(bool)));
148  if (!mTracker->isOpen())
149  mTracker->open();
150  }
151  else
152  {
153  mTracker->detachTools(mTools); //not sure we have to detach all tools before we close, read NDI manual
154  if (mTracker->isOpen())
155  mTracker->close();
156  }
157 }
158 
160 {
161  if (on && !mTracker->isTracking())
162  mTracker->startTracking();
163  else if (!on && mTracker->isTracking())
164  mTracker->stopTracking();
165 }
166 
167 void IgstkToolManager::checkTimeoutsAndRequestTransformSlot()
168 {
169  igstk::PulseGenerator::CheckTimeouts();
170 
171  std::map<QString, IgstkToolPtr>::iterator it = mTools.begin();
172  for (; it != mTools.end(); ++it)
173  {
174  if (!it->second)
175  continue;
176 
177  if (mReferenceTool)
178  it->second->getPointer()->RequestComputeTransformTo(mReferenceTool->getPointer());
179  else
180  it->second->getPointer()->RequestComputeTransformTo(mTracker->getPointer());
181  }
182 }
183 
184 void IgstkToolManager::deviceInitializedSlot(bool deviceInit)
185 {
186  size_t numberOfDevices = mTools.size() + 1; //+1 is the tracker
187 
188  if (deviceInit)
189  {
190  mInitAnsweres++;
191 
192  if (mInitAnsweres == numberOfDevices)
193  {
194  mInternalInitialized = true;
195  emit initialized(true);
196  }
197  }
198  else
199  {
200  mInitAnsweres--;
201 
202  if (mInitAnsweres < numberOfDevices)
203  {
204  if (mInternalInitialized)
205  {
206  mInitAnsweres = 0;
207  mInternalInitialized = false;
208  emit initialized(false);
209  }
210  }
211  }
212 }
213 
214 void IgstkToolManager::attachToolsWhenTrackerIsInitializedSlot(bool open)
215 {
216  if (!open)
217  return;
218 
219  disconnect(mTracker.get(), SIGNAL(initialized(bool)), this, SLOT(attachToolsWhenTrackerIsInitializedSlot(bool)));
220  mTracker->attachTools(mTools);
221 }
222 
223 void IgstkToolManager::printStatus()
224 {
225  std::cout << "mInternalInitialized " << mInternalInitialized << std::endl;
226  std::cout << "mInitAnsweres " << mInitAnsweres << std::endl;
227  std::cout << "mTracker->isValid() " << mTracker->isValid() << std::endl;
228  std::cout << "mTracker->isOpen() " << mTracker->isOpen() << std::endl;
229  std::cout << "mTracker->isInitialized() " << mTracker->isInitialized() << std::endl;
230  std::cout << "mTracker->isTracking() " << mTracker->isTracking() << std::endl;
231  std::cout << "mTools.size() " << string_cast(mTools.size()) << std::endl;
232 }
233 }
cx::IgstkToolManager::error
void error()
cx::IgstkToolManager::IgstkToolManager
IgstkToolManager(ToolFileParser::TrackerInternalStructure trackerStructure, std::vector< ToolFileParser::ToolInternalStructurePtr > toolStructures, ToolFileParser::ToolInternalStructurePtr referenceToolStructure)
Definition: cxIgstkToolManager.cpp:34
cx::IgstkToolManager::tracking
void tracking(bool on)
cx::sampleInfo2xml
void sampleInfo2xml(const igstk::NDITracker::TrackingSampleInfo &info, QDomElement &node)
Definition: cxIgstkToolManager.cpp:20
cxLogger.h
cx::IgstkToolManager::getRefereceTool
IgstkToolPtr getRefereceTool()
ThreadSafe.
Definition: cxIgstkToolManager.cpp:71
cx
Namespace for all CustusX production code.
Definition: cx_dev_group_definitions.h:13
cx::IgstkToolPtr
boost::shared_ptr< class IgstkTool > IgstkToolPtr
Definition: cxOpenIGTLinkTool.h:35
cx::IgstkToolManager::initializeSlot
void initializeSlot(bool on)
connects to the hardware
Definition: cxIgstkToolManager.cpp:143
cx::TrackerPtr
boost::shared_ptr< IgstkTracker > TrackerPtr
Definition: cxIgstkTracker.h:157
cx::IgstkToolManager::trackSlot
void trackSlot(bool on)
tracking on or off
Definition: cxIgstkToolManager.cpp:159
string_cast
std::string string_cast(const T &val)
Definition: cxTypeConversions.h:37
cxTypeConversions.h
cxIgstkToolManager.h
cx::ToolFileParser::TrackerInternalStructure
Definition: cxToolFileParser.h:37
cx::IgstkToolManager::getTools
std::map< QString, IgstkToolPtr > getTools()
ThreadSafe.
Definition: cxIgstkToolManager.cpp:65
cx::IgstkToolManager::~IgstkToolManager
virtual ~IgstkToolManager()
Definition: cxIgstkToolManager.cpp:57
cx::ToolFileParser::ToolInternalStructurePtr
boost::shared_ptr< ToolInternalStructure > ToolInternalStructurePtr
Definition: cxToolFileParser.h:99
cx::reportWarning
void reportWarning(QString msg)
Definition: cxLogger.cpp:70
cx::IgstkToolManager::initialized
void initialized(bool on)
when all trackers and tools are initialized == true, else false