Changeset 448 in flair-src

Timestamp:

Sep 20, 2021, 5:16:50 PM (3 years ago)

Author:

Sanahuja Guillaume

Message:

handle ack in vrpnlite

Location:

trunk

Files:

• lib/FlairSensorActuator/src/VrpnClient_impl.cpp (modified) (2 diffs)
• tools/VrpnLite/src/VrpnLite.cpp (modified) (3 diffs)
• tools/VrpnLite/src/VrpnLite.h (modified) (1 diff)

trunk/lib/FlairSensorActuator/src/VrpnClient_impl.cpp

@@ -202 +202 @@
         timeout.tv_sec=0;
         timeout.tv_usec=100000;
+  uint16_t vrpnliteCounter=0;
                         
   while (!self->ToBeStopped()) {
@@ -321 +322 @@
       
       mutex->ReleaseMutex();
+      
+      if(trackables.size()!=0) {
+        vrpnliteCounter++;//send ack to monitor connection on vrpnlite
+        //on bth, vrpn is configured to 50Hz, send ack at 1Hz
+        if(vrpnliteCounter==50) {
+          vrpnliteCounter=0;
+          char data=0;
+          dataSocket->SendMessage(&data,sizeof(char));
+        }
+      }
     }
   }

trunk/tools/VrpnLite/src/VrpnLite.cpp

@@ -30 +30 @@
 using namespace flair::gui;
 
+//if we don'receive ack from client, do not send vrpnobjects anymore
+#define REMOVE_TIMEOUT (Time)2000000000
+
+
 VrpnLite::VrpnLite(int vrpnLitePort,string vrpnServerAddress): Thread(getFrameworkManager(),"VrpnLite",90) {
   vrpnclient=new VrpnClient("vrpn", vrpnServerAddress,80); 
@@ -48 +52 @@
   while (!ToBeStopped()) {
     if(killButton->Clicked()) SafeStop();
+    //get last vrpnobject
+    VrpnObject* lastVrpnObject=NULL;
+    if(connections.size()!=0) {
+      if(connections.back().vrpnobjects.size()!=0) lastVrpnObject=lastVrpnObject=connections.back().vrpnobjects.back();
+    }
     
-    if(vrpnobjects.size()>0) {
+    if(lastVrpnObject!=NULL) {
       //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)) {
+      if(WaitUpdate(lastVrpnObject,100000000)) {
           SendObjects();
       }
@@ -61 +70 @@
     
     ssize_t rcv=dataSocket->RecvMessage(msg,sizeof(msg),dataSocketTimeout,NULL,NULL,&src_id);
-    if(rcv>0) {
+    connection_t* matchConnection=ConnectionOfSrcId(src_id);
+    //receive an ack
+    if(rcv==1) {
+      if(matchConnection!=NULL) matchConnection->lastAck=GetTime();
+      continue;//do not handle add or remove cases
+    }
+    if(rcv>0) {//add or remove
       int16_t id;
-      string object=string(msg,rcv-sizeof(id));
+      string objectName=string(msg,rcv-sizeof(id));
       memcpy(&id,&msg[rcv-sizeof(id)],sizeof(id));
       dataSocket->HostToNetwork((char*)&id,sizeof(id));
-      Printf("src id %i\n",src_id);
+     
       if(id>0) { //add object
-        if(src_id==vrpnobjects_list.size()) {
-          vector<VrpnObject*> vect;
-          vrpnobjects_list.push_back(vect);
-          //Printf("add vect %i\n",src_id);
+        if(matchConnection==NULL) {
+          connection_t connection;
+          connection.lastAck=GetTime();
+          connection.srcId=src_id;
+          connections.push_back(connection);
+          matchConnection=&(connections.back());
         }
         //assume we receive it in the good order
-        if(id==vrpnobjects_list.at(src_id).size()+1) {//id 0 is not used as we use positive and negative id for add/remove
-          Printf("adding object %s with id %i from client %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);
+        if(id==matchConnection->vrpnobjects.size()+1) {//id 0 is not used as we use positive and negative id for add/remove
+          Printf("adding object %s with id %i from client %i\n",objectName.c_str(),id,src_id);
+          VrpnObject* vrpnobject = new VrpnObject(objectName,vrpnclient->GetTabWidget());
+          matchConnection->vrpnobjects.push_back(vrpnobject);
         }else {
-          Err("adding object %s failed, expected id %i, got %i\n",object.c_str(),vrpnobjects_list.at(src_id).size()+1,id);
+          Err("adding object %s failed, expected id %i, got %i\n",objectName.c_str(),matchConnection->vrpnobjects.size()+1,id);
         }
       } else { //remove object
-        for (vector<VrpnObject*>::iterator it = vrpnobjects.begin();it < vrpnobjects.end(); it++) {
-          if ((*it)->ObjectName() == object) {
-            Printf("removing object %s with id %i from client %i\n",object.c_str(),-id,src_id);
-            auto iteratorEnd=remove(vrpnobjects_list.at(src_id).begin(), vrpnobjects_list.at(src_id).end(),(*it));
-            vrpnobjects_list.at(src_id).erase(iteratorEnd, vrpnobjects_list.at(src_id).end());
-            if(vrpnobjects_list.at(src_id).size()==0) {
-              Printf("not removing empty vrpnobjects_list (need code rewrite\n");
-              //le src_id va continuer a augmenter cote udtsocket et la comparaison if(src_id==vrpnobjects_list.size()) {
-                //ne va pas marcher; il faut identifier autrement les src
-              //auto iteratorEnd=remove(vrpnobjects_list.begin(), vrpnobjects_list.end(),vrpnobjects_list.at(src_id));
-              //vrpnobjects_list.erase(iteratorEnd, vrpnobjects_list.end());
+        for (auto it = matchConnection->vrpnobjects.begin();it < matchConnection->vrpnobjects.end(); it++) {
+          if ((*it)->ObjectName() == objectName) {
+            Printf("removing object %s with id %i from client %i\n",objectName.c_str(),-id,src_id);
+            delete (*it);
+            auto iteratorEnd=remove(matchConnection->vrpnobjects.begin(), matchConnection->vrpnobjects.end(),(*it));
+            matchConnection->vrpnobjects.erase(iteratorEnd, matchConnection->vrpnobjects.end());
+            if(matchConnection->vrpnobjects.size()==0) {
+              Printf("TODO: removing empty client %i\n",src_id);
+              //auto iteratorEnd=remove(connections.begin(), connections.end(),*matchConnection);
+              //connections.erase(iteratorEnd, connections.end());
             }
-            delete (*it);
-            vrpnobjects.erase(it);
             break;
           }
         }
-        
-      }
-    }
-  }
-}
-
-void VrpnLite::SendObjects(void) const{
+      }
+    }
+  }
+}
+
+
+VrpnLite::connection_t* VrpnLite::ConnectionOfSrcId(uint16_t srcId) {
+  for (int i=0;i<connections.size();i++) {
+    if(connections.at(i).srcId==srcId) return &(connections.at(i));
+  }
+  return NULL;
+}
+
+void VrpnLite::SendObjects(void) {
  
   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)];
+  
+  for (int i=0;i<connections.size();i++) {
+      if(connections.at(i).lastAck+REMOVE_TIMEOUT<GetTime() && connections.at(i).vrpnobjects.size()!=0) {
+        Printf("client %i did not send ack from %ins, removing it\n",i,REMOVE_TIMEOUT);
+        for (auto it = connections.at(i).vrpnobjects.begin();it < connections.at(i).vrpnobjects.end(); it++) {
+          delete (*it);
+        }
+        connections.at(i).vrpnobjects.clear();
+      }
+      
+      char datas[connections.at(i).vrpnobjects.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++) {
+      for (auto it = connections.at(i).vrpnobjects.begin();it < connections.at(i).vrpnobjects.end(); it++) {
         Vector3Df objectPosition;
         Quaternion objectQuaternion;

trunk/tools/VrpnLite/src/VrpnLite.h

@@ -37 +37 @@
 
 private:
+      typedef struct {
+          std::vector<flair::sensor::VrpnObject*> vrpnobjects;
+          flair::core::Time lastAck;
+          uint16_t srcId;
+        } connection_t;
       flair::gui::PushButton* killButton;
+      flair::sensor::VrpnClient* vrpnclient;
+      std::vector<connection_t> connections;
+      flair::core::UdpSocket* dataSocket;
       void Run(void);
-      flair::sensor::VrpnClient* vrpnclient;
-      std::vector<flair::sensor::VrpnObject*> vrpnobjects;
-      std::vector<std::vector<flair::sensor::VrpnObject*>> vrpnobjects_list;
-      flair::core::UdpSocket* dataSocket;
-      void SendObjects(void) const;
+      void SendObjects(void) ;
       int16_t ConvertPosition(float value) const;
       int16_t ConvertQuaternion(float value) const;
+      connection_t* ConnectionOfSrcId(uint16_t srcId);  
 };