NorMIT-nav  16.5
An IGT application
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
cxImageStreamerSonix.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 =========================================================================*/
32 
33 #include "cxImageStreamerSonix.h"
34 
35 #include "cxVector3D.h"
36 
37 #ifdef CX_WIN32
38 
39 #include <QTimer>
40 #include <QTime>
41 #include <QHostAddress>
42 #include "igtlOSUtil.h"
43 #include "igtlImageMessage.h"
44 #include "igtlServerSocket.h"
45 #include "vtkImageData.h"
46 #include "vtkSmartPointer.h"
47 #include "vtkMetaImageReader.h"
48 #include "vtkImageImport.h"
49 #include "vtkLookupTable.h"
50 #include "vtkImageMapToColors.h"
51 #include "vtkMetaImageWriter.h"
52 #include "cxSonixProbeFileReader.h"
53 
54 #include "vtkSonixVideoSource.h"
55 
56 #include "cxDataLocations.h"
57 #include "cxSonixProbeFileReader.h"
58 //#include "cxTypeConversions.h"
59 
60 namespace cx
61 {
62 
63 ImageStreamerSonix::ImageStreamerSonix() :
64  mEmitStatusMessage(false),
65  mLastFrameTimestamp(0.0),
66  mCurrentFrameTimestamp(0.0),
67  mMaxqueueInfo(20),
68  mDroppedImages(0),
69  mSonixHelper(new SonixHelper())
70 {
71  mSendTimer = new QTimer;
72 }
73 
74 QString ImageStreamerSonix::getType()
75 {
76  return "Sonix";
77 }
78 
79 QStringList ImageStreamerSonix::getArgumentDescription()
80 {
81  QStringList retval;
82  retval << "--ipaddress: IP address to connect to, default=127.0.0.1 (localhost)";
83  retval << "--imagingmode: default=2 (0 = B-mode, 2 = Color)";//, 6 = Dual, 12 = RF)";
84  retval << "--datatype: Video type, default=0x008 (4 = gray, 8 = color)";
85  retval << "--buffersize: Grabber buffer size, default=500";
86  retval << "--properties: dump image properties";
87  retval << "--debug: Debug output";
88  retval << "--delay: Delay (sec) before connecting to Sonix (first time), default=80";
89  return retval;
90 }
91 
92 ImageStreamerSonix::~ImageStreamerSonix()
93 {
94  mSendTimer->stop();
95  if (mSonixGrabber)
96  {
97  mSonixGrabber->Stop();
98  std::cout << "Releasing Ultrasonix resources" << std::endl;
99  mSonixGrabber->ReleaseSystemResources();
100  }
101 }
102 
103 
104 void ImageStreamerSonix::initialize(StringMap arguments)
105 {
106  std::cout << "Creating sender type Sonix" << std::endl;
107 
109 
110 // mMaxBufferSize = 19200000; //800(width)*600(height)*4(bytes)*10(images)
111 
112  typedef cx::Frame Frame;
113  qRegisterMetaType<Frame>("Frame");
114 
115  connect(this, SIGNAL(imageOnQueue(int)), this, SLOT(sendOpenIGTLinkImageSlot(int)), Qt::QueuedConnection);
116  connect(this, SIGNAL(statusOnQueue(int)), this, SLOT(sendOpenIGTLinkStatusSlot(int)), Qt::QueuedConnection);//Do not work yet
117 
118  connect(mSendTimer, SIGNAL(timeout()), this, SLOT(initializeSonixSlot()));
119  this->setSendInterval(10000);
120  mSendTimer->setInterval(getSendInterval());
121  mSendTimer->start();
122 
123  this->initializeSonixGrabber();
124 }
125 
126 void ImageStreamerSonix::initializeSonixSlot()
127 {
128  if(!mSonixGrabber->IsInitialized())
129  {
130  mSonixGrabber->Initialize();
131  return;
132  }
133 
134  //Don't reinitialize if in freeze state
135  if(!mSonixGrabber->getFreezeState() && similar(mLastFrameTimestamp, mCurrentFrameTimestamp, 0.001))
136  {
137  std::cout << "initializeSonixSlot() Got no new frame. Reinitializing..." << std::endl;
138  // If sonix exam is closed we need to create a new mSonixGrabber
139  // Free resources. Do we need to delete?
140  mSonixGrabber->Stop();
141  std::cout << "Releasing Ultrasonix resources" << std::endl;
142  mSonixGrabber->ReleaseSystemResources();
143  disconnect(mSonixHelper, SIGNAL(frame(Frame&)), this, SLOT(receiveFrameSlot(Frame&)));
144 
145  this->initializeSonixGrabber();
146  }
147  else
148  {
149  mLastFrameTimestamp = mCurrentFrameTimestamp;
150  }
151 }
152 
153 void ImageStreamerSonix::initializeSonixGrabber()
154 {
155  if (!mArguments.count("ipaddress"))
156  mArguments["ipaddress"] = "127.0.0.1";
157  if (!mArguments.count("imagingmode"))
158  mArguments["imagingmode"] = "0";
159  if (!mArguments.count("datatype"))
160  mArguments["datatype"] = "0x00000008";
161  if (!mArguments.count("buffersize"))
162  mArguments["buffersize"] = "500";
163  if (!mArguments.count("delay"))
164  mArguments["buffersize"] = "80";
165 
166  QString ipaddress = mArguments["ipaddress"];
167  int imagingMode = convertStringWithDefault(mArguments["imagingmode"], 0);
168  int acquisitionDataType = convertStringWithDefault(mArguments["datatype"], 0x00000008);
169  int bufferSize = convertStringWithDefault(mArguments["buffersize"], 500);
170  int delay = convertStringWithDefault(mArguments["delay"], 80);
171  bool useSharedMemory = false;
172  if (ipaddress.contains("127.0.0.1"))
173  useSharedMemory = true;
174 
175  mSonixGrabber = vtkSonixVideoSource::New();
176  if (mArguments.count("debug"))
177  mSonixGrabber->setDebugOutput(true);
178  else
179  mSonixGrabber->setDebugOutput(false);
180  mSonixGrabber->setSonixConnectionDelay(delay);
181  mSonixGrabber->SetSonixIP(ipaddress.toStdString().c_str());
182  mSonixGrabber->SetImagingMode(imagingMode);
183  mSonixGrabber->SetAcquisitionDataType(acquisitionDataType);
184  mSonixGrabber->SetFrameBufferSize(bufferSize); // Number of image frames in buffer
185  mSonixGrabber->setUseSharedMemory(useSharedMemory);
186  mSonixGrabber->Initialize(); // Run initialize to set spacing and offset
187 
188  //TODO move to debug function OR DELETE!!! <jbake>
189  //std::cout << "imagingMode: " << imagingMode << std::endl;
190  //std::cout << "datatype: " << mArguments["datatype"].toStdString().c_str() << std::endl;
191  //std::cout << "acquisitionDataType: " << acquisitionDataType << " ";
192  //std::cout << "GetAcquisitionDataType: " << mSonixGrabber->GetAcquisitionDataType() << std::endl;
193 
194  mSonixGrabber->setSonixHelper(this->mSonixHelper);
195  connect(mSonixHelper, SIGNAL(frame(Frame&)), this, SLOT(receiveFrameSlot(Frame&)), Qt::DirectConnection);
196 }
197 
198 bool ImageStreamerSonix::startStreaming(SenderPtr sender)
199 {
200  mSender = sender;
201  mSonixGrabber->Record();
202  mEmitStatusMessage = true;
203  std::cout << "Started streaming from sonix device" << std::endl;
204  return true;
205 }
206 
207 void ImageStreamerSonix::stopStreaming()
208 {
209  mSonixGrabber->Stop();
210  mSender.reset();
211 }
212 
213 void ImageStreamerSonix::receiveFrameSlot(Frame& frame)
214 {
215  mCurrentFrameTimestamp = frame.mTimestamp;
216 
217  if (!mSender || !mSender->isReady())
218  return;
219 
220  //TODO: Get info like origin from frame and create a IGTLinkUSStatusMessage
221  if (frame.mNewStatus || mEmitStatusMessage)
222  {
223  IGTLinkUSStatusMessage::Pointer statMsg = getFrameStatus(frame);
224  //double spacing[3];
225  //statMsg->GetSpacing(spacing);
226  //std::cout << "Spacing3: " << spacing[0] << ", " << spacing[1] << ", " << spacing[2] << std::endl;
227  this->addStatusMessageToQueue(statMsg);
228  // Pack (serialize) and send
229 // statMsg->Pack();
230 // mSocket->write(reinterpret_cast<const char*>(statMsg->GetPackPointer()), statMsg->GetPackSize());
231  mEmitStatusMessage = false;
232  }
233 
234  IGTLinkImageMessage::Pointer imgMsg = convertFrame(frame);
235 // std::cout << "Socket bytesToWrite: " << mSocket->bytesToWrite() << std::endl;
236 // std::cout << "Socket readBufferSize: " << mSocket->readBufferSize() << std::endl;
237  this->addImageToQueue(imgMsg);
238 
239 
240  //------------------------------------------------------------
241  // Pack (serialize) and send
242 // imgMsg->Pack();
243 // mSocket->write(reinterpret_cast<const char*>(imgMsg->GetPackPointer()), imgMsg->GetPackSize());
244 }
245 
246 IGTLinkUSStatusMessage::Pointer ImageStreamerSonix::getFrameStatus(Frame& frame)
247 {
248  IGTLinkUSStatusMessage::Pointer retval = IGTLinkUSStatusMessage::New();
249 
250  retval->SetOrigin(frame.mOrigin[0], frame.mOrigin[1], 0);
251 
252  //std::cout << "Received probe name: " << frame.probeName << std::endl;
253 
254  //Path to probes.xml on on ultrasonix scanner:
255  QString probeFile = "C:/Program Files/Ultrasonix/Exam/config/probes.xml";
256  cx::SonixProbeFileReader reader(probeFile);
257  if(!reader.init())
258  {
259  std::cout << "Failed to initialize probe xml reader" << std::endl;
260  return retval;
261  }
262  QDomNode probeNode = reader.getProbeNode(frame.probeName.c_str());
263  long radius = reader.getProbeParam(probeNode, "radius") / 1000; //microns -> mm
264  long probeLenght = reader.getProbeLenght(probeNode) / 1000; //microns -> mm
265  double probeWidth;
266 
267  if(frame.mSectorSizeInPercent <= 100)
268  {
269  double widthReductionFactor = 0.98;
270  probeWidth = probeLenght * frame.mSectorSizeInPercent / 100.0 * widthReductionFactor;
271  }
272  else
273  probeWidth = probeLenght;
274 
275  if(reader.isProbeLinear(probeNode))
276  {
277  retval->SetProbeType(2);
278  retval->SetDepthStart((frame.uly-frame.mOrigin[1]) * frame.mSpacing[1]);// Start of sector in mm from origin
279  retval->SetDepthEnd((frame.bly-frame.mOrigin[1]) * frame.mSpacing[1]); // End of sector in mm from origin
280  retval->SetWidth(probeWidth);
281  }
282  else // sector
283  {
284  retval->SetProbeType(1);
285  retval->SetWidth(probeWidth/radius);
286  double depthStart = radius;
287  double depthStartIncreaseFactor = 1.01;
288  double depthEndReductionFactor = 0.99;
289  retval->SetDepthStart(depthStart * depthStartIncreaseFactor);
290  retval->SetDepthEnd((depthStart + frame.mImagingDepth) * depthEndReductionFactor);
291  }
292 
293  retval->SetDeviceName(createDeviceName().c_str());
294  return retval;
295 }
296 
297 std::string ImageStreamerSonix::createDeviceName()
298 {
299  std::string retval = "Sonix[BGRX]";
300  return retval;
301 }
302 
303 IGTLinkImageMessage::Pointer ImageStreamerSonix::convertFrame(Frame& frame)
304 {
305  IGTLinkImageMessage::Pointer retval = IGTLinkImageMessage::New();
306 
307  //extract data needed variables from the frame
308  int size[] = {frame.mWidth, frame.mHeight, 1};
309  int offset[] = {0, 0, 0};
310 
311  // Create a new IMAGE type message
312  retval->SetDimensions(size);
313  retval->SetSpacing(frame.mSpacing[0], frame.mSpacing[1],1);
314  //std::cout << "Frame spacing: " << frame.mSpacing[0] << " " << frame.mSpacing[1] << std::endl;
315  retval->SetScalarType(frame.mPixelFormat); //Use frame.mPixelFormat directly
316  retval->SetDeviceName(createDeviceName().c_str());
317 
318  retval->SetSubVolume(size,offset);
319  retval->AllocateScalars();
320 
321  igtl::TimeStamp::Pointer ts;
322  ts = igtl::TimeStamp::New();
323  double seconds = frame.mTimestamp;
324  ts->SetTime(seconds); //in seconds
325  retval->SetTimeStamp(ts);
326 
327  igtl::Matrix4x4 matrix;
328  matrix[0][0] = 1.0; matrix[1][0] = 0.0; matrix[2][0] = 0.0; matrix[3][0] = 0.0;
329  matrix[0][1] = 0.0; matrix[1][1] = 1.0; matrix[2][1] = 0.0; matrix[3][1] = 0.0;
330  matrix[0][2] = 0.0; matrix[1][2] = 0.0; matrix[2][2] = 1.0; matrix[3][2] = 0.0;
331  matrix[0][3] = 0.0; matrix[1][3] = 0.0; matrix[2][3] = 0.0; matrix[3][3] = 1.0;
332  retval->SetMatrix(matrix);
333 
334  retval->SetOrigin(frame.mOrigin[0], frame.mOrigin[1], 0);//Set origin after frame is set
335  // Set image data
336  int fsize = retval->GetImageSize();
337  memcpy(retval->GetScalarPointer(), frame.mFirstPixel, fsize); // not sure if we need to copy
338 
339  //float origin[3];
340  //retval->GetOrigin(origin);
341  //std::cout << "origin3: " << origin[0] << " " << origin[1] << " " << origin[2] << " " << std::endl;
342  //float spacing[3];
343  //retval->GetSpacing(spacing);
344  //std::cout << "spacing: " << spacing[0] << " " << spacing[1] << " " << spacing[2] << " " << std::endl;
345 
346  //int dimensions[3];
347  //retval->GetDimensions(dimensions);
348  //std::cout << "dimensions: " << dimensions[0] << " " << dimensions[1] << " " << dimensions[2] << " " << std::endl;
349  return retval;
350 }
351 void ImageStreamerSonix::sendOpenIGTLinkImageSlot(int sendNumberOfMessages)
352 {
353  if (!mSender || !mSender->isReady())
354  return;
355 
356 // if(!mSocket)
357 // return;
358 // if(mSocket->bytesToWrite() > mMaxBufferSize)
359 // return;
360 
361  for(int i=0; i<sendNumberOfMessages; ++i)
362  {
363  IGTLinkImageMessage::Pointer message = this->getLastImageMessageFromQueue();
364  if(!message)
365  break;
366  PackagePtr package(new Package());
367  package->mIgtLinkImageMessage = message;
368  mSender->send(package);
369 // message->Pack();
370 // mSocket->write(reinterpret_cast<const char*>(message->GetPackPointer()), message->GetPackSize());
371  }
372 }
373 void ImageStreamerSonix::sendOpenIGTLinkStatusSlot(int sendNumberOfMessage)
374 {
375  if (!this->isReadyToSend())
376  return;
377 
378 // if(!mSocket)
379 // return;
380  //std::cout << "ImageStreamerSonix::sendOpenIGTLinkStatusSlot" << std::endl;
381 // if(mSocket->bytesToWrite() > mMaxBufferSize)
382 // return;
383 
384  for(int i=0; i<sendNumberOfMessage; ++i)
385  {
386  IGTLinkUSStatusMessage::Pointer message = this->getLastStatusMessageFromQueue();
387  if(!message)
388  break;
389 
390  PackagePtr package(new Package());
391  package->mIgtLinkUSStatusMessage = message;
392  mSender->send(package);
393 
394 // message->Pack();
395 // mSocket->write(reinterpret_cast<const char*>(message->GetPackPointer()), message->GetPackSize());
396  }
397 }
400 void ImageStreamerSonix::addImageToQueue(IGTLinkImageMessage::Pointer msg)
401 {
402  QMutexLocker sentry(&mImageMutex);
403  if(mMutexedImageMessageQueue.size() > mMaxqueueInfo)
404  {
405  mMutexedImageMessageQueue.pop_front();
406  mDroppedImages++;
407  }
408 
409  mMutexedImageMessageQueue.push_back(msg);
410  int size = mMutexedImageMessageQueue.size();
411  sentry.unlock();
412 
413  emit queueInfo(size, mDroppedImages);
414  emit imageOnQueue(size); // emit signal outside lock, catch possibly in another thread
415 }
416 
419 IGTLinkImageMessage::Pointer ImageStreamerSonix::getLastImageMessageFromQueue()
420 {
421  QMutexLocker sentry(&mImageMutex);
422  if (mMutexedImageMessageQueue.empty())
423  return IGTLinkImageMessage::Pointer();
424  IGTLinkImageMessage::Pointer retval = mMutexedImageMessageQueue.front();
425  mMutexedImageMessageQueue.pop_front();
426  return retval;
427 }
428 
431 void ImageStreamerSonix::addStatusMessageToQueue(IGTLinkUSStatusMessage::Pointer msg)
432 {
433  QMutexLocker sentry(&mStatusMutex);
434  if(mMutexedStatusMessageQueue.size() > mMaxqueueInfo)
435  {
436  mMutexedStatusMessageQueue.pop_front();
437  }
438 
439  mMutexedStatusMessageQueue.push_back(msg);
440  int size = mMutexedStatusMessageQueue.size();
441  sentry.unlock();
442  emit statusOnQueue(size); // emit signal outside lock, catch possibly in another thread
443 }
444 
447 IGTLinkUSStatusMessage::Pointer ImageStreamerSonix::getLastStatusMessageFromQueue()
448 {
449  QMutexLocker sentry(&mStatusMutex);
450  if (mMutexedStatusMessageQueue.empty())
452  IGTLinkUSStatusMessage::Pointer retval = mMutexedStatusMessageQueue.front();
453  mMutexedStatusMessageQueue.pop_front();
454  return retval;
455 }
456 
457 }
458 
459 #endif // CX_WIN32
460 
int mImagingDepth
Definition: cxFrame.h:64
double mTimestamp
Timestamp in seconds since 1/1/1970 (epoch)
Definition: cxFrame.h:48
Support Qt support for vtkSonixVideoSource.
Definition: SonixHelper.h:57
double mSpacing[2]
Definition: cxFrame.h:53
int mHeight
Height in pixels.
Definition: cxFrame.h:50
std::string probeName
Definition: cxFrame.h:63
bool similar(const DoubleBoundingBox3D &a, const DoubleBoundingBox3D &b, double tol)
int mSectorSizeInPercent
Definition: cxFrame.h:65
std::map< QString, QString > StringMap
int bly
Definition: cxFrame.h:62
int mWidth
Width in pixels.
Definition: cxFrame.h:49
cx::Frame Frame
Definition: SonixHelper.h:42
int uly
Definition: cxFrame.h:56
int mPixelFormat
Pixel format in OSType (FourCC)
Definition: cxFrame.h:51
boost::shared_ptr< struct Package > PackagePtr
unsigned char * mFirstPixel
Pointer to first pixel in frame.
Definition: cxFrame.h:52
QTimer * mSendTimer
Definition: cxStreamer.h:87
int convertStringWithDefault(QString text, int def)
boost::shared_ptr< Sender > SenderPtr
Definition: cxSender.h:85
float mOrigin[2]
Definition: cxFrame.h:54
virtual void initialize(StringMap arguments)
Definition: cxStreamer.cpp:80
static vtkSonixVideoSource * New()
bool mNewStatus
Definition: cxFrame.h:47