biaskinect2server.cpp

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

#include <VideoSource/VideoSource_Kinect2.hh>
#include <NetworkComm/CScommServer.hh>

using namespace std;
using namespace BIAS;

int main(int argc, char *argv[])
{
  int devID = argc > 1 ? atoi(argv[1]) : 0;
  int portID = argc > 2 ? atoi(argv[2]) : 8090;

  VideoSource_Kinect2 cam;
  cam.SetMode(VideoSource_Kinect2::depthAndIr);  
  if (cam.OpenDevice(devID) < 0) {
    std::cout << "Error: Failed to open device " << devID << "!" << std::endl;
    return -1;
  } else {
    std::cout << "Opened device " << devID << std::endl;
  }

  std::cout << "Initializing capture" << std::endl;
  Camera<float> depth, ir;
  cam.InitDepthImage(depth);
  cam.InitIrImage(ir);
  cam.PreGrab();

  std::cout << "Starting server" << std::endl;
  CScommServer server;
  unsigned int byteSize = depth.GetSizeByte();
  server.RegisterMsg("img", BIAS::CS_BINARY, 2*byteSize);
  server.RegisterMsg("stop", BIAS::CS_INT, 1);
  server.RegisterMsg("ack", BIAS::CS_INT, 1);
  server.SetVerbose(true);

  std::cout << "Wait for connections at port " << portID << std::endl;
  server.WaitForConnections(portID);

  unsigned char *data = new unsigned char[byteSize*2];
  unsigned char *datair = data + byteSize;

  bool doStop = false;
  while (!doStop) {
    cam.GrabSingleDepth(depth);
    cam.GrabSingleIR(ir);
    memcpy(data, depth.GetImageData(), byteSize);
    memcpy(datair, ir.GetImageData(), byteSize);
    server.SendMsg("img", (char*)data, 2*byteSize);
  }

  std::cout << "Closing device" << std::endl;
  cam.PostGrab();
  cam.CloseDevice();

  return 0;
}