33 #define _USE_MATH_DEFINES
37 #include "boost/bind.hpp"
44 #include <vtkDoubleArray.h>
45 #include <QCoreApplication>
76 mLastLoadPositionHistory(0),
86 this->initializeManualTool();
88 connect(
settings(), SIGNAL(valueChangedFor(QString)),
this, SLOT(globalConfigurationFileChangedSlot(QString)));
90 this->listenForTrackingSystemServices(context);
95 while (!mTrackingSystems.empty())
105 void TrackingImplService::onSystemStateChanged()
107 this->rebuildCachedTools();
124 std::vector<TrackingSystemServicePtr> old = mPlaybackSystem->getBase();
125 for (
unsigned i=0; i<old.size(); ++i)
127 mPlaybackSystem.reset();
134 std::vector<TrackingSystemServicePtr> old = mPlaybackSystem->getBase();
135 for (
unsigned i=0; i<old.size(); ++i)
159 mTrackingSystems.push_back(system);
160 report(
"Installing tracking system: " + system->getUid());
162 this->onSystemStateChanged();
167 report(
"Uninstalling tracking system: " + system->getUid());
170 for (
unsigned i=0; i<mTrackingSystems.size(); ++i)
172 if (mTrackingSystems[i]!=system)
174 mTrackingSystems.erase(mTrackingSystems.begin()+i);
178 this->onSystemStateChanged();
183 return mTrackingSystems;
186 void TrackingImplService::initializeManualTool()
192 mTools[
"ManualTool"] = mManualTool;
193 mManualTool->setVisible(
true);
194 connect(mManualTool.get(), &
Tool::toolVisible,
this, &TrackingImplService::activeCheckSlot);
196 connect(mManualTool.get(), &
Tool::tooltipOffset,
this, &TrackingImplService::onTooltipOffset);
201 mManualTool->set_prMt(prMt);
202 this->activeCheckSlot();
208 for (
unsigned i=0; i<mTrackingSystems.size(); ++i)
209 state = std::max(state, mTrackingSystems[i]->
getState());
215 for (
unsigned i=0; i<mTrackingSystems.size(); ++i)
219 void TrackingImplService::listenForTrackingSystemServices(ctkPluginContext *context)
223 boost::bind(&TrackingImplService::onTrackingSystemAdded,
this, _1),
224 boost::bind(&TrackingImplService::onTrackingSystemModified,
this, _1),
225 boost::bind(&TrackingImplService::onTrackingSystemRemoved,
this, _1)
227 mServiceListener->open();
230 void TrackingImplService::onTrackingSystemAdded(TrackingSystemService* service)
235 void TrackingImplService::onTrackingSystemRemoved(TrackingSystemService* service)
240 void TrackingImplService::onTrackingSystemModified(TrackingSystemService* service)
244 void TrackingImplService::rebuildCachedTools()
247 for (
unsigned i=0; i<mTrackingSystems.size(); ++i)
249 this->addToolsFrom(mTrackingSystems[i]);
251 mTools[mManualTool->getUid()] = mManualTool;
252 this->imbueManualToolWithRealProperties();
253 this->loadPositionHistory();
254 this->resetTrackingPositionFilters();
255 this->onTooltipOffset(mToolTipOffset);
259 void TrackingImplService::imbueManualToolWithRealProperties()
262 if (!
settings()->value(
"giveManualToolPhysicalProperties").toBool())
265 for (ToolMap::iterator iter = mTools.begin(); iter != mTools.end(); ++iter)
267 if (iter->second == mManualTool)
271 mManualTool->setBase(iter->second);
272 mManualTool->startEmittingContinuousPositions(100);
274 report(
"Manual tool imbued with properties from " + iter->first);
281 std::vector<ToolPtr> tools = system->getTools();
282 for (
unsigned i=0; i<tools.size(); ++i)
285 mTools[tool->getUid()] = tool;
286 connect(tool.get(), SIGNAL(toolVisible(
bool)),
this, SLOT(activeCheckSlot()));
291 mReferenceTool = tool;
295 void TrackingImplService::onTooltipOffset(
double val)
297 mToolTipOffset = val;
298 for (ToolMap::iterator iter = mTools.begin(); iter != mTools.end(); ++iter)
300 iter->second->setTooltipOffset(val);
309 ToolMap::iterator it = tools.begin();
310 for (; it != tools.end(); ++it)
315 retval[it->second] = toolMap;
331 ToolMap::iterator it = mTools.find(uid);
332 if (it != mTools.end())
345 if (mActiveTool && mActiveTool->getUid() == uid)
352 mActiveTool = newTool;
357 if (oldTool && (oldTool!=mManualTool))
358 mManualTool->set_prMt(oldTool->get_prMt(), oldTool->getTimestamp() -1);
359 mManualTool->setVisible(
true);
363 mManualTool->setVisible(
false);
371 return mReferenceTool;
374 void TrackingImplService::savePositionHistory()
376 QString filename = this->getLoggingFolder() +
"/toolpositions.snwpos";
380 ToolMap::iterator it = mTools.begin();
381 for (; it != mTools.end(); ++it)
390 TimedTransformMap::iterator iter = data->lower_bound(mLastLoadPositionHistory);
391 for (; iter != data->end(); ++iter)
392 writer.write(iter->second, (iter->first), current->getUid());
398 void TrackingImplService::loadPositionHistory()
403 this->savePositionHistory();
405 QString filename = this->getLoggingFolder()+
"/toolpositions.snwpos";
407 PositionStorageReader reader(filename);
413 QStringList missingTools;
415 while (!reader.atEnd())
417 if (!reader.read(&matrix, ×tamp, &toolUid))
423 (*current->getPositionHistory())[timestamp] = matrix;
427 missingTools << toolUid;
431 missingTools.removeDuplicates();
432 missingTools.removeAll(
"");
434 if (!missingTools.empty())
436 reportWarning(QString(
"Loaded position history, but some of the tools "
437 "are not present in the configuration:"
438 "\n \t%1").arg(missingTools.join(
"\n \t")));
454 void TrackingImplService::globalConfigurationFileChangedSlot(QString key)
456 if (key.contains(
"TrackingPositionFilter"))
458 this->resetTrackingPositionFilters();
462 void TrackingImplService::resetTrackingPositionFilters()
464 bool enabled =
settings()->
value(
"TrackingPositionFilter/enabled",
false).toBool();
465 double cutoff =
settings()->
value(
"TrackingPositionFilter/cutoffFrequency", 0).toDouble();
467 for (ToolMap::iterator iter=mTools.begin(); iter!=mTools.end(); ++iter)
472 filter.reset(
new TrackingPositionFilter());
474 filter->setCutOffFrequency(cutoff);
476 iter->second->resetTrackingPositionFilter(filter);
480 void TrackingImplService::activeCheckSlot()
482 if (this->manualToolHasMostRecentTimestamp() && mManualTool->getVisible())
488 bool use =
settings()->
value(
"Automation/autoSelectActiveTool").toBool();
493 std::vector<ToolPtr> tools = this->getVisibleTools();
494 tools.push_back(mManualTool);
500 const QString uid = tools[0]->getUid();
505 bool TrackingImplService::manualToolHasMostRecentTimestamp()
518 for (ToolMap::iterator it = mTools.begin(); it != mTools.end(); ++it)
524 bestTime = std::max(bestTime, it->second->getTimestamp());
530 return (mts > bestTime);
533 std::vector<ToolPtr> TrackingImplService::getVisibleTools()
535 std::vector<ToolPtr> retval;
536 for (ToolMap::iterator it = mTools.begin(); it != mTools.end(); ++it)
537 if (it->second->getVisible())
538 retval.push_back(it->second);
575 QString TrackingImplService::getLoggingFolder()
577 return mSession->getSubFolder(
"Logs/");
580 void TrackingImplService::onSessionChanged()
582 QString loggingFolder = this->getLoggingFolder();
584 for (
unsigned i=0; i<mTrackingSystems.size(); ++i)
585 mTrackingSystems[i]->setLoggingFolder(loggingFolder);
588 void TrackingImplService::onSessionCleared()
590 mManualTool->set_prMt(Transform3D::Identity());
593 void TrackingImplService::onSessionLoad(QDomElement& node)
595 XMLNodeParser root(node);
596 QDomElement toolManagerNode = root.descend(
"managers/toolManager").node().toElement();
597 if (!toolManagerNode.isNull())
598 this->parseXml(toolManagerNode);
602 void TrackingImplService::onSessionSave(QDomElement& node)
604 XMLNodeAdder root(node);
605 QDomElement managerNode = root.descend(
"managers").node().toElement();
606 this->addXml(managerNode);
608 this->savePositionHistory();
611 void TrackingImplService::addXml(QDomNode& parentNode)
613 XMLNodeAdder parent(parentNode);
614 XMLNodeAdder base(parent.addElement(
"toolManager"));
616 base.addTextToElement(
"toolTipOffset",
qstring_cast(mToolTipOffset));
617 base.addTextToElement(
"manualTool",
"\n" +
qstring_cast(mManualTool->get_prMt()));
620 XMLNodeAdder toolsNode(base.addElement(
"tools"));
622 ToolMap::iterator toolIt = tools.begin();
623 for (; toolIt != tools.end(); ++toolIt)
625 cxToolPtr tool = boost::dynamic_pointer_cast<ToolImpl>(toolIt->second);
628 toolsNode.addObjectToElement(
"tool", tool);
634 void TrackingImplService::parseXml(QDomNode& dataNode)
636 if (dataNode.isNull())
639 XMLNodeParser base(dataNode);
641 QString manualToolText = dataNode.namedItem(
"manualTool").toElement().text();
642 mManualTool->set_prMt(Transform3D::fromString(manualToolText));
644 mToolTipOffset = base.parseDoubleFromElementWithDefault(
"toolTipOffset", 0.0);
645 this->onTooltipOffset(mToolTipOffset);
649 XMLNodeParser toolssNode(dataNode.namedItem(
"tools"));
650 std::vector<QDomElement> toolNodes = toolssNode.getDuplicateElements(
"tool");
652 for (
unsigned i=0; i<toolNodes.size(); ++i)
654 QDomElement toolNode = toolNodes[i];
655 QString tool_uid = toolNode.attribute(
"uid");
656 if (tools.find(tool_uid) != tools.end())
658 cxToolPtr tool = boost::dynamic_pointer_cast<ToolImpl>(tools.find(tool_uid)->second);
659 tool->parseXml(toolNode);
684 if (active && active->getProbe())
690 for (ToolMap::iterator iter = tools.begin(); iter != tools.end(); ++iter)
691 if (iter->second->getProbe() && iter->second->getProbe()->isValid() && iter->second->getVisible())
695 for (ToolMap::iterator iter = tools.begin(); iter != tools.end(); ++iter)
696 if (iter->second->getProbe() && iter->second->getProbe()->isValid())
705 for (
unsigned i=0; i<mTrackingSystems.size(); ++i)
int getPriority(DataPtr data)
QString qstring_cast(const T &val)
Transform3D createTransformRotateY(const double angle)
virtual void runDummyTool(DummyToolPtr tool)
void isLoading(QDomElement &root)
emitted while loading a session. Xml storage is available, getRootFolder() is set to loaded value...
virtual ToolPtr getFirstProbe()
Find a probe that can be connected to a rt source.
std::map< ToolPtr, TimedTransformMap > SessionToolHistoryMap
virtual ToolPtr getTool(const QString &uid)
get a specific tool
Transform3D Transform3D
Transform3D is a representation of an affine 3D transform.
boost::shared_ptr< ToolImpl > cxToolPtr
double getMilliSecondsSinceEpoch()
static SessionStorageServicePtr create(ctkPluginContext *pluginContext)
QVariant value(const QString &key, const QVariant &defaultValue=QVariant()) const
virtual Tool::State getState() const
virtual void setState(const Tool::State val)
virtual void setPlaybackMode(PlaybackTimePtr controller)
void sessionChanged()
emitted after change to a new session (new or loaded or cleared)
bool toolTypeSort(const ToolPtr tool1, const ToolPtr tool2)
function for sorting tools by type
boost::shared_ptr< class PlaybackTime > PlaybackTimePtr
virtual void setActiveTool(const QString &uid)
can be set to either a connected or configured tool
boost::shared_ptr< class DummyTool > DummyToolPtr
boost::shared_ptr< class TrackerConfiguration > TrackerConfigurationPtr
void activeToolChanged(const QString &uId)
void reportWarning(QString msg)
boost::shared_ptr< TimedTransformMap > TimedTransformMapPtr
virtual ToolMap getTools()
get all configured and initialized tools
virtual TrackerConfigurationPtr getConfiguration()
virtual ToolPtr getManualTool()
a mouse-controllable virtual tool that is available even when not tracking.
virtual void unInstallTrackingSystem(TrackingSystemServicePtr system)
Settings * settings()
Shortcut for accessing the settings instance.
Writer class for the position file.
void cleared()
emitted when session is cleared, before isLoading is called
Helper class for listening to services being added, modified and removed.
std::map< QString, ToolPtr > ToolMap
virtual std::vector< TrackingSystemServicePtr > getTrackingSystems()
virtual ToolPtr getReferenceTool() const
get the tool that is used as a reference, if any
Interface towards a dummy tracking system.
virtual ~TrackingImplService()
TrackingImplService(ctkPluginContext *context)
virtual ToolPtr getActiveTool()
get the tool that has higest priority when tracking
Transform3D createTransformRotateZ(const double angle)
virtual bool isPlaybackMode() const
Interface towards a playback tracking system.Wraps another tracking system, enabling playback of the ...
boost::shared_ptr< class TrackingPositionFilter > TrackingPositionFilterPtr
virtual SessionToolHistoryMap getSessionHistory(double startTime, double stopTime)
virtual void installTrackingSystem(TrackingSystemServicePtr system)
void isSaving(QDomElement &root)
xml storage is available
std::map< double, Transform3D > TimedTransformMap
boost::shared_ptr< class Tool > ToolPtr
boost::shared_ptr< class TrackingSystemService > TrackingSystemServicePtr