CustusX  2023.01.05-dev+develop.0da12
An IGT application
cxOpenIGTLinkTrackingSystemService.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 
13 
14 #include "cxLogger.h"
15 #include "cxOpenIGTLinkTool.h"
16 #include "cxProfile.h"
17 
20 
21 namespace cx
22 {
23 
24 std::vector<ToolPtr> toVector(std::map<QString, OpenIGTLinkToolPtr> map)
25 {
26  std::vector<ToolPtr> retval;
27  std::map<QString, OpenIGTLinkToolPtr>::iterator it = map.begin();
28  for(; it!= map.end(); ++it)
29  {
30  retval.push_back(it->second);
31  }
32  return retval;
33 }
34 
36  mNetworkHandler(networkHandler)
37 {
38  if(mNetworkHandler == NULL)
39  return;
40 
41  connect(mNetworkHandler.get(), &NetworkHandler::connected,this, &OpenIGTLinkTrackingSystemService::serverIsConnected);
42  connect(mNetworkHandler.get(), &NetworkHandler::disconnected, this, &OpenIGTLinkTrackingSystemService::serverIsDisconnected);
44  //connect(mNetworkHandler.get(), &NetworkHandler::calibration, this, &OpenIGTLinkTrackingSystemService::receiveCalibration);
45  connect(mNetworkHandler.get(), &NetworkHandler::probedefinition, this, &OpenIGTLinkTrackingSystemService::receiveProbedefinition);
46 
48 
49  this->setConfigurationFile(profile()->getToolConfigFilePath());
50 }
51 
53 {
54  this->deconfigure();
55 }
56 
58 {
59  return "org.custusx.core.openigtlink3";
60 }
61 
63 {
64  emit setInternalState(val);
65 }
66 
68 {
70  mState = val;
71  emit stateChanged();
72 }
73 
75 {
76  return toVector(mTools);
77 }
78 
80 {
82  retval.reset(new TrackerConfigurationImpl());
83  retval->setTrackingSystemImplementation(TRACKING_SYSTEM_IMPLEMENTATION_IGTLINK);
84  return retval;
85 }
86 
88 {
89  return mReference;
90 }
91 
93 {
94  //parse
96 
97  if(!configParser.getTrackingSystemImplementation().contains(TRACKING_SYSTEM_IMPLEMENTATION_IGTLINK, Qt::CaseInsensitive))
98  {
99  CX_LOG_DEBUG() << "OpenIGTLinkTrackingSystemService::configure(): Not using OpenIGTLink tracking.";
100  this->setState(Tool::tsNONE);
101  return;
102  }
104 
105  CX_LOG_DEBUG() << "OpenIGTLinkTrackingSystemService::configure(): Using OpenIGTLink3 tracking";
106 
107  std::vector<ConfigurationFileParser::ToolStructure> toolList = configParser.getToolListWithMetaInformation();
108 
109  //Create tools
110  for(std::vector<ConfigurationFileParser::ToolStructure>::iterator it = toolList.begin(); it != toolList.end(); ++it)
111  {
112  ToolFileParser toolParser((*it).mAbsoluteToolFilePath, mLoggingFolder);
113  ToolFileParser::ToolInternalStructurePtr internalTool = toolParser.getTool();
114 
115  QString devicename = internalTool->mUid;
116  OpenIGTLinkToolPtr newTool = OpenIGTLinkToolPtr(new OpenIGTLinkTool((*it), internalTool));
117  if(mTools.count(devicename))
118  CX_LOG_WARNING() << "Tool configuration already contain tool: " << devicename << ". Existing tool will be overwritten";
119  mTools[devicename] = newTool;
120  if(newTool->isReference())
121  mReference = newTool;
122  }
123  if(!mReference)
124  CX_LOG_WARNING() << "OpenIGTLinkTrackingSystemService::configure() Got no reference tool";
125 
126  mState = Tool::tsCONFIGURED;//Setting state directly. Cannot get setState() to work with test
127 }
128 
130 {
132 
133  if (!this->isConfigured())
134  return;
135 
136  mState = Tool::tsNONE;//Setting state directly. Cannot get setState() to work with test
137 
138  mTools.clear();
139  mReference.reset();
140 }
141 
142 void OpenIGTLinkTrackingSystemService::serverIsConnected()
143 {
145  this->setState(Tool::tsTRACKING);
146 }
147 
148 void OpenIGTLinkTrackingSystemService::serverIsDisconnected()
149 {
152 }
153 
154 void OpenIGTLinkTrackingSystemService::receiveTransform(QString devicename, Transform3D transform, double timestampMS)
155 {
156 // CX_LOG_DEBUG() << "receiveTransform for: " << devicename;
157  OpenIGTLinkToolPtr tool = this->getTool(devicename);
158  if(tool)
159  {
160  if(tool->applyReference())
161  {
162  Transform3D tsMs = transform;
163  Transform3D prMs = tsMs;//If no reference tool: ts==pr (ts = tracking system )
164  if(mReference && !tool->isReference())
165  {
166  Transform3D tsMpr = mReference->get_prMt();
167  prMs = tsMpr.inv() * tsMs;
168  }
169  //Apply ref sensor pos to all tool positions
170  tool->toolTransformAndTimestampSlot(prMs, timestampMS);
171  }
172  else
173  tool->toolTransformAndTimestampSlot(transform, timestampMS);
174  }
175  else
176  this->printTransformIdWarning(devicename);
177 }
178 
180 {
181  if(!mTransformIdWarningPrinted.contains(devicename))
182  {
183  CX_LOG_WARNING() << "No tool with openigtlinktransformid: " << devicename;
184  CX_LOG_WARNING() << "Add the id to an openigtlinktransformid tag in one of the used tool files to fix this";
185  mTransformIdWarningPrinted << devicename;
186  return true;
187  }
188  return false;
189 }
190 
191 void OpenIGTLinkTrackingSystemService::receiveCalibration(QString devicename, Transform3D calibration)
192 {
193  CX_LOG_DEBUG() << "receiveCalibration for: " << devicename;
194  OpenIGTLinkToolPtr tool = this->getTool(devicename);
195  if(tool)
196  tool->setCalibration_sMt(calibration);
197 }
198 
199 void OpenIGTLinkTrackingSystemService::receiveProbedefinition(QString devicename, ProbeDefinitionPtr definition)
200 {
201  OpenIGTLinkToolPtr tool = this->getTool(devicename);
202  if(tool)
203  {
204  ProbePtr probe = tool->getProbe();
205  if(probe)
206  {
207  ProbeDefinition old_def = probe->getProbeDefinition();
208  definition->setUid(old_def.getUid());
209  definition->applySoundSpeedCompensationFactor(old_def.getSoundSpeedCompensationFactor());
210 
211  probe->setProbeDefinition(*(definition.get()));
212  emit stateChanged();
213  }
214  }
215 }
216 
218 {
219 // CX_LOG_DEBUG() << "OpenIGTLinkTrackingSystemService::getTool: " << devicename;
220 
221  std::map<QString, OpenIGTLinkToolPtr>::iterator it;
222  for (it = mTools.begin(); it != mTools.end(); ++it)
223  {
224  OpenIGTLinkToolPtr tool = it->second;
225  if (tool->doIdCorrespondToTool(devicename))
226  {
227  return tool;
228  }
229  }
230  return OpenIGTLinkToolPtr();
231 }
232 
233 
235 {
236  mNetworkHandler->clearTimestampSynchronization();
237 }
238 
239 } /* namespace cx */
cxResource_EXPORT ProfilePtr profile()
Definition: cxProfile.cpp:160
DoubleBoundingBox3D transform(const Transform3D &m, const DoubleBoundingBox3D &bb)
boost::shared_ptr< ToolInternalStructure > ToolInternalStructurePtr
Transform3D Transform3D
Transform3D is a representation of an affine 3D transform.
QString mConfigurationFilePath
path to the configuration file
virtual void setConfigurationFile(QString configurationFile)
double getSoundSpeedCompensationFactor() const
boost::shared_ptr< class NetworkHandler > NetworkHandlerPtr
QString getUid() const
configured with basic info
Definition: cxTool.h:75
void stateChanged()
Reset time synchronization. Used for resetting time synchronization of incoming timestamps in OpenIGT...
boost::shared_ptr< Probe > ProbePtr
Definition: cxProbe.h:72
boost::shared_ptr< class TrackerConfiguration > TrackerConfigurationPtr
void updateTrackingSystemImplementation(QString trackingSystemImplementation)
QString mLoggingFolder
path to where logging should be saved
virtual ToolInternalStructurePtr getTool()
const char * TRACKING_SYSTEM_IMPLEMENTATION_IGTLINK
void setUid(QString uid)
void transform(QString devicename, Transform3D transform, double timestamp)
std::vector< ToolPtr > toVector(std::map< QString, OpenIGTLinkToolPtr > map)
void probedefinition(QString devicename, ProbeDefinitionPtr definition)
Class for reading the files defining a CustusX tool.
not available
Definition: cxTool.h:74
connected to hardware, if any, ready to use
Definition: cxTool.h:76
Definition of characteristics for an Ultrasound Probe Sector.
#define CX_LOG_DEBUG
Definition: cxLogger.h:95
virtual bool isConfigured() const
#define CX_LOG_WARNING
Definition: cxLogger.h:98
emitting tracking data
Definition: cxTool.h:77
Class for reading the files defining a CustusX tool.
void internalSetState(Tool::State val)
boost::shared_ptr< class ProbeDefinition > ProbeDefinitionPtr
std::vector< ConfigurationFileParser::ToolStructure > getToolListWithMetaInformation()
Namespace for all CustusX production code.
boost::shared_ptr< class Tool > ToolPtr