Changeset 448 in flair-src


Ignore:
Timestamp:
Sep 20, 2021, 5:16:50 PM (3 years ago)
Author:
Sanahuja Guillaume
Message:

handle ack in vrpnlite

Location:
trunk
Files:
3 edited

Legend:

Unmodified
Added
Removed
  • trunk/lib/FlairSensorActuator/src/VrpnClient_impl.cpp

    r447 r448  
    202202        timeout.tv_sec=0;
    203203        timeout.tv_usec=100000;
     204  uint16_t vrpnliteCounter=0;
    204205                       
    205206  while (!self->ToBeStopped()) {
     
    321322     
    322323      mutex->ReleaseMutex();
     324     
     325      if(trackables.size()!=0) {
     326        vrpnliteCounter++;//send ack to monitor connection on vrpnlite
     327        //on bth, vrpn is configured to 50Hz, send ack at 1Hz
     328        if(vrpnliteCounter==50) {
     329          vrpnliteCounter=0;
     330          char data=0;
     331          dataSocket->SendMessage(&data,sizeof(char));
     332        }
     333      }
    323334    }
    324335  }
  • trunk/tools/VrpnLite/src/VrpnLite.cpp

    r447 r448  
    3030using namespace flair::gui;
    3131
     32//if we don'receive ack from client, do not send vrpnobjects anymore
     33#define REMOVE_TIMEOUT (Time)2000000000
     34
     35
    3236VrpnLite::VrpnLite(int vrpnLitePort,string vrpnServerAddress): Thread(getFrameworkManager(),"VrpnLite",90) {
    3337  vrpnclient=new VrpnClient("vrpn", vrpnServerAddress,80);
     
    4852  while (!ToBeStopped()) {
    4953    if(killButton->Clicked()) SafeStop();
     54    //get last vrpnobject
     55    VrpnObject* lastVrpnObject=NULL;
     56    if(connections.size()!=0) {
     57      if(connections.back().vrpnobjects.size()!=0) lastVrpnObject=lastVrpnObject=connections.back().vrpnobjects.back();
     58    }
    5059   
    51     if(vrpnobjects.size()>0) {
     60    if(lastVrpnObject!=NULL) {
    5261      //wait for last one to be sure all are up to date
    5362      //but if object is not tracked we send nothing... send it unsynchronized???
    54       if(WaitUpdate(vrpnobjects.at(vrpnobjects.size()-1),100000000)) {
     63      if(WaitUpdate(lastVrpnObject,100000000)) {
    5564          SendObjects();
    5665      }
     
    6170   
    6271    ssize_t rcv=dataSocket->RecvMessage(msg,sizeof(msg),dataSocketTimeout,NULL,NULL,&src_id);
    63     if(rcv>0) {
     72    connection_t* matchConnection=ConnectionOfSrcId(src_id);
     73    //receive an ack
     74    if(rcv==1) {
     75      if(matchConnection!=NULL) matchConnection->lastAck=GetTime();
     76      continue;//do not handle add or remove cases
     77    }
     78    if(rcv>0) {//add or remove
    6479      int16_t id;
    65       string object=string(msg,rcv-sizeof(id));
     80      string objectName=string(msg,rcv-sizeof(id));
    6681      memcpy(&id,&msg[rcv-sizeof(id)],sizeof(id));
    6782      dataSocket->HostToNetwork((char*)&id,sizeof(id));
    68       Printf("src id %i\n",src_id);
     83     
    6984      if(id>0) { //add object
    70         if(src_id==vrpnobjects_list.size()) {
    71           vector<VrpnObject*> vect;
    72           vrpnobjects_list.push_back(vect);
    73           //Printf("add vect %i\n",src_id);
     85        if(matchConnection==NULL) {
     86          connection_t connection;
     87          connection.lastAck=GetTime();
     88          connection.srcId=src_id;
     89          connections.push_back(connection);
     90          matchConnection=&(connections.back());
    7491        }
    7592        //assume we receive it in the good order
    76         if(id==vrpnobjects_list.at(src_id).size()+1) {//id 0 is not used as we use positive and negative id for add/remove
    77           Printf("adding object %s with id %i from client %i\n",object.c_str(),id,src_id);
    78           VrpnObject* vrpnobject = new VrpnObject(object,vrpnclient->GetTabWidget());
    79           vrpnobjects.push_back(vrpnobject);
    80           vrpnobjects_list.at(src_id).push_back(vrpnobject);
     93        if(id==matchConnection->vrpnobjects.size()+1) {//id 0 is not used as we use positive and negative id for add/remove
     94          Printf("adding object %s with id %i from client %i\n",objectName.c_str(),id,src_id);
     95          VrpnObject* vrpnobject = new VrpnObject(objectName,vrpnclient->GetTabWidget());
     96          matchConnection->vrpnobjects.push_back(vrpnobject);
    8197        }else {
    82           Err("adding object %s failed, expected id %i, got %i\n",object.c_str(),vrpnobjects_list.at(src_id).size()+1,id);
     98          Err("adding object %s failed, expected id %i, got %i\n",objectName.c_str(),matchConnection->vrpnobjects.size()+1,id);
    8399        }
    84100      } else { //remove object
    85         for (vector<VrpnObject*>::iterator it = vrpnobjects.begin();it < vrpnobjects.end(); it++) {
    86           if ((*it)->ObjectName() == object) {
    87             Printf("removing object %s with id %i from client %i\n",object.c_str(),-id,src_id);
    88             auto iteratorEnd=remove(vrpnobjects_list.at(src_id).begin(), vrpnobjects_list.at(src_id).end(),(*it));
    89             vrpnobjects_list.at(src_id).erase(iteratorEnd, vrpnobjects_list.at(src_id).end());
    90             if(vrpnobjects_list.at(src_id).size()==0) {
    91               Printf("not removing empty vrpnobjects_list (need code rewrite\n");
    92               //le src_id va continuer a augmenter cote udtsocket et la comparaison if(src_id==vrpnobjects_list.size()) {
    93                 //ne va pas marcher; il faut identifier autrement les src
    94               //auto iteratorEnd=remove(vrpnobjects_list.begin(), vrpnobjects_list.end(),vrpnobjects_list.at(src_id));
    95               //vrpnobjects_list.erase(iteratorEnd, vrpnobjects_list.end());
     101        for (auto it = matchConnection->vrpnobjects.begin();it < matchConnection->vrpnobjects.end(); it++) {
     102          if ((*it)->ObjectName() == objectName) {
     103            Printf("removing object %s with id %i from client %i\n",objectName.c_str(),-id,src_id);
     104            delete (*it);
     105            auto iteratorEnd=remove(matchConnection->vrpnobjects.begin(), matchConnection->vrpnobjects.end(),(*it));
     106            matchConnection->vrpnobjects.erase(iteratorEnd, matchConnection->vrpnobjects.end());
     107            if(matchConnection->vrpnobjects.size()==0) {
     108              Printf("TODO: removing empty client %i\n",src_id);
     109              //auto iteratorEnd=remove(connections.begin(), connections.end(),*matchConnection);
     110              //connections.erase(iteratorEnd, connections.end());
    96111            }
    97             delete (*it);
    98             vrpnobjects.erase(it);
    99112            break;
    100113          }
    101114        }
    102        
    103       }
    104     }
    105   }
    106 }
    107 
    108 void VrpnLite::SendObjects(void) const{
     115      }
     116    }
     117  }
     118}
     119
     120
     121VrpnLite::connection_t* VrpnLite::ConnectionOfSrcId(uint16_t srcId) {
     122  for (int i=0;i<connections.size();i++) {
     123    if(connections.at(i).srcId==srcId) return &(connections.at(i));
     124  }
     125  return NULL;
     126}
     127
     128void VrpnLite::SendObjects(void) {
    109129 
    110130  int16_t position[3];
    111131  int16_t quaternion[4];
    112132  Time time;
    113  
    114   //for (vector<vector<VrpnObject*>>::const_iterator connection= vrpnobjects_list.begin();connection < vrpnobjects_list.end(); connection++) {
    115   for (int i=0;i<vrpnobjects_list.size();i++) {
    116       char datas[vrpnobjects_list.at(i).size()*(sizeof(position)+sizeof(quaternion))+ sizeof(time)];
     133 
     134  for (int i=0;i<connections.size();i++) {
     135      if(connections.at(i).lastAck+REMOVE_TIMEOUT<GetTime() && connections.at(i).vrpnobjects.size()!=0) {
     136        Printf("client %i did not send ack from %ins, removing it\n",i,REMOVE_TIMEOUT);
     137        for (auto it = connections.at(i).vrpnobjects.begin();it < connections.at(i).vrpnobjects.end(); it++) {
     138          delete (*it);
     139        }
     140        connections.at(i).vrpnobjects.clear();
     141      }
     142     
     143      char datas[connections.at(i).vrpnobjects.size()*(sizeof(position)+sizeof(quaternion))+ sizeof(time)];
    117144      char *datasPtr=datas;
    118       for (vector<VrpnObject*>::const_iterator it = vrpnobjects_list.at(i).begin();it < vrpnobjects_list.at(i).end(); it++) {
     145      for (auto it = connections.at(i).vrpnobjects.begin();it < connections.at(i).vrpnobjects.end(); it++) {
    119146        Vector3Df objectPosition;
    120147        Quaternion objectQuaternion;
  • trunk/tools/VrpnLite/src/VrpnLite.h

    r446 r448  
    3737
    3838private:
     39      typedef struct {
     40          std::vector<flair::sensor::VrpnObject*> vrpnobjects;
     41          flair::core::Time lastAck;
     42          uint16_t srcId;
     43        } connection_t;
    3944      flair::gui::PushButton* killButton;
     45      flair::sensor::VrpnClient* vrpnclient;
     46      std::vector<connection_t> connections;
     47      flair::core::UdpSocket* dataSocket;
    4048      void Run(void);
    41       flair::sensor::VrpnClient* vrpnclient;
    42       std::vector<flair::sensor::VrpnObject*> vrpnobjects;
    43       std::vector<std::vector<flair::sensor::VrpnObject*>> vrpnobjects_list;
    44       flair::core::UdpSocket* dataSocket;
    45       void SendObjects(void) const;
     49      void SendObjects(void) ;
    4650      int16_t ConvertPosition(float value) const;
    4751      int16_t ConvertQuaternion(float value) const;
     52      connection_t* ConnectionOfSrcId(uint16_t srcId); 
    4853};
    4954
Note: See TracChangeset for help on using the changeset viewer.