Fraxinus  2023.01.05-dev+develop.0da12
An IGT application
cxTrackingSystemPlaybackService.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 =========================================================================*/
12 
13 
14 #include "cxLogger.h"
15 #include "cxTime.h"
16 #include "cxPlaybackTime.h"
17 #include "cxPlaybackTool.h"
18 #include <QApplication>
19 #include "cxUtilHelpers.h"
20 #include "cxManualTool.h"
21 #include "cxTrackerConfiguration.h"
22 
23 namespace cx
24 {
25 
26 TrackingSystemPlaybackService::TrackingSystemPlaybackService(PlaybackTimePtr controller, std::vector<TrackingSystemServicePtr> base, ManualToolPtr manual)
27 {
28  mBases = base;
29  mState = Tool::tsNONE;
30  mController = controller;
31  mManual = manual;
32  mActiveToolId = QString();
33 }
34 
36 {
37  this->setState(Tool::tsNONE);
38 }
39 
40 void TrackingSystemPlaybackService::start()
41 {
42  if (!this->forceBaseToConfiguredState())
43  {
44  reportWarning("ToolManager must be configured before setting playback");
45  return;
46  }
47 
48  std::vector<ToolPtr> original;
49  for (unsigned i=0; i<mBases.size(); ++i)
50  {
51  std::vector<ToolPtr> original_one = mBases[i]->getTools();
52  std::copy(original_one.begin(), original_one.end(), std::back_inserter(original));
53  }
54 
55  std::pair<double,double> timeRange(getMilliSecondsSinceEpoch(), 0);
56 
57  for (unsigned i=0; i<original.size(); ++i)
58  {
59  cx::PlaybackToolPtr current(new PlaybackTool(original[i], mController));
60  connect(current.get(), &Tool::toolTransformAndTimestamp, this, &TrackingSystemPlaybackService::onToolPositionChanged);
61  mTools.push_back(current);
62 
63  TimedTransformMapPtr history = original[i]->getPositionHistory();
64  if (!history->empty())
65  {
66  timeRange.first = std::min(timeRange.first, history->begin()->first);
67  timeRange.second = std::max(timeRange.second, history->rbegin()->first);
68  }
69  }
70 
71  mController->initialize(QDateTime::fromMSecsSinceEpoch(timeRange.first), timeRange.second - timeRange.first);
72 
73  report("Opened Playback Mode");
74  mState = Tool::tsTRACKING;
75  emit stateChanged();
76 }
77 
78 void TrackingSystemPlaybackService::stop()
79 {
80  mTools.clear();
81  mState = Tool::tsNONE;
82  report("Closed Playback Mode");
83  emit stateChanged();
84 }
85 
86 bool TrackingSystemPlaybackService::forceBaseToConfiguredState()
87 {
88  bool atLeastOneConfigured = false;
89 
90  for (unsigned i=0; i<mBases.size(); ++i)
91  {
92  // attempt to configure tracker if not configured,
93  // or deinit/stop tracking if on.
94  if (mBases[i]->getState() != Tool::tsCONFIGURED)
95  {
96  mBases[i]->setState(Tool::tsCONFIGURED);
97  this->waitForState(mBases[i], Tool::tsCONFIGURED, 200);
98  }
99 
100  bool configured = (mBases[i]->getState() >= Tool::tsCONFIGURED);
101  atLeastOneConfigured = atLeastOneConfigured || configured;
102  }
103 
104  return atLeastOneConfigured;
105 }
106 
110 void TrackingSystemPlaybackService::waitForState(TrackingSystemServicePtr base, Tool::State state, int timeout)
111 {
112  int interval = 10;
113  for (unsigned i=0; i<timeout/interval; ++i)
114  {
115  if (base->getState() == state)
116  break;
117  qApp->processEvents();
118  sleep_ms(interval);
119  }
120 }
121 
123 {
124  std::vector<ToolPtr> retval;
125  std::copy(mTools.begin(), mTools.end(), back_inserter(retval));
126  return retval;
127 }
128 
130 {
131  return mState;
132 }
133 
134 bool TrackingSystemPlaybackService::isRunning() const
135 {
136  return !mTools.empty();
137 }
138 
140 {
141  mState = val;
142 
143  if (mState >= Tool::tsCONFIGURED)
144  {
145  if (!this->isRunning())
146  this->start();
147  }
148  else
149  {
150  if (this->isRunning())
151  this->stop();
152  }
153 }
154 
156 {
157 
158 }
159 
161 {
163 }
164 
165 void TrackingSystemPlaybackService::onToolPositionChanged(Transform3D matrix, double timestamp)
166 {
167  // Overwrite manual tool pos, set timestamp to 1ms previous.
168  // This makes sure manual tool is not picked as active.
169  if(mManual && (mManual->getUid() != mActiveToolId))
170  mManual->set_prMt(matrix, timestamp-1);
171 // mManual->setVisible(false);
172 // mManual->setVisible(true);
173 }
174 
176 {
177  mActiveToolId = uId;
178 }
179 
180 
181 
182 } // namespace cx
virtual void setLoggingFolder(QString loggingFolder)
virtual void setState(const Tool::State val)
asynchronously request a state. Wait for signal stateChanged()
Transform3D Transform3D
Transform3D is a representation of an affine 3D transform.
boost::shared_ptr< class PlaybackTool > PlaybackToolPtr
double getMilliSecondsSinceEpoch()
Definition: cxTime.cpp:44
void toolTransformAndTimestamp(Transform3D matrix, double timestamp)
boost::shared_ptr< class ManualTool > ManualToolPtr
configured with basic info
Definition: cxTool.h:75
virtual TrackerConfigurationPtr getConfiguration()
void stateChanged()
Reset time synchronization. Used for resetting time synchronization of incoming timestamps in OpenIGT...
boost::shared_ptr< class PlaybackTime > PlaybackTimePtr
boost::shared_ptr< class TrackerConfiguration > TrackerConfigurationPtr
void reportWarning(QString msg)
Definition: cxLogger.cpp:70
static TrackerConfigurationPtr getNullObject()
boost::shared_ptr< TimedTransformMap > TimedTransformMapPtr
Definition: cxTool.h:36
not available
Definition: cxTool.h:74
A tool used during playback.
void report(QString msg)
Definition: cxLogger.cpp:69
TrackingSystemPlaybackService(PlaybackTimePtr controller, std::vector< TrackingSystemServicePtr > base, ManualToolPtr manual)
emitting tracking data
Definition: cxTool.h:77
void sleep_ms(int ms)
Namespace for all CustusX production code.
boost::shared_ptr< class TrackingSystemService > TrackingSystemServicePtr