Fraxinus  16.5.0-fx-rc1
An IGT application
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
cxOpenIGTLinkProtocol.cpp
Go to the documentation of this file.
2 
3 #include <QMutexLocker>
4 #include "cxLogger.h"
7 #include "cxIGTLinkConversion.h"
10 
11 namespace cx
12 {
13 
15  mHeader(igtl::MessageHeader::New()),
16  mBody(igtl::MessageBase::New())
17 {
18  this->getReadyToReceiveHeader();
19 }
20 
22 {
23  return true;
24 }
25 
27 {
28 // QMutexLocker locker(&mReadyReadMutex);
29 // QMutexLocker lock(&mPackMutex);
30  return mReadyToReceive && mPack->isFinishedWith();
31 }
32 
34 {
35  IGTLinkConversionImage imageConverter;
36  igtl::ImageMessage::Pointer msg = imageConverter.encode(image, this->coordinateSystem());
37  msg->Pack();
38 
40 }
41 
43 {
44  IGTLinkConversionPolyData polyConverter;
45  igtl::PolyDataMessage::Pointer msg = polyConverter.encode(data, this->coordinateSystem());
46  msg->Pack();
47 
49 }
50 
51 void OpenIGTLinkProtocol::translate(const igtl::MessageHeader::Pointer &header, const igtl::MessageBase::Pointer &body)
52 {
53  //CX_LOG_DEBUG() << "Incoming message to OpenIGTLinkProtocol";
54  QString type = QString(header->GetDeviceType()).toUpper();
55  if (type=="TRANSFORM")
56  {
57  const igtl::TransformMessage::Pointer temp = dynamic_cast<igtl::TransformMessage*>(body.GetPointer());
58  this->translate(temp);
59  }
60  else if (type=="POLYDATA")
61  {
62  const igtl::PolyDataMessage::Pointer temp = dynamic_cast<igtl::PolyDataMessage*>(body.GetPointer());
63  this->translate(temp);
64  }
65  else if (type=="IMAGE")
66  {
67 // //----- CustusX openigtlink server -----
68 // //there is a special kind of image package coming from custusx
69 // //server where crc is set to 0.
70 // QString name(header->GetDeviceName());
71 // if(name.contains("Sonix", Qt::CaseInsensitive))
72 // {
73 // const IGTLinkImageMessage::Pointer temp = dynamic_cast<IGTLinkImageMessage*>(body.GetPointer());
74 // this->translate(temp);
75 // }
76 // //----------
77 // else
78 // {
79  const igtl::ImageMessage::Pointer temp = dynamic_cast<igtl::ImageMessage*>(body.GetPointer());
80  this->translate(temp);
81 // }
82  }
83  else if (type=="STATUS")
84  {
85  const igtl::StatusMessage::Pointer temp = dynamic_cast<igtl::StatusMessage*>(body.GetPointer());
86  this->translate(temp);
87  }
88  else if (type=="STRING")
89  {
90  const igtl::StringMessage::Pointer temp = dynamic_cast<igtl::StringMessage*>(body.GetPointer());
91  this->translate(temp);
92  }
93  else if (type=="CX_US_ST")
94  {
95  const IGTLinkUSStatusMessage::Pointer temp = dynamic_cast<IGTLinkUSStatusMessage*>(body.GetPointer());
96  this->translate(temp);
97  }
98  else
99  {
100  this->writeNotSupportedMessage(body);
101  }
102 }
103 
104 void OpenIGTLinkProtocol::translate(const igtl::TransformMessage::Pointer body)
105 {
106  //CX_LOG_DEBUG() << "Transform incoming to OpenIGTLinkProtocol";
107  QString deviceName = body->GetDeviceName();
108 
109  IGTLinkConversion converter;
110  Transform3D prMs = converter.decode(body);
111 
112  IGTLinkConversionBase baseConverter;
113  QDateTime timestamp = baseConverter.decode_timestamp(body);
114 
116  {
117  mStreamSynchronizer->addTimestamp(timestamp);
118  timestamp = timestamp.addMSecs(mStreamSynchronizer->getShift());
119  }
120 
121  emit transform(deviceName, prMs, timestamp.toMSecsSinceEpoch());
122 }
123 
124 void OpenIGTLinkProtocol::translate(const igtl::PolyDataMessage::Pointer body)
125 {
126  this->writeAcceptingMessage(body);
127 
128  IGTLinkConversionPolyData polyConverter;
129  MeshPtr retval = polyConverter.decode(body, this->coordinateSystem());
130  emit mesh(retval);
131 }
132 
133 void OpenIGTLinkProtocol::translate(const igtl::ImageMessage::Pointer body)
134 {
135  this->writeAcceptingMessage(body);
136 
137  IGTLinkConversionImage imageConverter;
138  ImagePtr retval = imageConverter.decode(body);
139 
141  mStreamSynchronizer->syncToCurrentTime(retval);
142 
143  emit image(retval);
144 }
145 
146 void OpenIGTLinkProtocol::translate(const igtl::StatusMessage::Pointer body)
147 {
148  IGTLinkConversion converter;
149  QString status = converter.decode(body);
150  CX_LOG_CHANNEL_VOLATILE(CX_OPENIGTLINK_CHANNEL_NAME) << "IGTL status: " << status;
151 }
152 
153 void OpenIGTLinkProtocol::translate(igtl::StringMessage::Pointer body)
154 {
155  IGTLinkConversion converter;
156  QString string = converter.decode(body);
157  //This was spamming the console
158  CX_LOG_CHANNEL_INFO(CX_OPENIGTLINK_CHANNEL_NAME) << "IGTL string: " << string;
159 }
160 
162 {
163  this->writeNotSupportedMessage(body);
164 }
165 
166 void OpenIGTLinkProtocol::writeNotSupportedMessage(igtl::MessageBase* base) const
167 {
168  QString dtype(base->GetDeviceType());
169  QString dname(base->GetDeviceName());
170  CX_LOG_CHANNEL_INFO(CX_OPENIGTLINK_CHANNEL_NAME) << QString("Ignoring incoming igtlink message (%1, %2): ")
171  .arg(dtype)
172  .arg(dname);
173 }
174 
175 void OpenIGTLinkProtocol::writeAcceptingMessage(igtl::MessageBase* body) const
176 {
177  QString dtype(body->GetDeviceType());
178  QString dname(body->GetDeviceName());
179  CX_LOG_CHANNEL_DEBUG(CX_OPENIGTLINK_CHANNEL_NAME) << QString("Accepting incoming igtlink message (%1, %2): ")
180  .arg(dtype)
181  .arg(dname);
182 }
183 
184 void OpenIGTLinkProtocol::getReadyToReceiveBody()
185 {
186  //CX_LOG_DEBUG() << "getting ready to receive BODY";
187  this->prepareBody(mHeader, mBody);
188  //CX_LOG_DEBUG() << "mBody->pointer " << mBody->GetPackBodyPointer() << " size " << mBody->GetPackBodySize();
189  this->prepareBodyPack(mBody);
190  this->setReadyToReceive(true);
191 }
192 
193 void OpenIGTLinkProtocol::getReadyToReceiveHeader()
194 {
195  //CX_LOG_DEBUG() << "getting ready to receive HEADER";
196  this->prepareHeader(mHeader);
197  this->prepareHeaderPack(mHeader);
198  this->setReadyToReceive(true);
199 }
200 
201 bool OpenIGTLinkProtocol::isValid(const igtl::MessageBase::Pointer &msg) const
202 {
203  if(this->isSupportedBodyType(msg->GetDeviceType()))
204  return true;
205  else
206  return false;
207 }
208 
209 bool OpenIGTLinkProtocol::isSupportedBodyType(QString type) const
210 {
211  QStringList supportedTypes;
212  supportedTypes << "TRANSFORM";
213  supportedTypes << "POLYDATA";
214  supportedTypes << "IMAGE";
215  supportedTypes << "STATUS";
216  supportedTypes << "STRING";
217  supportedTypes << "CX_US_ST";
218 
219  if(supportedTypes.contains(type, Qt::CaseInsensitive))
220  return true;
221  else
222  return false;
223 
224 }
225 
227 {
228  //creates deadlock....
229  //QMutexLocker lock(&mPackMutex);
230 
231  //CX_LOG_DEBUG() << "1 processPack ";
232  this->setReadyToReceive(false);
233  //CX_LOG_DEBUG() << "2 processPack ";
234  if(mPack->data()->size == mHeader->GetPackSize())
235  {
236  this->unpackHeader(mHeader);
237  CX_LOG_DEBUG() << "HEADER: " << mHeader->GetDeviceType();
238  if(this->isValid(mHeader))
239  this->getReadyToReceiveBody();
240  else
241  this->getReadyToReceiveHeader();
242  }
243  else
244  {
245  this->unpackBody(mBody);
246  CX_LOG_DEBUG() << "BODY: " << mBody->GetDeviceType();
247  if(this->isValid(mBody))
248  this->getReadyToReceiveHeader();
249  else
250  this->getReadyToReceiveBody();
251  }
252 
253 }
254 
255 void OpenIGTLinkProtocol::setReadyToReceive(bool ready)
256 {
257  //CX_LOG_DEBUG() << (ready ? "Is " : "NOT ") << "ready to receive";
258  mReadyToReceive = ready;
259 
260 }
261 
262 void OpenIGTLinkProtocol::prepareHeaderPack(igtl::MessageHeader::Pointer &message)
263 {
264  if(mPack)
267  connect(mPack.get(), &EncodedPackage::dataArrived, this, &OpenIGTLinkProtocol::processPack);
268 }
269 
270 void OpenIGTLinkProtocol::prepareBodyPack(igtl::MessageBase::Pointer &message)
271 {
272  if(mPack)
273  disconnect(mPack.get(), &EncodedPackage::dataArrived, this, &OpenIGTLinkProtocol::processPack);
274  mPack = igtlEncodedBodyPackage::create(message);
275  connect(mPack.get(), &EncodedPackage::dataArrived, this, &OpenIGTLinkProtocol::processPack);
276 }
277 
278 void OpenIGTLinkProtocol::prepareHeader(const igtl::MessageHeader::Pointer &header) const
279 {
280  header->InitPack();
281  //CX_LOG_DEBUG() << "( ) prepareHeader, pointer: " << header->GetPackPointer() << " size: " << header->GetPackSize();
282  //CX_LOG_DEBUG() << "(BODY) prepareHeader, pointer: " << header->GetPackBodyPointer() << " size: " << header->GetPackBodySize();
283 }
284 
285 void OpenIGTLinkProtocol::prepareBody(const igtl::MessageHeader::Pointer &header, igtl::MessageBase::Pointer &body)
286 {
287  QString type = QString(header->GetDeviceType()).toUpper();
288  if (type=="TRANSFORM")
289  {
290  this->prepareBody<igtl::TransformMessage>(header, body);
291  }
292  else if (type=="POLYDATA")
293  {
294  this->prepareBody<igtl::PolyDataMessage>(header, body);
295  }
296  else if (type=="IMAGE")
297  {
298  this->prepareBody<igtl::ImageMessage>(header, body);
299  }
300  else if (type=="STATUS")
301  {
302  this->prepareBody<igtl::StatusMessage>(header, body);
303  }
304  else if (type=="STRING")
305  {
306  this->prepareBody<igtl::StringMessage>(header, body);
307  }
308  else if (type=="CX_US_ST")
309  {
310  this->prepareBody<IGTLinkUSStatusMessage>(header, body);
311  }
312  else
313  {
314  this->writeNotSupportedMessage(header);
315  }
316 }
317 
318 bool OpenIGTLinkProtocol::unpackHeader(const igtl::MessageHeader::Pointer &header) const
319 {
320  int doCRCOnHeader = 1;
321  int c = header->Unpack(doCRCOnHeader);
322  if (c & igtl::MessageHeader::UNPACK_HEADER)
323  {
324  //CX_LOG_DEBUG() << "unpacked header of type " << header->GetDeviceType();
325  return true;
326  }
327  else
328  {
329  CX_LOG_CHANNEL_ERROR(CX_OPENIGTLINK_CHANNEL_NAME) << "Could not unpack header";
330  return false;
331  }
332 }
333 
334 
335 template <typename T>
336 void OpenIGTLinkProtocol::prepareBody(const igtl::MessageHeader::Pointer &header, igtl::MessageBase::Pointer &body)
337 {
338  body = T::New();
339  body->SetMessageHeader(header);
340  body->AllocatePack();
341  //CX_LOG_DEBUG() << "( ) prepareBody, pointer: " << body->GetPackPointer() << " size: " << body->GetPackSize();
342  //CX_LOG_DEBUG() << "(BODY) prepareBody, pointer: " << body->GetPackBodyPointer() << " size: " << body->GetPackBodySize();
343 }
344 
345 bool OpenIGTLinkProtocol::unpackBody(const igtl::MessageBase::Pointer &body)
346 {
347  int c = body->Unpack(this->doCRC());
348  if (c & igtl::MessageHeader::UNPACK_BODY)
349  {
350  this->translate(mHeader, body);
351  }
352  else
353  {
354  CX_LOG_CHANNEL_ERROR(CX_OPENIGTLINK_CHANNEL_NAME) << "Could not unpack the body of type: " << body->GetDeviceType();
355  return false;
356  }
357  return true;
358 }
359 } //namespace cx
360 
#define CX_LOG_CHANNEL_INFO(channel)
Definition: cxLogger.h:123
Transform3D Transform3D
Transform3D is a representation of an affine 3D transform.
void mesh(MeshPtr mesh)
static EncodedPackagePtr create(igtl::MessageBase::Pointer msg)
void transform(QString devicename, Transform3D transform, double timestamp)
EncodedPackagePtr mPack
Definition: cxProtocol.h:84
boost::shared_ptr< class Image > ImagePtr
Definition: cxDicomWidget.h:48
boost::shared_ptr< EncodedPackage > EncodedPackagePtr
void image(ImagePtr image)
#define CX_LOG_CHANNEL_VOLATILE(channel)
Definition: cxLogger.h:127
StreamedTimestampSynchronizerPtr mStreamSynchronizer
Definition: cxProtocol.h:85
#define CX_LOG_CHANNEL_DEBUG(channel)
Definition: cxLogger.h:122
#define CX_LOG_DEBUG
Definition: cxLogger.h:110
static EncodedPackagePtr create(typename TYPE::Pointer msg)
boost::shared_ptr< class Mesh > MeshPtr
#define CX_LOG_CHANNEL_ERROR(channel)
Definition: cxLogger.h:126