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

Timestamp:

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

Author:

Sanahuja Guillaume

Message:

use less bandwidth in vprnlite

Location:

trunk/tools/VrpnLite/src

Files:

• VrpnLite.cpp (modified) (2 diffs)
• VrpnLite.h (modified) (2 diffs)
• main.cpp (modified) (5 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;
+}

trunk/tools/VrpnLite/src/VrpnLite.h

@@ -30 +30 @@
 class VrpnLite: public flair::core::Thread {
   public:
-      VrpnLite(std::string clientAddress,std::string serveurAddress);
+      VrpnLite(int vrpnLitePort,std::string vrpnServerAddress);
       ~VrpnLite();
 
@@ -38 +38 @@
       std::vector<flair::sensor::VrpnObject*> vrpnobjects;
       flair::core::UdpSocket* dataSocket;
-      void SendObject(const flair::sensor::VrpnObject* vrpnobject,uint8_t id) const;
-      
+      void SendObjects(void) const;
+      int16_t ConvertPosition(float value) const;
+      int16_t ConvertQuaternion(float value) const;
 };
 

trunk/tools/VrpnLite/src/main.cpp

@@ -22 +22 @@
 
 string xml_file;
-string clientAddress,serverAddress;
-int port;
+string vrpnServerAddress;
+int gcsPort,vrpnLitePort;
 
 void parseOptions(int argc, char **argv);
@@ -32 +32 @@
   FrameworkManager *manager;
   manager = new FrameworkManager("vrpnforwarder");
-  manager->SetupConnection("127.0.0.1", port);
+  manager->SetupConnection("127.0.0.1", gcsPort);
   manager->SetupUserInterface(xml_file);
   
-  VrpnLite* vrpnlite=new VrpnLite(clientAddress,serverAddress);
+  VrpnLite* vrpnlite=new VrpnLite(vrpnLitePort,vrpnServerAddress);
   
   vrpnlite->Start();
@@ -47 +47 @@
     CmdLine cmd("Command description message", ' ', "0.1");
 
-    ValueArg<string> clientaddressArg("c", "caddress","client address", true,
-                                "127.0.0.1:3884", "string");
-    cmd.add(clientaddressArg);
+    ValueArg<int> vrpnLitePortArg("v", "vport","vrpn lite port", true,
+                                3884, "int");
+    cmd.add(vrpnLitePortArg);
     
     ValueArg<string> serveraddressArg("s", "saddress","server address", true,
@@ -55 +55 @@
     cmd.add(serveraddressArg);
 
-    ValueArg<int> portArg("p", "port","local port used to connect to the ground station",
+    ValueArg<int> gcsPortArg("p", "port","local port used to connect to the ground station",
                           false, 9000, "int");
-    cmd.add(portArg);
+    cmd.add(gcsPortArg);
 
 
@@ -66 +66 @@
     cmd.parse(argc, argv);
 
-    clientAddress = clientaddressArg.getValue();
-    serverAddress = serveraddressArg.getValue();
-    port = portArg.getValue();
+    vrpnLitePort = vrpnLitePortArg.getValue();
+    vrpnServerAddress = serveraddressArg.getValue();
+    gcsPort = gcsPortArg.getValue();
     xml_file = xmlArg.getValue();