usNetworkGrabberPostScanBiPlan.cpp

/****************************************************************************
 *
 * This file is part of the ustk software.
 * Copyright (C) 2016 - 2017 by Inria. All rights reserved.
 *
 * This software is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * ("GPL") version 2 as published by the Free Software Foundation.
 * See the file LICENSE.txt at the root directory of this source
 * distribution for additional information about the GNU GPL.
 *
 * For using ustk with software that can not be combined with the GNU
 * GPL, please contact Inria about acquiring a ViSP Professional
 * Edition License.
 *
 * This software was developed at:
 * Inria Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 *
 * If you have questions regarding the use of this file, please contact
 * Inria at ustk@inria.fr
 *
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 *
 * Authors:
 * Pedro Patlan
 * Marc Pouliquen
 *
 *****************************************************************************/
 
#include <visp3/ustk_grabber/usNetworkGrabberPostScanBiPlan.h>
 
#if defined(USTK_HAVE_QT5) || defined(USTK_HAVE_VTK_QT)
 
#include <QtCore/QDataStream>
#include <QtCore/QEventLoop>
 
usNetworkGrabberPostScanBiPlan::usNetworkGrabberPostScanBiPlan(usNetworkGrabber *parent) : usNetworkGrabber(parent)
{
  // 2 buffers of size 3
  m_outputBuffer1.push_back(new usFrameGrabbedInfo<usImagePostScan2D<unsigned char> >);
  m_outputBuffer1.push_back(new usFrameGrabbedInfo<usImagePostScan2D<unsigned char> >);
  m_outputBuffer1.push_back(new usFrameGrabbedInfo<usImagePostScan2D<unsigned char> >);
  m_outputBuffer2.push_back(new usFrameGrabbedInfo<usImagePostScan2D<unsigned char> >);
  m_outputBuffer2.push_back(new usFrameGrabbedInfo<usImagePostScan2D<unsigned char> >);
  m_outputBuffer2.push_back(new usFrameGrabbedInfo<usImagePostScan2D<unsigned char> >);
 
  m_bufferToFill = 1;
 
  m_firstFrameAvailable = false;
 
  connect(m_tcpSocket, SIGNAL(readyRead()), this, SLOT(dataArrived()));
}
 
usNetworkGrabberPostScanBiPlan::~usNetworkGrabberPostScanBiPlan() {}
 
void usNetworkGrabberPostScanBiPlan::dataArrived()
{
  QDataStream in;
  in.setDevice(m_tcpSocket);
#if (defined(USTK_HAVE_QT5) || defined(USTK_HAVE_VTK_QT5))
  in.setVersion(QDataStream::Qt_5_0);
#elif defined(USTK_HAVE_VTK_QT4)
  in.setVersion(QDataStream::Qt_4_8);
#else
  throw(vpException(vpException::fatalError, "your Qt version is not managed in ustk"));
#endif
 
  int headerType;
 
  // select buffer to write in
  std::vector<usFrameGrabbedInfo<usImagePostScan2D<unsigned char> > *> *refOnBufferToFill;
  if (m_bufferToFill == 1) {
    refOnBufferToFill = &m_outputBuffer1;
  } else {
    refOnBufferToFill = &m_outputBuffer2;
  }
 
  if (m_bytesLeftToRead == 0) { // do not try to read a header if last frame was not complete
    in >> headerType;
    if (m_verbose)
      std::cout << "header received, type = " << headerType << std::endl;
  } else {
    headerType = 0; // not a header received, but a part of a frame
  }
  // init confirm header received
  if (headerType == m_confirmHeader.headerId) {
    // read whole header
    in >> m_confirmHeader.initOk;
    in >> m_confirmHeader.probeId;
 
    if (m_confirmHeader.initOk == 0) {
      m_tcpSocket->close();
      throw(vpException(vpException::fatalError, "porta initialisation error, closing connection."));
    }
    if (m_verbose)
      std::cout << "porta init sucess, detected probe id = " << m_confirmHeader.probeId << std::endl;
 
    // read all acquisition parameters received
    readAcquisitionParameters(in);
 
    emit(serverUpdateEnded(m_confirmHeader.initOk));
  }
 
  // image header received
  else if (headerType == m_imageHeader.headerId) {
    // read whole header
    in >> m_imageHeader.frameCount;
    quint64 timestamp;
    in >> timestamp;
    m_imageHeader.timeStamp = timestamp;
    in >> m_imageHeader.dataRate;
    in >> m_imageHeader.dataLength;
    in >> m_imageHeader.ss;
    in >> m_imageHeader.imageType;
    in >> m_imageHeader.frameWidth;
    in >> m_imageHeader.frameHeight;
    in >> m_imageHeader.pixelWidth;
    in >> m_imageHeader.pixelHeight;
    in >> m_imageHeader.transmitFrequency;
    in >> m_imageHeader.samplingFrequency;
 
    in >> m_imageHeader.transducerRadius;
    in >> m_imageHeader.scanLinePitch;
    in >> m_imageHeader.scanLineNumber;
    in >> m_imageHeader.imageDepth;
    in >> m_imageHeader.anglePerFr;
    in >> m_imageHeader.framesPerVolume;
    in >> m_imageHeader.motorRadius;
    in >> m_imageHeader.motorType;
 
    if (m_verbose) {
      std::cout << "frameCount = " << m_imageHeader.frameCount << std::endl;
      std::cout << "timeStamp = " << m_imageHeader.timeStamp << std::endl;
      std::cout << "dataRate = " << m_imageHeader.dataRate << std::endl;
      std::cout << "dataLength = " << m_imageHeader.dataLength << std::endl;
      std::cout << "ss = " << m_imageHeader.ss << std::endl;
      std::cout << "imageType = " << m_imageHeader.imageType << std::endl;
      std::cout << "frameWidth = " << m_imageHeader.frameWidth << std::endl;
      std::cout << "frameHeight = " << m_imageHeader.frameHeight << std::endl;
      std::cout << "pixelWidth = " << m_imageHeader.pixelWidth << std::endl;
      std::cout << "pixelHeight = " << m_imageHeader.pixelHeight << std::endl;
      std::cout << "transmitFrequency = " << m_imageHeader.transmitFrequency << std::endl;
      std::cout << "samplingFrequency = " << m_imageHeader.samplingFrequency << std::endl;
      std::cout << "transducerRadius = " << m_imageHeader.transducerRadius << std::endl;
      std::cout << "scanLinePitch = " << m_imageHeader.scanLinePitch << std::endl;
      std::cout << "scanLineNumber = " << m_imageHeader.scanLineNumber << std::endl;
      std::cout << "imageDepth = " << m_imageHeader.imageDepth << std::endl;
      std::cout << "anglePerFr = " << m_imageHeader.anglePerFr << std::endl;
      std::cout << "framesPerVolume = " << m_imageHeader.framesPerVolume << std::endl;
      std::cout << "motorRadius = " << m_imageHeader.motorRadius << std::endl;
      std::cout << "motorType = " << m_imageHeader.motorType << std::endl;
    }
 
    // update transducer settings with image header received
    refOnBufferToFill->at(CURRENT_FILLED_FRAME_POSITION_IN_VEC)->setTransducerRadius(m_imageHeader.transducerRadius);
    refOnBufferToFill->at(CURRENT_FILLED_FRAME_POSITION_IN_VEC)->setScanLinePitch(m_imageHeader.scanLinePitch);
    refOnBufferToFill->at(CURRENT_FILLED_FRAME_POSITION_IN_VEC)->setScanLineNumber(m_imageHeader.scanLineNumber);
    refOnBufferToFill->at(CURRENT_FILLED_FRAME_POSITION_IN_VEC)->setDepth(m_imageHeader.imageDepth / 1000.0);
    refOnBufferToFill->at(CURRENT_FILLED_FRAME_POSITION_IN_VEC)->setTransmitFrequency(m_imageHeader.transmitFrequency);
    refOnBufferToFill->at(CURRENT_FILLED_FRAME_POSITION_IN_VEC)->setSamplingFrequency(m_imageHeader.samplingFrequency);
 
    // set data info
    refOnBufferToFill->at(CURRENT_FILLED_FRAME_POSITION_IN_VEC)->setFrameCount(m_imageHeader.frameCount);
    refOnBufferToFill->at(CURRENT_FILLED_FRAME_POSITION_IN_VEC)->setFramesPerVolume(m_imageHeader.framesPerVolume);
    refOnBufferToFill->at(CURRENT_FILLED_FRAME_POSITION_IN_VEC)->setTimeStamp(m_imageHeader.timeStamp);
 
    // warning if timestamps are close (< 1 ms)
    if (refOnBufferToFill->at(CURRENT_FILLED_FRAME_POSITION_IN_VEC)->getTimeStamp() -
            refOnBufferToFill->at(MOST_RECENT_FRAME_POSITION_IN_VEC)->getTimeStamp() <
        1) {
      std::cout << "WARNING : new image received with an acquisition timestamp close to previous image" << std::endl;
    }
 
    refOnBufferToFill->at(CURRENT_FILLED_FRAME_POSITION_IN_VEC)
        ->resize(m_imageHeader.frameHeight, m_imageHeader.frameWidth);
 
    // pixel size
    if (refOnBufferToFill->at(CURRENT_FILLED_FRAME_POSITION_IN_VEC)->getTransducerRadius() > 0) { // convex probe
      refOnBufferToFill->at(CURRENT_FILLED_FRAME_POSITION_IN_VEC)->setWidthResolution(m_imageHeader.pixelWidth);
 
      refOnBufferToFill->at(CURRENT_FILLED_FRAME_POSITION_IN_VEC)->setHeightResolution(m_imageHeader.pixelHeight);
    } else { // linear probe
      refOnBufferToFill->at(CURRENT_FILLED_FRAME_POSITION_IN_VEC)
          ->setWidthResolution(refOnBufferToFill->at(CURRENT_FILLED_FRAME_POSITION_IN_VEC)->getScanLinePitch() /
                               refOnBufferToFill->at(CURRENT_FILLED_FRAME_POSITION_IN_VEC)->getWidth());
 
      refOnBufferToFill->at(CURRENT_FILLED_FRAME_POSITION_IN_VEC)
          ->setHeightResolution(refOnBufferToFill->at(CURRENT_FILLED_FRAME_POSITION_IN_VEC)->getDepth() /
                                refOnBufferToFill->at(CURRENT_FILLED_FRAME_POSITION_IN_VEC)->getHeight());
    }
    // read image content
    m_bytesLeftToRead = m_imageHeader.dataLength;
 
    m_bytesLeftToRead -= in.readRawData((char *)refOnBufferToFill->at(CURRENT_FILLED_FRAME_POSITION_IN_VEC)->bitmap,
                                        m_imageHeader.dataLength);
 
    if (m_bytesLeftToRead == 0) { // we've read all the frame in 1 packet.
      // Now CURRENT_FILLED_FRAME_POSITION_IN_VEC has become the last frame received
      // So we switch pointers beween MOST_RECENT_FRAME_POSITION_IN_VEC and CURRENT_FILLED_FRAME_POSITION_IN_VEC
      {
        usFrameGrabbedInfo<usImagePostScan2D<unsigned char> > *savePtr =
            refOnBufferToFill->at(CURRENT_FILLED_FRAME_POSITION_IN_VEC);
        refOnBufferToFill->at(CURRENT_FILLED_FRAME_POSITION_IN_VEC) =
            refOnBufferToFill->at(MOST_RECENT_FRAME_POSITION_IN_VEC);
        refOnBufferToFill->at(MOST_RECENT_FRAME_POSITION_IN_VEC) = savePtr;
      }
      m_bufferToFill = (m_bufferToFill == 1) ? 2 : 1;
      if (m_bufferToFill == 1) { // we just read image 2
        m_firstFrameAvailable = true;
        emit(newFrameAvailable());
      }
    }
    if (m_verbose)
      std::cout << "Bytes left to read for whole frame = " << m_bytesLeftToRead << std::endl;
 
  }
 
  // we have a part of the image still not read (arrived with next tcp packet)
  else {
    if (m_verbose) {
      std::cout << "reading following part of the frame, left to read = " << m_bytesLeftToRead << std::endl;
      std::cout << "local image size = " << refOnBufferToFill->at(CURRENT_FILLED_FRAME_POSITION_IN_VEC)->getSize()
                << std::endl;
    }
    m_bytesLeftToRead -=
        in.readRawData((char *)refOnBufferToFill->at(CURRENT_FILLED_FRAME_POSITION_IN_VEC)->bitmap +
                           (refOnBufferToFill->at(CURRENT_FILLED_FRAME_POSITION_IN_VEC)->getSize() - m_bytesLeftToRead),
                       m_bytesLeftToRead);
 
    if (m_bytesLeftToRead == 0) { // we've read the last part of the frame.
      // Now CURRENT_FILLED_FRAME_POSITION_IN_VEC has become the last frame received
      // So we switch pointers beween MOST_RECENT_FRAME_POSITION_IN_VEC and CURRENT_FILLED_FRAME_POSITION_IN_VEC
      usFrameGrabbedInfo<usImagePostScan2D<unsigned char> > *savePtr =
          refOnBufferToFill->at(CURRENT_FILLED_FRAME_POSITION_IN_VEC);
      refOnBufferToFill->at(CURRENT_FILLED_FRAME_POSITION_IN_VEC) =
          refOnBufferToFill->at(MOST_RECENT_FRAME_POSITION_IN_VEC);
      refOnBufferToFill->at(MOST_RECENT_FRAME_POSITION_IN_VEC) = savePtr;
      m_bufferToFill = (m_bufferToFill == 1) ? 2 : 1;
      if (m_bufferToFill == 1) { // we just read image 2
        m_firstFrameAvailable = true;
        emit(newFrameAvailable());
      }
    }
  }
}
 
std::vector<usFrameGrabbedInfo<usImagePostScan2D<unsigned char> > *> usNetworkGrabberPostScanBiPlan::acquire()
{
  // manage first frame or if user grabs too fast
  if (!m_firstFrameAvailable ||
      (m_outputBuffer1.at(OUTPUT_FRAME_POSITION_IN_VEC)->getFrameCount() >=
           m_outputBuffer1.at(MOST_RECENT_FRAME_POSITION_IN_VEC)->getFrameCount() &&
       m_outputBuffer2.at(OUTPUT_FRAME_POSITION_IN_VEC)->getFrameCount() >=
           m_outputBuffer2.at(MOST_RECENT_FRAME_POSITION_IN_VEC)->getFrameCount())) {
    // we wait until a new frame is available
    QEventLoop loop;
    loop.connect(this, SIGNAL(newFrameAvailable()), SLOT(quit()));
    loop.exec();
  }
 
  // switch pointers
  usFrameGrabbedInfo<usImagePostScan2D<unsigned char> > *savePtr = m_outputBuffer1.at(OUTPUT_FRAME_POSITION_IN_VEC);
  m_outputBuffer1.at(OUTPUT_FRAME_POSITION_IN_VEC) = m_outputBuffer1.at(MOST_RECENT_FRAME_POSITION_IN_VEC);
  m_outputBuffer1.at(MOST_RECENT_FRAME_POSITION_IN_VEC) = savePtr;
 
  savePtr = m_outputBuffer2.at(OUTPUT_FRAME_POSITION_IN_VEC);
  m_outputBuffer2.at(OUTPUT_FRAME_POSITION_IN_VEC) = m_outputBuffer2.at(MOST_RECENT_FRAME_POSITION_IN_VEC);
  m_outputBuffer2.at(MOST_RECENT_FRAME_POSITION_IN_VEC) = savePtr;
 
  // return the pointers on the two images
  std::vector<usFrameGrabbedInfo<usImagePostScan2D<unsigned char> > *> ret;
  ret.push_back(m_outputBuffer1.at(OUTPUT_FRAME_POSITION_IN_VEC));
  ret.push_back(m_outputBuffer2.at(OUTPUT_FRAME_POSITION_IN_VEC));
  
  return ret;
}
 
#endif