12 #define _USE_MATH_DEFINES
16 #include "boost/bind.hpp"
23 #include <vtkDoubleArray.h>
24 #include <QCoreApplication>
54 mLastLoadPositionHistory(0),
66 this->initializeManualTool();
68 connect(
settings(), SIGNAL(valueChangedFor(QString)),
this, SLOT(globalConfigurationFileChangedSlot(QString)));
70 this->listenForTrackingSystemServices(context);
75 while (!mTrackingSystems.empty())
85 void TrackingImplService::onSystemStateChanged()
87 this->rebuildCachedTools();
104 std::vector<TrackingSystemServicePtr> old = mPlaybackSystem->getBase();
105 for (
unsigned i=0; i<old.size(); ++i)
107 mPlaybackSystem.reset();
114 std::vector<TrackingSystemServicePtr> old = mPlaybackSystem->getBase();
115 for (
unsigned i=0; i<old.size(); ++i)
140 mTrackingSystems.push_back(system);
141 report(
"Installing tracking system: " + system->getUid());
144 this->onSystemStateChanged();
149 report(
"Uninstalling tracking system: " + system->getUid());
153 for (
unsigned i=0; i<mTrackingSystems.size(); ++i)
155 if (mTrackingSystems[i]!=system)
157 mTrackingSystems.erase(mTrackingSystems.begin()+i);
161 this->onSystemStateChanged();
166 return mTrackingSystems;
169 void TrackingImplService::initializeManualTool()
175 mTools[
"ManualTool"] = mManualTool;
176 mManualTool->setVisible(
true);
177 connect(mManualTool.get(), &
Tool::toolVisible,
this, &TrackingImplService::activeCheckSlot);
179 connect(mManualTool.get(), &
Tool::tooltipOffset,
this, &TrackingImplService::onTooltipOffset);
184 mManualTool->set_prMt(prMt);
185 this->activeCheckSlot();
191 for (
unsigned i=0; i<mTrackingSystems.size(); ++i)
192 state = std::max(state, mTrackingSystems[i]->
getState());
198 for (
unsigned i=0; i<mTrackingSystems.size(); ++i)
202 void TrackingImplService::listenForTrackingSystemServices(ctkPluginContext *context)
206 boost::bind(&TrackingImplService::onTrackingSystemAdded,
this, _1),
207 boost::bind(&TrackingImplService::onTrackingSystemModified,
this, _1),
208 boost::bind(&TrackingImplService::onTrackingSystemRemoved,
this, _1)
210 mServiceListener->open();
213 void TrackingImplService::onTrackingSystemAdded(TrackingSystemService* service)
218 void TrackingImplService::onTrackingSystemRemoved(TrackingSystemService* service)
223 void TrackingImplService::onTrackingSystemModified(TrackingSystemService* service)
228 void TrackingImplService::rebuildCachedTools()
231 for (
unsigned i=0; i<mTrackingSystems.size(); ++i)
233 this->addToolsFrom(mTrackingSystems[i]);
235 mTools[mManualTool->getUid()] = mManualTool;
236 this->imbueManualToolWithRealProperties();
237 this->loadPositionHistory();
238 this->resetTrackingPositionFilters();
239 this->onTooltipOffset(mToolTipOffset);
240 if(!mActiveTool || !
getTool(mActiveTool->getUid()))
244 void TrackingImplService::imbueManualToolWithRealProperties()
247 if (!
settings()->value(
"giveManualToolPhysicalProperties").toBool())
250 for (ToolMap::iterator iter = mTools.begin(); iter != mTools.end(); ++iter)
252 if (iter->second == mManualTool)
256 mManualTool->setBase(iter->second);
257 mManualTool->startEmittingContinuousPositions(100);
259 report(
"Manual tool imbued with properties from " + iter->first);
266 std::vector<ToolPtr> tools = system->getTools();
267 for (
unsigned i=0; i<tools.size(); ++i)
270 mTools[tool->getUid()] = tool;
271 connect(tool.get(), SIGNAL(toolVisible(
bool)),
this, SLOT(activeCheckSlot()));
276 mReferenceTool = tool;
280 void TrackingImplService::onTooltipOffset(
double val)
282 mToolTipOffset = val;
283 for (ToolMap::iterator iter = mTools.begin(); iter != mTools.end(); ++iter)
285 iter->second->setTooltipOffset(val);
294 ToolMap::iterator it = tools.begin();
295 for (; it != tools.end(); ++it)
300 retval[it->second] = toolMap;
316 ToolMap::iterator it = mTools.find(uid);
317 if (it != mTools.end())
330 if (mActiveTool && mActiveTool->getUid() == uid)
337 mActiveTool = newTool;
342 if (oldTool && (oldTool!=mManualTool))
343 mManualTool->set_prMt(oldTool->get_prMt(), oldTool->getTimestamp() -1);
344 mManualTool->setVisible(
true);
348 mManualTool->setVisible(
false);
356 mActiveTool = mManualTool;
362 return mReferenceTool;
365 void TrackingImplService::savePositionHistory()
367 QString filename = this->getLoggingFolder() +
"/toolpositions.snwpos";
371 ToolMap::iterator it = mTools.begin();
372 for (; it != mTools.end(); ++it)
381 TimedTransformMap::iterator iter = data->lower_bound(mLastLoadPositionHistory);
382 for (; iter != data->end(); ++iter)
383 writer.write(iter->second, (iter->first), current->getUid());
389 void TrackingImplService::loadPositionHistory()
394 this->savePositionHistory();
396 QString filename = this->getLoggingFolder()+
"/toolpositions.snwpos";
398 PositionStorageReader reader(filename);
404 QStringList missingTools;
406 while (!reader.atEnd())
408 if (!reader.read(&matrix, ×tamp, &toolUid))
414 (*current->getPositionHistory())[timestamp] = matrix;
418 missingTools << toolUid;
422 missingTools.removeDuplicates();
423 missingTools.removeAll(
"");
425 if (!missingTools.empty())
427 reportWarning(QString(
"Loaded position history, but some of the tools "
428 "are not present in the configuration:"
429 "\n \t%1").arg(missingTools.join(
"\n \t")));
445 void TrackingImplService::globalConfigurationFileChangedSlot(QString key)
447 if (key.contains(
"TrackingPositionFilter"))
449 this->resetTrackingPositionFilters();
453 void TrackingImplService::resetTrackingPositionFilters()
455 bool enabled =
settings()->
value(
"TrackingPositionFilter/enabled",
false).toBool();
456 double cutoff =
settings()->
value(
"TrackingPositionFilter/cutoffFrequency", 0).toDouble();
458 for (ToolMap::iterator iter=mTools.begin(); iter!=mTools.end(); ++iter)
463 filter.reset(
new TrackingPositionFilter());
465 filter->setCutOffFrequency(cutoff);
467 iter->second->resetTrackingPositionFilter(filter);
471 void TrackingImplService::activeCheckSlot()
473 if (this->manualToolHasMostRecentTimestamp() && mManualTool->getVisible())
479 bool use =
settings()->
value(
"Automation/autoSelectActiveTool").toBool();
484 std::vector<ToolPtr> tools = this->getVisibleTools();
485 tools.push_back(mManualTool);
491 const QString uid = tools[0]->getUid();
496 bool TrackingImplService::manualToolHasMostRecentTimestamp()
509 for (ToolMap::iterator it = mTools.begin(); it != mTools.end(); ++it)
515 bestTime = std::max(bestTime, it->second->getTimestamp());
521 return (mts > bestTime);
524 std::vector<ToolPtr> TrackingImplService::getVisibleTools()
526 std::vector<ToolPtr> retval;
527 for (ToolMap::iterator it = mTools.begin(); it != mTools.end(); ++it)
528 if (it->second->getVisible())
529 retval.push_back(it->second);
566 QString TrackingImplService::getLoggingFolder()
568 return mSession->getSubFolder(
"Logs/");
571 void TrackingImplService::onSessionChanged()
573 QString loggingFolder = this->getLoggingFolder();
575 for (
unsigned i=0; i<mTrackingSystems.size(); ++i)
576 mTrackingSystems[i]->setLoggingFolder(loggingFolder);
579 void TrackingImplService::onSessionCleared()
581 mManualTool->set_prMt(Transform3D::Identity());
584 void TrackingImplService::onSessionLoad(QDomElement& node)
586 XMLNodeParser root(node);
587 QDomElement toolManagerNode = root.descend(
"managers/toolManager").node().toElement();
588 if (!toolManagerNode.isNull())
589 this->parseXml(toolManagerNode);
593 void TrackingImplService::onSessionSave(QDomElement& node)
595 XMLNodeAdder root(node);
596 QDomElement managerNode = root.descend(
"managers").node().toElement();
597 this->addXml(managerNode);
599 this->savePositionHistory();
602 void TrackingImplService::addXml(QDomNode& parentNode)
604 XMLNodeAdder parent(parentNode);
605 XMLNodeAdder base(parent.addElement(
"toolManager"));
607 base.addTextToElement(
"toolTipOffset",
qstring_cast(mToolTipOffset));
608 base.addTextToElement(
"manualTool",
"\n" +
qstring_cast(mManualTool->get_prMt()));
611 XMLNodeAdder toolsNode(base.addElement(
"tools"));
613 ToolMap::iterator toolIt = tools.begin();
614 for (; toolIt != tools.end(); ++toolIt)
616 cxToolPtr tool = boost::dynamic_pointer_cast<ToolImpl>(toolIt->second);
619 toolsNode.addObjectToElement(
"tool", tool);
625 void TrackingImplService::parseXml(QDomNode& dataNode)
627 if (dataNode.isNull())
630 XMLNodeParser base(dataNode);
632 QString manualToolText = dataNode.namedItem(
"manualTool").toElement().text();
633 mManualTool->set_prMt(Transform3D::fromString(manualToolText));
635 mToolTipOffset = base.parseDoubleFromElementWithDefault(
"toolTipOffset", 0.0);
636 this->onTooltipOffset(mToolTipOffset);
640 XMLNodeParser toolssNode(dataNode.namedItem(
"tools"));
641 std::vector<QDomElement> toolNodes = toolssNode.getDuplicateElements(
"tool");
643 for (
unsigned i=0; i<toolNodes.size(); ++i)
645 QDomElement toolNode = toolNodes[i];
646 QString tool_uid = toolNode.attribute(
"uid");
647 if (tools.find(tool_uid) != tools.end())
649 cxToolPtr tool = boost::dynamic_pointer_cast<ToolImpl>(tools.find(tool_uid)->second);
650 tool->parseXml(toolNode);
675 if (active && active->getProbe())
681 for (ToolMap::iterator iter = tools.begin(); iter != tools.end(); ++iter)
682 if (iter->second->getProbe() && iter->second->getProbe()->isValid() && iter->second->getVisible())
686 for (ToolMap::iterator iter = tools.begin(); iter != tools.end(); ++iter)
687 if (iter->second->getProbe() && iter->second->getProbe()->isValid())
695 std::vector<TrackerConfigurationPtr> retval;
696 bool gotConfig =
false;
697 for (
unsigned i=0; i<mTrackingSystems.size(); ++i)
702 retval.push_back(config);
716 for(
unsigned i = 0; i < configs.size(); ++i)
718 if(configs[i]->getTrackingSystemImplementation() == trackingSystemImplementation)
731 if(configs.size() > 0)
732 retval = configs.at(0);
739 mTrackingSystemImplementation = trackingSystemImplementation;
744 return mTrackingSystemImplementation;
749 for (
unsigned i=0; i<mTrackingSystems.size(); ++i)
751 mTrackingSystems[i]->resetTimeSynchronization();