VideoSource_Kinect2Net.cpp

#include "VideoSource_Kinect2Net.hh"
#include <iostream>
#include <Base/Image/ImageIO.hh>

#include <iostream>

using namespace std;
using namespace BIAS;

VideoSource_Kinect2Net::VideoSource_Kinect2Net(){
  numDevices_ = 0;
  stopThread_ = false;
  port_ = 8090;
  host_ = "localhost";
  //grabThread_ = 0;
}

VideoSource_Kinect2Net::~VideoSource_Kinect2Net(){
}

void* VideoSource_Kinect2Net::runner(void *args) {
  VideoSource_Kinect2Net *n = (VideoSource_Kinect2Net*)args;
  n->client_.SetVerbose(true);
  n->client_.RegisterMsg("img", BIAS::CS_BINARY, 512*424*2*sizeof(float));
  int ret = n->client_.ConnectServer(n->host_, n->port_);

  if (ret == 0) {

    std::vector<char> binary;
    while (!n->stopThread_) {
      timespec ts;
#if WIN32
      __int64 wintime;
      GetSystemTimeAsFileTime((FILETIME*)&wintime);
      wintime -= 116444736000000000i64;
      ts.tv_sec  = (long)(wintime / 10000000i64);
      ts.tv_nsec = (long)(wintime % 10000000i64 *100);
#else
      clock_gettime(CLOCK_REALTIME, &ts);
#endif
      ts.tv_sec += 2;
      if (n->client_.GetData("img", binary) == 0) {
        int semOk = sem_timedwait(&n->depthbuffer_.sProd, &ts);
        if (semOk != 0) {
          std::cout << "net runner dropping frames!" << std::endl;
          biasusleep(250);
        } else {
          n->depthbuffer_.buffer[ n->depthbuffer_.curProd ].CopyIn_NoInit ( binary.data() );
          n->depthbuffer_.curProd = (n->depthbuffer_.curProd+1) % buffersize_;
          sem_post(&n->depthbuffer_.sCons);
        }
        semOk = sem_timedwait(&n->irbuffer_.sProd, &ts);
        if (semOk != 0) {
          std::cout << "net runner dropping frames!" << std::endl;
          biasusleep(250);
        } else {
          //float *data = (float*)binary.data();
          n->irbuffer_.buffer[ n->irbuffer_.curProd ].CopyIn_NoInit ( binary.data()+512*424*sizeof(float) );
          n->irbuffer_.curProd = (n->irbuffer_.curProd+1) % buffersize_;
          sem_post(&n->irbuffer_.sCons);
        }
      }
    }
  }
  return NULL;
}

int VideoSource_Kinect2Net::OpenDevice(){
  return OpenDevice("localhost", 8090);
}

int VideoSource_Kinect2Net::OpenDevice(std::string host, int port) {
  host_ = host;
  port_ = port;
  return 0;
}

int VideoSource_Kinect2Net::CloseDevice(){
  return 0;
}


int VideoSource_Kinect2Net::PreGrab(){
  stopThread_ = false;
  pthread_create(&grabThread_, NULL, &runner, this);
  return 0;
}

int VideoSource_Kinect2Net::PostGrab(){
  stopThread_ = true;
  pthread_join(grabThread_, NULL);
  return 0;
}

int VideoSource_Kinect2Net::InitDepthImage(ImageBase &Image){
  Image.Init(512,424,1,ImageBase::ST_float);
  return 0;
}

int VideoSource_Kinect2Net::InitIrImage(ImageBase &Image){
  Image.Init(512,424,1,ImageBase::ST_float);
  return 0;
}

int VideoSource_Kinect2Net::GrabSingleIR(Camera <float> &image){
  sem_wait(&irbuffer_.sCons);
  image.CopyIn_NoInit( irbuffer_.buffer[ irbuffer_.curCons ].GetImageData() );
  irbuffer_.curCons = (irbuffer_.curCons+1) % buffersize_;
  sem_post(&irbuffer_.sProd);
  return 0;
}

int VideoSource_Kinect2Net::GrabSingleDepth(Camera <float> &image){
  sem_wait(&depthbuffer_.sCons);
  image.CopyIn_NoInit( depthbuffer_.buffer[ depthbuffer_.curCons ].GetImageData() );
  depthbuffer_.curCons = (depthbuffer_.curCons+1) % buffersize_;
  sem_post(&depthbuffer_.sProd);
  return 0;
}


int VideoSource_Kinect2Net::GetNumDevices(){
  return numDevices_;
}

int VideoSource_Kinect2Net::GetCapabilities(VideoSourceCapabilities &caps){
  return 0;
}


int VideoSource_Kinect2Net::GetCapabilities(const char *device, VideoSourceCapabilities &caps){
  return 0;
}