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