Changeset 330 in flair-src for trunk/lib/FlairSensorActuator/src/VrpnClient_impl.cpp

Timestamp:

09/25/19 15:29:26 (5 years ago)

Author:

Sanahuja Guillaume

Message:

use less bandwidth in vprnlite

File:

• trunk/lib/FlairSensorActuator/src/VrpnClient_impl.cpp (modified) (7 diffs)

trunk/lib/FlairSensorActuator/src/VrpnClient_impl.cpp

@@ -40 +40 @@
 
 VrpnClient_impl::VrpnClient_impl(VrpnClient *self, string name,
-                                 std::string address) {
+                                 std::string address,flair::sensor::VrpnClient::ConnectionType_t connectionType) {
   this->self = self;
   this->address = address;
+  this->connectionType = connectionType;
   isConnected=false;
-  connectionType=VrpnClient::Vrpn;
-
-  connection = vrpn_get_connection_by_name(address.c_str());
+  
+  if(connectionType==VrpnClient::Vrpn) {
+    connection = vrpn_get_connection_by_name(address.c_str());
+    Printf("Connecting to VRPN server on %s\n",address.c_str());
+  } else if(connectionType==VrpnClient::VrpnLite) {
+    dataSocket =new UdpSocket(getFrameworkManager(), "data_socket", address);
+    Printf("Connecting to VRPN-lite server on %s\n",address.c_str());
+  } else {
+    self->Err("Bad connection type, try using naother constructor\n");
+  }
 
   CommonConstructor(name);
-  
-  Printf("Connecting to VRPN server on %s\n",address.c_str());
-}
-
-VrpnClient_impl::VrpnClient_impl(VrpnClient *self, string name,
-                                 uint16_t port) {
-  this->self = self;
-        isConnected=false;
-  connectionType=VrpnClient::VrpnLite;
-
-  dataSocket =new UdpSocket(getFrameworkManager(), "data_socket", port);
-
-  CommonConstructor(name);
-  
-  Printf("Connecting to VRPN-lite server on port %i\n",port);
-}
+}
+
+
 
 VrpnClient_impl::VrpnClient_impl(VrpnClient *self, string name,
@@ -132 +127 @@
     trackables.push_back(obj);
     mutex->ReleaseMutex();
+  } else if (connectionType==VrpnClient::VrpnLite) {
+    if(liteObjects.size()<0xffff) {
+      liteObject_t tmp;
+      tmp.vrpnobject = obj;
+      tmp.id = liteObjects.size();
+      mutex->GetMutex();
+      liteObjects.push_back(tmp);
+      mutex->ReleaseMutex();
+      //Printf("%i %s\n",tmp.id,obj->self->ObjectName().c_str());
+      
+      char char_array[obj->self->ObjectName().length() + 2];//id coded on 16bits 
+      strcpy(char_array, obj->self->ObjectName().c_str()); 
+      uint16_t* idPtr=(uint16_t*)&char_array[obj->self->ObjectName().length()];
+      *idPtr=tmp.id;
+      dataSocket->HostToNetwork((char*)idPtr,sizeof(uint16_t));
+      dataSocket->SendMessage(char_array,obj->self->ObjectName().length() + 2);
+    }else {
+      self->Warn("too much trackables for vrpnlite connection, not adding %s\n",obj->self->ObjectName().c_str());
+    }
   } else {
-    self->Warn("AddTrackable called but not in vrpn mode\n");
+    self->Warn("AddTrackable called but not in vrpn mode nor in vrpnlite mode\n");
   }
 }
@@ -146 +160 @@
     mutex->ReleaseMutex();
   } else {
-    self->Warn("AddTrackable called but not in vrpnlite nor in xbee mode\n");
+    self->Warn("AddTrackable called but not in xbee mode\n");
   }
 }
@@ -153 +167 @@
   mutex->GetMutex();
   if (connectionType==VrpnClient::Vrpn) {
-    for (vector<VrpnObject_impl *>::iterator it = trackables.begin();
-         it < trackables.end(); it++) {
+    for (vector<VrpnObject_impl *>::iterator it = trackables.begin();it < trackables.end(); it++) {
       if (*it == obj) {
         trackables.erase(it);
@@ -162 +175 @@
   }
   if (connectionType==VrpnClient::VrpnLite || connectionType==VrpnClient::Xbee) {
-    for (vector<liteObject_t>::iterator it = liteObjects.begin();
-         it < liteObjects.end(); it++) {
+    for (vector<liteObject_t>::iterator it = liteObjects.begin();it < liteObjects.end(); it++) {
       if ((*it).vrpnobject == obj) {
         liteObjects.erase(it);
@@ -243 +255 @@
             //printf("%lld\n",GetTime()/(1000*1000));
             mutex->GetMutex();
-            for (unsigned int i = 0; i < trackables.size(); i++)
-                trackables.at(i)->tracker->mainloop();
+            for (unsigned int i = 0; i < trackables.size(); i++) {
+             // Printf("tracker %i\n",i);
+              trackables.at(i)->tracker->mainloop();
+              //Printf("tracker %i ok\n",i);
+            }
             mutex->ReleaseMutex();
         } else {
@@ -251 +266 @@
         }
     }else if(connectionType==VrpnClient::VrpnLite) {
-      vrpn_TRACKERCB t;
-      float pos[3];
-      float quat[4];
+      mutex->GetMutex();
+    
+      int16_t pos[3];
+      int16_t quat[4];
       Time time;
-      uint8_t id;
-      char datas[sizeof(id) + sizeof(pos)+sizeof(quat)+ sizeof(time)];
+      char datas[liteObjects.size()*(sizeof(pos)+sizeof(quat))+ sizeof(time)];
+      char *datasPtr=datas;
+      
       int rcv=dataSocket->RecvMessage(datas,sizeof(datas),50*1000*1000);
-      if(rcv!=sizeof(datas)) continue;  
-      id = datas[0];
-      memcpy(pos, datas+sizeof(id), sizeof(pos));
-      memcpy(quat, datas +sizeof(id)+ sizeof(pos), sizeof(quat));
-      memcpy(&time, datas+sizeof(id) + sizeof(pos)+sizeof(quat), sizeof(time));
-      
-      for(int i=0;i<3;i++) dataSocket->NetworkToHost((char*)(&pos[i]),sizeof(pos[i]));
-      for(int i=0;i<4;i++) dataSocket->NetworkToHost((char*)(&quat[i]),sizeof(quat[i]));
+      if(rcv!=sizeof(datas)) {
+        if(rcv>0) Printf("discarding message (size %i/%i)\n",rcv,sizeof(datas));
+        mutex->ReleaseMutex();
+        continue;  
+      }
+      
+      memcpy(&time, datasPtr+sizeof(datas)-sizeof(time), sizeof(time));
       dataSocket->NetworkToHost((char*)(&time),sizeof(time));
-
-      mutex->GetMutex();
-      if (id < liteObjects.size()) {
-        for (int i = 0; i < 3; i++) t.pos[i] = pos[i];
+      
+      for (vector<liteObject_t>::iterator it = liteObjects.begin();it < liteObjects.end(); it++) {
+        memcpy(pos, datasPtr, sizeof(pos));
+        datasPtr+=sizeof(pos);
+        memcpy(quat,datasPtr, sizeof(quat));
+        datasPtr+=sizeof(quat);
+        
+        for(int i=0;i<3;i++) dataSocket->NetworkToHost((char*)(&pos[i]),sizeof(pos[i]));
+        for(int i=0;i<4;i++) dataSocket->NetworkToHost((char*)(&quat[i]),sizeof(quat[i]));
+        
+        vrpn_TRACKERCB t;
+        for (int i = 0; i < 3; i++) t.pos[i] = ConvertPosition(pos[i]);
         // warning: t.quat is defined as (qx,qy,qz,qw), which is different from
         // flair::core::Quaternion
-        t.quat[0] = quat[1];
-        t.quat[1] = quat[2];
-        t.quat[2] = quat[3];
-        t.quat[3] = quat[0];
+        t.quat[0] = ConvertQuaternion(quat[1]);
+        t.quat[1] = ConvertQuaternion(quat[2]);
+        t.quat[2] = ConvertQuaternion(quat[3]);
+        t.quat[3] = ConvertQuaternion(quat[0]);
         t.msg_time.tv_sec=time/((Time)1000000000);
         t.msg_time.tv_usec=(time%((Time)1000000000))/((Time)1000);
         //Printf("%i %lld %lld %lld\n",id,time,t.msg_time.tv_sec,t.msg_time.tv_usec);
-        VrpnObject_impl::handle_pos(liteObjects.at(id).vrpnobject, t);
-      }
+        VrpnObject_impl::handle_pos((void*)(it->vrpnobject), t);
+      }
+      
       mutex->ReleaseMutex();
     }
   }
 }
+
+float VrpnClient_impl::ConvertPosition(int16_t value) const {
+  return (float)value/1000.;
+}
+
+float VrpnClient_impl::ConvertQuaternion(int16_t value) const {
+  return (float)value/32767.;
+}