source: flair-src/trunk/tools/VrpnLite/src/VrpnLite.cpp@ 364

//  created:    2019/03/12

//  filename:   VrpnLite.cpp

//

//  author:     Guillaume Sanahuja

//              Copyright Heudiasyc UMR UTC/CNRS 7253

//

//  version:    $Id: $

//

//  purpose:    vrpnlite, to forward it to bth for exemple

//              usefull to reduce vrpn frame size

//

/*********************************************************************/



#include "VrpnLite.h"

#include <VrpnClient.h>

#include <VrpnObject.h>

#include <Thread.h>

#include <UdpSocket.h>

#include <FrameworkManager.h>

#include <Quaternion.h>

#include <string.h>



using namespace std;

using namespace flair::core;

using namespace flair::sensor;



VrpnLite::VrpnLite(int vrpnLitePort,string vrpnServerAddress): Thread(getFrameworkManager(),"VrpnLite",90) {

  vrpnclient=new VrpnClient("vrpn", vrpnServerAddress,80); 

  dataSocket = new UdpSocket(this,"client socket",vrpnLitePort);

  vrpnclient->Start();

}



VrpnLite::~VrpnLite() {

}



void VrpnLite::Run(void) {

  Time dataSocketTimeout;

  char msg[256];

  int src_id;

  

  while (!ToBeStopped()) {

    if(vrpnobjects.size()>0) {

      //wait for last one to be sure all are up to date

      //but if object is not tracked we send nothing... send it unsynchronized???

      if(WaitUpdate(vrpnobjects.at(vrpnobjects.size()-1),100000000)) {

          SendObjects();

      }

      dataSocketTimeout=TIME_NONBLOCK;

    } else {

      dataSocketTimeout=100000000;

    }

    

    ssize_t rcv=dataSocket->RecvMessage(msg,sizeof(msg),dataSocketTimeout,NULL,NULL,&src_id);

    if(rcv>0) {

      string object=msg;

      uint16_t id;

      memcpy(&id,&msg[rcv-2],sizeof(id));

      dataSocket->HostToNetwork((char*)&id,sizeof(id));

      

      if(src_id==vrpnobjects_list.size()) {

        vector<VrpnObject*> vect;

        vrpnobjects_list.push_back(vect);

        Printf("add vect %i\n",src_id);

      }

      //assume we receive it in the good order

      if(id==vrpnobjects_list.at(src_id).size()) {

        Printf("adding object %s with id %i from %i\n",object.c_str(),id,src_id);

        VrpnObject* vrpnobject = new VrpnObject(object,vrpnclient->GetTabWidget());

        vrpnobjects.push_back(vrpnobject);

        vrpnobjects_list.at(src_id).push_back(vrpnobject);

      }else {

        Err("adding object %s failed, expected id %i, got %i\n",object.c_str(),vrpnobjects.size(),id);

      }

    }

  }

}



void VrpnLite::SendObjects(void) const{

 

  int16_t position[3];

  int16_t quaternion[4];

  Time time;

 

  //for (vector<vector<VrpnObject*>>::const_iterator connection= vrpnobjects_list.begin();connection < vrpnobjects_list.end(); connection++) {

  for (int i=0;i<vrpnobjects_list.size();i++) {

      char datas[vrpnobjects_list.at(i).size()*(sizeof(position)+sizeof(quaternion))+ sizeof(time)];

      char *datasPtr=datas;

      for (vector<VrpnObject*>::const_iterator it = vrpnobjects_list.at(i).begin();it < vrpnobjects_list.at(i).end(); it++) {

        Vector3Df objectPosition;

        Quaternion objectQuaternion;

        const VrpnObject* vrpnobject=*it;



        vrpnobject->GetPosition(objectPosition);

        vrpnobject->GetQuaternion(objectQuaternion);

        time=vrpnobject->GetLastPacketTime();



        position[0]=ConvertPosition(objectPosition.x);

        position[1]=ConvertPosition(objectPosition.y);

        position[2]=ConvertPosition(objectPosition.z);

        quaternion[0]=ConvertQuaternion(objectQuaternion.q0);

        quaternion[1]=ConvertQuaternion(objectQuaternion.q1);

        quaternion[2]=ConvertQuaternion(objectQuaternion.q2);

        quaternion[3]=ConvertQuaternion(objectQuaternion.q3);



        for(int i=0;i<3;i++) dataSocket->HostToNetwork((char*)(&position[i]),sizeof(position[i]));

        for(int i=0;i<4;i++) dataSocket->HostToNetwork((char*)(&quaternion[i]),sizeof(quaternion[i]));



        memcpy(datasPtr,position, sizeof(position));

        datasPtr+=sizeof(position);

        memcpy(datasPtr,quaternion, sizeof(quaternion));

        datasPtr+=sizeof(quaternion);

      }

      dataSocket->HostToNetwork((char*)(&time),sizeof(Time));

      memcpy(datasPtr,&time, sizeof(time));//only one time for all VrpnObject; suppose it is the same!

      dataSocket->SendMessage(datas,sizeof(datas),i);

  }

}



int16_t VrpnLite::ConvertQuaternion(float value) const{

  int16_t tmp;

  tmp=value*32767.;

  if(value<-1) {

    tmp=-32767;

    Warn("position value is %f, saturating it to %i\n",value,tmp);

  }

  if(value>1) {

    tmp=32767;

    Warn("position value is %f, saturating it to %i\n",value,tmp);

  }

  return tmp;

}



int16_t VrpnLite::ConvertPosition(float value) const{

  int16_t tmp;

  tmp=value*1000;

  if(value<-32.768) {

    tmp=-32768;

    Warn("position value is %f, saturating it to %i\n",value,tmp);

  }

  if(value>32.767) {

    tmp=32767;

    Warn("position value is %f, saturating it to %i\n",value,tmp);

  }

  return tmp;

}