NorMIT-nav  16.5
An IGT application
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
cxPlusProtocol.cpp
Go to the documentation of this file.
1 #include "cxPlusProtocol.h"
2 
3 #include "cxIGTLinkConversion.h"
4 #include "cxLogger.h"
7 
8 namespace cx{
9 
11  mCalibrationKeyword("CalibrationTo"),
12  mProbeToTrackerName("ProbeToTracker"), //set in the PlusServer config file
13  mLastKnownOriginalTimestamp(-1),
14  mLastKnownLocalTimestamp(-1)
15 {
16  /* Rotation from igtl coordinate system to
17  * custusxs tool coordination system definition:
18  * 0 1 0 0
19  * 0 0 1 0
20  * 1 0 0 0
21  * 0 0 0 1
22  */
23  igtltool_M_custustool = Transform3D::Identity();
24  igtltool_M_custustool.matrix() = Eigen::Matrix4d::Constant(0);
25  igtltool_M_custustool(2, 0) = 1;
26  igtltool_M_custustool(0, 1) = 1;
27  igtltool_M_custustool(1, 2) = 1;
28  igtltool_M_custustool(3,3) = 1;
29 }
30 
31 QString PlusProtocol::getName() const
32 {
33  return "PlusServer";
34 }
35 
36 void PlusProtocol::translate(const igtl::TransformMessage::Pointer body)
37 {
38  CX_LOG_DEBUG() << "Transform incoming to plusprotocol";
39  QString deviceName = body->GetDeviceName();
40  this->registerTransformDeviceName(deviceName);
41 
42  IGTLinkConversion converter;
43  Transform3D matrix = converter.decode(body);
44 
45  if(this->isCalibration(deviceName))
46  {
47  Transform3D s_M_igtltool = matrix;
48  Transform3D s_M_custustool = s_M_igtltool * igtltool_M_custustool;
49  Transform3D sMt = s_M_custustool;
50  QString calibrationBelongsToDeviceName = this->findDeviceForCalibration(deviceName);
51  if(calibrationBelongsToDeviceName != "NOT_FOUND")
52  {
53  emit calibration(calibrationBelongsToDeviceName, sMt);
54  }
55  }
56  else
57  {
58  IGTLinkConversionBase baseConverter;
59  double timestamp_ms = baseConverter.decode_timestamp(body).toMSecsSinceEpoch();
60  timestamp_ms = this->getSyncedTimestampForTransformsAndImages(timestamp_ms);
61  Transform3D prMs = matrix;
62  emit transform(deviceName, prMs, timestamp_ms);
63  }
64 }
65 
66 void PlusProtocol::translate(const igtl::ImageMessage::Pointer body)
67 {
68  CX_LOG_DEBUG() << "Image incoming to plusprotocol";
69  //DIMENSION
70  int x = 0;
71  int y = 1;
72  int z = 2;
73 
74  //There seems to be a bug in the received images spacing from the plusserver
75  /*
76  float wrong_spacing[3];
77  body->GetSpacing(wrong_spacing);
78  float right_spacing[3];
79  right_spacing[x] = wrong_spacing[x];
80  right_spacing[y] = wrong_spacing[z];
81  right_spacing[z] = 1;
82  body->SetSpacing(right_spacing);
83  */
84 
85  int dimensions_p[3];
86  body->GetDimensions(dimensions_p);
87  float spacing[3];
88  body->GetSpacing(spacing);
89  int extent_p[3];
90  extent_p[x] = dimensions_p[x]-1;
91  extent_p[y] = dimensions_p[y]-1;
92  extent_p[z] = dimensions_p[z]-1;
93 
94  //IMAGE
95  IGTLinkConversion converter;
96  ImagePtr theImage = converter.decode(body);
97 
98  IGTLinkConversionBase baseConverter;
99  double timestamp_ms = baseConverter.decode_timestamp(body).toMSecsSinceEpoch();
100  timestamp_ms = this->getSyncedTimestampForTransformsAndImages(timestamp_ms);
101  theImage->setAcquisitionTime(QDateTime::fromMSecsSinceEpoch(timestamp_ms));
102  emit image(theImage);
103 
104  //PROBEDEFINITION
105  ProbeDefinitionPtr definition(new ProbeDefinition);
106  definition->setUseDigitalVideo(true);
107  definition->setType(ProbeDefinition::tLINEAR);
108  definition->setSpacing(Vector3D(spacing[x], spacing[y], spacing[z]));
109  definition->setSize(QSize(dimensions_p[x], dimensions_p[y]));
110  definition->setOrigin_p(Vector3D(dimensions_p[x]/2, 0, 0));
111  double depthstart_mm = 0;
112  double depthend_mm = extent_p[y]*spacing[y];
113  double width_mm = extent_p[x]*spacing[x];
114  definition->setSector(depthstart_mm, depthend_mm, width_mm);
115  definition->setClipRect_p(DoubleBoundingBox3D(0, extent_p[x], 0, extent_p[y], 0, extent_p[z]));
116 
117  emit probedefinition(mProbeToTrackerName, definition);
118 }
119 
120 void PlusProtocol::translate(const igtl::StringMessage::Pointer body)
121 {
122  IGTLinkConversion converter;
123  QString string = converter.decode(body);
124  if(string.contains("CommandReply", Qt::CaseInsensitive))
126  //else
127  // CX_LOG_CHANNEL_DEBUG(CX_OPENIGTLINK_CHANNEL_NAME) << string;
128 }
129 
130 double PlusProtocol::getCurrentTimestamp() const
131 {
132  double current_timestamp_ms = QDateTime::currentDateTime().toMSecsSinceEpoch();
133  return current_timestamp_ms;
134 }
135 double PlusProtocol::getSyncedTimestampForTransformsAndImages(double currentOriginalTimestamp)
136 {
137  double retval = 0;
138 
139  if(currentOriginalTimestamp != mLastKnownOriginalTimestamp)
140  {
141  double currentLocalTimestamp = this->getCurrentTimestamp();
142 
144  {
145  mStreamSynchronizer->addTimestamp(currentOriginalTimestamp);
146  currentLocalTimestamp = currentOriginalTimestamp + mStreamSynchronizer->getShift();
147  }
148 
149  mLastKnownOriginalTimestamp = currentOriginalTimestamp;
150  mLastKnownLocalTimestamp = currentLocalTimestamp;
151  }
152  retval = mLastKnownLocalTimestamp;
153  return retval;
154 }
155 
156 void PlusProtocol::registerTransformDeviceName(QString deviceName)
157 {
158  if(!mKnownTransformDeviceNames.contains(deviceName))
159  mKnownTransformDeviceNames << deviceName;
160 }
161 
162 bool PlusProtocol::isCalibration(QString deviceName) const
163 {
164  if(deviceName.contains(mCalibrationKeyword))
165  return true;
166  else
167  return false;
168 }
169 
170 QString PlusProtocol::findDeviceForCalibration(QString calibrationDeviceName) const
171 {
172  QString partialDeviceName = this->extractDeviceNameFromCalibrationDeviceName(calibrationDeviceName);
173  QString calibrationBelongsToDeviceName = this->findRegisteredTransformDeviceNameThatContains(partialDeviceName);
174  return calibrationBelongsToDeviceName;
175 }
176 
177 QString PlusProtocol::extractDeviceNameFromCalibrationDeviceName(QString calibrationDeviceName) const
178 {
179  QString retval = calibrationDeviceName.remove(mCalibrationKeyword);
180  return retval;
181 }
182 
183 QString PlusProtocol::findRegisteredTransformDeviceNameThatContains(QString deviceName) const
184 {
185  QString retval("NOT_FOUND");
186  deviceName.remove("X");
187  QRegExp rx(deviceName+"*");
188  rx.setPatternSyntax(QRegExp::Wildcard);
189  int foundAtIndex = mKnownTransformDeviceNames.indexOf(rx);
190  if(foundAtIndex != -1)
191  retval = mKnownTransformDeviceNames[foundAtIndex];
192 
193  return retval;
194 
195 }
196 } //namespace cx
#define CX_LOG_CHANNEL_INFO(channel)
Definition: cxLogger.h:123
void calibration(QString devicename, Transform3D calibration)
Transform3D Transform3D
Transform3D is a representation of an affine 3D transform.
void transform(QString devicename, Transform3D transform, double timestamp)
US beam is emitted straight forward.
boost::shared_ptr< class Image > ImagePtr
Definition: cxDicomWidget.h:48
void image(ImagePtr image)
virtual QString getName() const
StreamedTimestampSynchronizerPtr mStreamSynchronizer
Definition: cxProtocol.h:85
Representation of a floating-point bounding box in 3D. The data are stored as {xmin,xmax,ymin,ymax,zmin,zmax}, in order to simplify communication with vtk.
Eigen::Vector3d Vector3D
Vector3D is a representation of a point or vector in 3D.
Definition: cxVector3D.h:63
Definition of characteristics for an Ultrasound Probe Sector.
#define CX_LOG_DEBUG
Definition: cxLogger.h:110
void probedefinition(QString devicename, ProbeDefinitionPtr definition)
virtual void translate(const igtl::TransformMessage::Pointer body)
boost::shared_ptr< class ProbeDefinition > ProbeDefinitionPtr