Changeset 330 in flair-src for trunk/tools/VrpnLite/src/VrpnLite.cpp

Timestamp:

Sep 25, 2019, 3:29:26 PM (5 years ago)

Author:

Sanahuja Guillaume

Message:

use less bandwidth in vprnlite

File:

• trunk/tools/VrpnLite/src/VrpnLite.cpp (modified) (2 diffs)

trunk/tools/VrpnLite/src/VrpnLite.cpp

@@ -25 +25 @@
 using namespace flair::sensor;
 
-VrpnLite::VrpnLite(string clientAddress,string serveurAddress): Thread(getFrameworkManager(),"VrpnLite",90) {
-  vrpnclient=new VrpnClient("vrpn", serveurAddress,80); 
-  VrpnObject* vrpnobject = new VrpnObject("x4_0",vrpnclient->GetTabWidget(),vrpnclient);
-  //VrpnObject* vrpnobject2 = new VrpnObject("target",vrpnclient->GetTabWidget(),vrpnclient);
-  
-  vrpnobjects.push_back(vrpnobject);
-  //vrpnobjects.push_back(vrpnobject2);
-  
-  dataSocket = new UdpSocket(this,"client socket",clientAddress);
-  
+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();
 }
@@ -42 +35 @@
 
 void VrpnLite::Run(void) {
+  Time dataSocketTimeout;
+  char msg[256];
+  
   while (!ToBeStopped()) {
     
-    WaitUpdate(vrpnobjects.at(0));//tood improve this wait, to be sure all vrpnobject are up to date; or one thread by object?
-    //could be also lighter to send only one frame with all objects
-    int i=0;
-    for (vector<VrpnObject*>::iterator it = vrpnobjects.begin();it < vrpnobjects.end(); it++) {
-      SendObject(*it,i);
-      i++;
+    if(vrpnobjects.size()>0) {
+      //wait for last one to be sure all are up to date
+      if(WaitUpdate(vrpnobjects.at(vrpnobjects.size()-1),100000000)) {
+          SendObjects();
+      }
+      dataSocketTimeout=TIME_NONBLOCK;
+    } else {
+      dataSocketTimeout=100000000;
     }
     
+    ssize_t rcv=dataSocket->RecvMessage(msg,sizeof(msg),dataSocketTimeout);
+    if(rcv>0) {
+      string object=msg;
+      uint16_t* idPtr=(uint16_t*)&msg[rcv-2];
+      dataSocket->HostToNetwork((char*)idPtr,sizeof(uint16_t));
+      //assume we receive it in the good order
+      if(*idPtr==vrpnobjects.size()) {
+        Printf("adding object %s with id %i\n",object.c_str(),*idPtr);
+        VrpnObject* vrpnobject = new VrpnObject(object,vrpnclient->GetTabWidget());
+        vrpnobjects.push_back(vrpnobject);
+      }else {
+        Err("adding object %s failed, expected id %i, got %i\n",object.c_str(),vrpnobjects.size(),*idPtr);
+      }
+    }
   }
 }
 
-void VrpnLite::SendObject(const VrpnObject* vrpnobject,uint8_t id) const{
+void VrpnLite::SendObjects(void) const{
+  int16_t position[3];
+  int16_t quaternion[4];
+  Time time;
+  char datas[vrpnobjects.size()*(sizeof(position)+sizeof(quaternion))+ sizeof(time)];
+  char *datasPtr=datas;
+  
+  for (vector<VrpnObject*>::const_iterator it = vrpnobjects.begin();it < vrpnobjects.end(); it++) {
     Vector3Df objectPosition;
     Quaternion objectQuaternion;
-    
+    const VrpnObject* vrpnobject=*it;
+
     vrpnobject->GetPosition(objectPosition);
     vrpnobject->GetQuaternion(objectQuaternion);
-    Time time=vrpnobject->GetLastPacketTime();
-    
-    float position[3];
-    position[0]=objectPosition.x;
-    position[1]=objectPosition.y;
-    position[2]=objectPosition.z;
-    float quaternion[4];
-    quaternion[0]=objectQuaternion.q0;
-    quaternion[1]=objectQuaternion.q1;
-    quaternion[2]=objectQuaternion.q2;
-    quaternion[3]=objectQuaternion.q3;
-    
+    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]));
-    dataSocket->HostToNetwork((char*)(&time),sizeof(Time));
-    
-    char datas[sizeof(id) + sizeof(position)+sizeof(quaternion)+ sizeof(time)];
-    
-    datas[0]=id;
-    memcpy(datas+sizeof(id),position, sizeof(position));
-    memcpy(datas +sizeof(id)+ sizeof(position),quaternion, sizeof(quaternion));
-    memcpy(datas+sizeof(id) + sizeof(position)+sizeof(quaternion),&time, sizeof(time));
-    
-    dataSocket->SendMessage(datas,sizeof(datas));
+
+    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));
 }
+
+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;
+}