Changeset 448 in flair-src for trunk/tools/VrpnLite
- Timestamp:
- Sep 20, 2021, 5:16:50 PM (3 years ago)
- Location:
- trunk/tools/VrpnLite/src
- Files:
-
- 2 edited
Legend:
- Unmodified
- Added
- Removed
-
trunk/tools/VrpnLite/src/VrpnLite.cpp
r447 r448 30 30 using namespace flair::gui; 31 31 32 //if we don'receive ack from client, do not send vrpnobjects anymore 33 #define REMOVE_TIMEOUT (Time)2000000000 34 35 32 36 VrpnLite::VrpnLite(int vrpnLitePort,string vrpnServerAddress): Thread(getFrameworkManager(),"VrpnLite",90) { 33 37 vrpnclient=new VrpnClient("vrpn", vrpnServerAddress,80); … … 48 52 while (!ToBeStopped()) { 49 53 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 } 50 59 51 if( vrpnobjects.size()>0) {60 if(lastVrpnObject!=NULL) { 52 61 //wait for last one to be sure all are up to date 53 62 //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)) { 55 64 SendObjects(); 56 65 } … … 61 70 62 71 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 64 79 int16_t id; 65 string object =string(msg,rcv-sizeof(id));80 string objectName=string(msg,rcv-sizeof(id)); 66 81 memcpy(&id,&msg[rcv-sizeof(id)],sizeof(id)); 67 82 dataSocket->HostToNetwork((char*)&id,sizeof(id)); 68 Printf("src id %i\n",src_id);83 69 84 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()); 74 91 } 75 92 //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); 81 97 }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); 83 99 } 84 100 } 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()); 96 111 } 97 delete (*it);98 vrpnobjects.erase(it);99 112 break; 100 113 } 101 114 } 102 103 } 104 } 105 } 106 } 107 108 void VrpnLite::SendObjects(void) const{ 115 } 116 } 117 } 118 } 119 120 121 VrpnLite::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 128 void VrpnLite::SendObjects(void) { 109 129 110 130 int16_t position[3]; 111 131 int16_t quaternion[4]; 112 132 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)]; 117 144 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++) { 119 146 Vector3Df objectPosition; 120 147 Quaternion objectQuaternion; -
trunk/tools/VrpnLite/src/VrpnLite.h
r446 r448 37 37 38 38 private: 39 typedef struct { 40 std::vector<flair::sensor::VrpnObject*> vrpnobjects; 41 flair::core::Time lastAck; 42 uint16_t srcId; 43 } connection_t; 39 44 flair::gui::PushButton* killButton; 45 flair::sensor::VrpnClient* vrpnclient; 46 std::vector<connection_t> connections; 47 flair::core::UdpSocket* dataSocket; 40 48 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) ; 46 50 int16_t ConvertPosition(float value) const; 47 51 int16_t ConvertQuaternion(float value) const; 52 connection_t* ConnectionOfSrcId(uint16_t srcId); 48 53 }; 49 54
Note:
See TracChangeset
for help on using the changeset viewer.