Fraxinus  16.5.0-fx-rc9
An IGT application
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
cxIGTLinkClientStreamer.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) 2008-2014, SINTEF Department of Medical Technology
5 All rights reserved.
6 
7 Redistribution and use in source and binary forms, with or without
8 modification, are permitted provided that the following conditions are met:
9 
10 1. Redistributions of source code must retain the above copyright notice,
11  this list of conditions and the following disclaimer.
12 
13 2. Redistributions in binary form must reproduce the above copyright notice,
14  this list of conditions and the following disclaimer in the documentation
15  and/or other materials provided with the distribution.
16 
17 3. Neither the name of the copyright holder nor the names of its contributors
18  may be used to endorse or promote products derived from this software
19  without specific prior written permission.
20 
21 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
22 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
23 IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
24 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
25 FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
26 DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
27 SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
28 CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
29 OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
30 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
31 =========================================================================*/
33 
34 #include <QTcpSocket>
35 #include "igtlOSUtil.h"
36 #include "igtlMessageHeader.h"
37 #include "igtlTransformMessage.h"
38 #include "igtlPositionMessage.h"
39 #include "igtlImageMessage.h"
40 #include "igtlClientSocket.h"
41 #include "igtlStatusMessage.h"
42 
43 #include "cxTypeConversions.h"
44 #include "cxLogger.h"
45 #include "cxIGTLinkConversion.h"
48 #include "cxCyclicActionLogger.h"
49 #include "cxUtilHelpers.h"
50 #include "cxTime.h"
51 #include "cxSender.h"
52 #include "vtkImageData.h"
53 
54 namespace cx
55 {
56 
58  mHeadingReceived(false),
59  mAddress(""),
60  mPort(0)
61 {
62 }
63 
65 {
66 
67 }
68 
69 void IGTLinkClientStreamer::setAddress(QString address, int port)
70 {
71  mAddress = address;
72  mPort = port;
73 }
74 
75 
77 {
78  mSender = sender;
79 // this->createSendTimer();
80 // mTestTimer = new QTimer(this);
81 // connect(mTestTimer, SIGNAL(timeout()), this, SLOT(myStreamSlot()));
82 // std::cout << "IGTLinkClientStreamer::startStreaming " << std::endl;
83 
84  // Establish Connection
85  mSocket.reset(new QTcpSocket());
86  connect(mSocket.get(), SIGNAL(readyRead()), this, SLOT(readyReadSlot()), Qt::DirectConnection);
87  connect(mSocket.get(), SIGNAL(hostFound()), this, SLOT(hostFoundSlot()), Qt::DirectConnection);
88  connect(mSocket.get(), SIGNAL(connected()), this, SLOT(connectedSlot()), Qt::DirectConnection);
89  connect(mSocket.get(), SIGNAL(disconnected()), this, SLOT(disconnectedSlot()), Qt::DirectConnection);
90  connect(mSocket.get(), SIGNAL(error(QAbstractSocket::SocketError)), this, SLOT(errorSlot(QAbstractSocket::SocketError)),
91  Qt::DirectConnection);
92 
93  if (!this->multipleTryConnectToHost())
94  {
95  reportError("IGTLinkClientStreamer: Failed to start streaming");
96  mSocket.reset();
97  return;
98  }
99 
100  // Create a message buffer to receive header
101  mHeaderMsg = igtl::MessageHeader::New();
102 }
103 
104 bool IGTLinkClientStreamer::multipleTryConnectToHost()
105 {
106  // hold here until all attempts are finished
107  int numberOfConnectionAttempts = 5;
108  int baseSleep = 300;
109  for (int i=0; i<numberOfConnectionAttempts; ++i)
110  {
111  if (i>0)
112  report(QString("[%2] Attempt %1 to connect to host").arg(i+1).arg(this->hostDescription()));
113  if (this->tryConnectToHost())
114  return true;
115  sleep_ms(baseSleep*(i+1));
116  }
117  reportError(QString("[%1] Timeout connecting to host").arg(this->hostDescription()));
118  return false;
119 }
120 
121 bool IGTLinkClientStreamer::tryConnectToHost()
122 {
123  mSocket->connectToHost(mAddress, mPort);
124 
125  int timeout = 5000;
126  if (!mSocket->waitForConnected(timeout))
127  {
128  mSocket->disconnectFromHost();
129  return false;
130  }
131  return true;
132 }
133 
135 {
136  if (mSocket)
137  {
138  mSocket->disconnectFromHost();
139  mSocket.reset();
140  }
141  mSender.reset();
142 }
143 
145 {
146  return (mSocket && mSocket->isValid());
147 }
148 
149 QString IGTLinkClientStreamer::hostDescription() const
150 {
151  return mAddress + ":" + qstring_cast(mPort);
152 }
153 
154 void IGTLinkClientStreamer::hostFoundSlot()
155 {
156  report(QString("[%1] Found host").arg(this->hostDescription()));
157 // report("Host found: " + this->hostDescription());
158 }
159 void IGTLinkClientStreamer::connectedSlot()
160 {
161  reportSuccess(QString("[%1] Connected to host").arg(this->hostDescription()));
162 }
163 void IGTLinkClientStreamer::disconnectedSlot()
164 {
165  report(QString("[%1] Disconnected from host").arg(this->hostDescription()));
166 // report("Disconnected from host " + this->hostDescription());
167 }
168 void IGTLinkClientStreamer::errorSlot(QAbstractSocket::SocketError socketError)
169 {
170  report(QString("[%1] Socket error [code=%2]: %3")
171  .arg(this->hostDescription())
172  .arg(QString::number(socketError))
173  .arg(mSocket->errorString()));
174 }
175 
176 void IGTLinkClientStreamer::readyReadSlot()
177 {
178  // read messages until one fails
179  while (this->readOneMessage());
180 }
181 
186 bool IGTLinkClientStreamer::readOneMessage()
187 {
188 
189 // std::cout << "tick " << std::endl;
190 
191  if (!mHeadingReceived)
192  {
193 // std::cout << "client::tick: received: " << mSocket->bytesAvailable() << ", head needed: " << mHeaderMsg->GetPackSize() << std::endl;
194  // Initialize receive buffer
195  mHeaderMsg->InitPack();
196 
197  // ignore if not enough data (yet)
198  if (mSocket->bytesAvailable() < mHeaderMsg->GetPackSize())
199  {
200  //std::cout << "Incomplete heading received, ignoring. " << std::endl;
201  //std::cout << "available: " << mSocket->bytesAvailable() << ", needed " << mHeaderMsg->GetPackSize() << std::endl;
202  return false;
203  }
204 
205  // after peek: read to increase pos
206  mSocket->read(reinterpret_cast<char*>(mHeaderMsg->GetPackPointer()), mHeaderMsg->GetPackSize());
207  mHeadingReceived = true;
208 
209  // Deserialize the header
210  mHeaderMsg->Unpack();
211  }
212 
213  if (mHeadingReceived)
214  {
215 // std::cout << "client::tick: received: " << mSocket->bytesAvailable() << ", body needed: " << mHeaderMsg->GetBodySizeToRead() << std::endl;
216  bool success = false;
217  // Check data type and receive data body
218 // if (QString(mHeaderMsg->GetDeviceType()) == "TRANSFORM")
219 // {
220 // ReceiveTransform(mSocket, mHeaderMsg);
221 // }
222 // else if (QString(mHeaderMsg->GetDeviceType() == "POSITION")
223 // {
224 // ReceivePosition(mSocket, mHeaderMsg);
225 // }
226  if (QString(mHeaderMsg->GetDeviceType()) == "IMAGE")
227  {
228  success = this->ReceiveImage(mSocket.get(), mHeaderMsg);
229  }
230  else if (QString(mHeaderMsg->GetDeviceType()) == "CX_US_ST")
231  {
232  success = this->ReceiveSonixStatus(mSocket.get(), mHeaderMsg);
233  }
234 // else if (QString(mHeaderMsg->GetDeviceType() == "STATUS")
235 // {
236 // ReceiveStatus(mSocket, mHeaderMsg);
237 // }
238  else
239  {
240  std::cerr << "Receiving : " << mHeaderMsg->GetDeviceType() << std::endl;
241  mSocket->read(mHeaderMsg->GetBodySizeToRead());
242  }
243 
244  if (success)
245  mHeadingReceived = false; // restart
246  else
247  return false;
248  }
249 // std::cout << " tock " << std::endl;
250  return true;
251 }
252 
253 bool IGTLinkClientStreamer::ReceiveSonixStatus(QTcpSocket* socket, igtl::MessageHeader::Pointer& header)
254 {
256  msg = IGTLinkUSStatusMessage::New();
257  msg->SetMessageHeader(header);
258  msg->AllocatePack();
259 
260  if (socket->bytesAvailable() < msg->GetPackBodySize())
261  {
262  //std::cout << "Incomplete body received, ignoring. " << std::endl;
263  return false;
264  }
265  socket->read(reinterpret_cast<char*>(msg->GetPackBodyPointer()), msg->GetPackBodySize());
266  // Deserialize the transform data
267  // If you want to do a CRC check, call Unpack(1).
268  // If you want to skip CRC check, call Unpack() without argument.
269  int c = msg->Unpack();
270  if (c & (igtl::MessageHeader::UNPACK_BODY | igtl::MessageHeader::UNPACK_UNDEF)) // if CRC check is OK or skipped
271  {
272  this->addToQueue(msg);
273 
274  return true;
275  }
276 
277  std::cout << "body crc failed!" << std::endl;
278  return true;
279 }
280 
281 namespace
282 {
283 QDateTime my_decode_timestamp(igtl::MessageBase* msg)
284 {
285  // get timestamp from igtl second-format:
286  igtl::TimeStamp::Pointer timestamp = igtl::TimeStamp::New();
287  msg->GetTimeStamp(timestamp);
288  double timestampMS = timestamp->GetTimeStamp() * 1000;
289  return QDateTime::fromMSecsSinceEpoch(timestampMS);
290 }
291 
292 void write_time_info(igtl::ImageMessage::Pointer imgMsg)
293 {
294  int kb = imgMsg->GetPackSize()/1024;
295 // CX_LOG_CHANNEL_DEBUG("igtl_rec_test") << "unpacked: , " << kb << " kByte, name=" << imgMsg->GetDeviceName();
296  QDateTime org_ts = my_decode_timestamp(imgMsg.GetPointer());
297  QDateTime now_ts = QDateTime::currentDateTime();
298  QString format = timestampMilliSecondsFormatNice();
299  CX_LOG_CHANNEL_DEBUG("igtl_rec_test") << "received " << kb << "kByte"
300  << ", time=(" << org_ts.toString(format) << "->" << now_ts.toString(format) << ")"
301  << ", lag=" << org_ts.msecsTo(now_ts) << "ms";
302 }
303 }
304 
305 bool IGTLinkClientStreamer::ReceiveImage(QTcpSocket* socket, igtl::MessageHeader::Pointer& header)
306 {
307  // Create a message buffer to receive transform data
308  igtl::ImageMessage::Pointer imgMsg = igtl::ImageMessage::New();
309  imgMsg->SetMessageHeader(header);
310  imgMsg->AllocatePack();
311 
312  // Receive transform data from the socket
313  // ignore if not enough data (yet)
314  if (socket->bytesAvailable() < imgMsg->GetPackBodySize())
315  {
316  //std::cout << "Incomplete body received, ignoring. " << std::endl;
317  return false;
318  }
319 
320  socket->read(reinterpret_cast<char*>(imgMsg->GetPackBodyPointer()), imgMsg->GetPackBodySize());
321  // Deserialize the transform data
322  // If you want to do a CRC check, call Unpack(1).
323  // If you want to skip CRC check, call Unpack() without argument.
324  int c = imgMsg->Unpack();
325 
326 // write_time_info(imgMsg);
327 
328  if (c & (igtl::MessageHeader::UNPACK_BODY | igtl::MessageHeader::UNPACK_UNDEF)) // if CRC check is OK or skipped
329  {
330  this->addToQueue(imgMsg);
331  return true;
332  }
333 
334  std::cout << "body crc failed!" << std::endl;
335  return true;
336 }
337 
338 void IGTLinkClientStreamer::addToQueue(IGTLinkUSStatusMessage::Pointer msg)
339 {
340  // set temporary, then assume the image adder will pass this message on.
341  mUnsentUSStatusMessage = msg;
342 }
343 
344 void IGTLinkClientStreamer::addToQueue(igtl::ImageMessage::Pointer msg)
345 {
346  IGTLinkConversion converter;
347  IGTLinkConversionImage imageconverter;
348  IGTLinkConversionSonixCXLegacy cxconverter;
349 
350  PackagePtr package(new Package());
351 
352  if (cxconverter.guessIsSonixLegacyFormat(msg->GetDeviceName()))
353  {
354  package->mImage = cxconverter.decode(msg);
355  }
356  else
357  {
358  package->mImage = imageconverter.decode(msg);
359  }
360 
361  // if us status not sent, do it here
362  if (mUnsentUSStatusMessage)
363  {
364  package->mProbe = converter.decode(mUnsentUSStatusMessage, msg, ProbeDefinitionPtr());
365 
366  if (cxconverter.guessIsSonixLegacyFormat(mUnsentUSStatusMessage->GetDeviceName()))
367  {
368  package->mProbe = cxconverter.decode(package->mProbe);
369  }
370 
371  mUnsentUSStatusMessage = IGTLinkUSStatusMessage::Pointer();
372  }
373 
374  //Should only be needed if time stamp is set on another computer that is
375  //not synched with the one running this code: e.g. The Ultrasonix scanner
376  mStreamSynchronizer.syncToCurrentTime(package->mImage);
377 
378  mSender->send(package);
379 }
380 
381 
382 } // namespace cx
383 
384 
QString qstring_cast(const T &val)
void reportError(QString msg)
Definition: cxLogger.cpp:92
#define CX_LOG_CHANNEL_DEBUG(channel)
Definition: cxLogger.h:122
void reportSuccess(QString msg)
Definition: cxLogger.cpp:93
boost::shared_ptr< struct Package > PackagePtr
void report(QString msg)
Definition: cxLogger.cpp:90
boost::shared_ptr< Sender > SenderPtr
Definition: cxSender.h:85
QString timestampMilliSecondsFormatNice()
Definition: cxTime.cpp:51
void sleep_ms(int ms)
boost::shared_ptr< class ProbeDefinition > ProbeDefinitionPtr