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

Timestamp:

Sep 3, 2020, 8:54:16 AM (4 years ago)

Author:

Sanahuja Guillaume

Message:

allow mutliple connection on udp (only nrt for the moment)

Location:

trunk/tools/VrpnLite/src

Files:

• VrpnLite.cpp (modified) (4 diffs)
• VrpnLite.h (modified) (1 diff)

trunk/tools/VrpnLite/src/VrpnLite.cpp

@@ -37 +37 @@
   Time dataSocketTimeout;
   char msg[256];
+  int src_id;
   
   while (!ToBeStopped()) {
-    
     if(vrpnobjects.size()>0) {
       //wait for last one to be sure all are up to date
@@ -51 +51 @@
     }
     
-    ssize_t rcv=dataSocket->RecvMessage(msg,sizeof(msg),dataSocketTimeout);
+    ssize_t rcv=dataSocket->RecvMessage(msg,sizeof(msg),dataSocketTimeout,NULL,NULL,&src_id);
     if(rcv>0) {
       string object=msg;
@@ -57 +57 @@
       memcpy(&id,&msg[rcv-2],sizeof(id));
       dataSocket->HostToNetwork((char*)&id,sizeof(id));
+      
+      if(src_id==vrpnobjects_list.size()) {
+        vector<VrpnObject*> vect;
+        vrpnobjects_list.push_back(vect);
+        Printf("add vect %i\n",src_id);
+      }
       //assume we receive it in the good order
-      if(id==vrpnobjects.size()) {
-        Printf("adding object %s with id %i\n",object.c_str(),id);
+      if(id==vrpnobjects_list.at(src_id).size()) {
+        Printf("adding object %s with id %i from %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);
       }else {
         Err("adding object %s failed, expected id %i, got %i\n",object.c_str(),vrpnobjects.size(),id);
@@ -70 +77 @@
 
 void VrpnLite::SendObjects(void) const{
-  int16_t position[3];
+ return; 
+ 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;
+ 
+  //for (vector<vector<VrpnObject*>>::const_iterator connection= vrpnobjects_list.begin();connection < vrpnobjects_list.end(); connection++) {
+  for (int i=0;i<vrpnobjects_list.size();i++) {
+      for (vector<VrpnObject*>::const_iterator it = vrpnobjects_list.at(i).begin();it < vrpnobjects_list.at(i).end(); it++) {
+        Vector3Df objectPosition;
+        Quaternion objectQuaternion;
+        const VrpnObject* vrpnobject=*it;
 
-    vrpnobject->GetPosition(objectPosition);
-    vrpnobject->GetQuaternion(objectQuaternion);
-    time=vrpnobject->GetLastPacketTime();
+        vrpnobject->GetPosition(objectPosition);
+        vrpnobject->GetQuaternion(objectQuaternion);
+        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);
+        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]));
+        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]));
 
-    memcpy(datasPtr,position, sizeof(position));
-    datasPtr+=sizeof(position);
-    memcpy(datasPtr,quaternion, sizeof(quaternion));
-    datasPtr+=sizeof(quaternion);
+        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),i);
   }
-  
-  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));
 }
 

trunk/tools/VrpnLite/src/VrpnLite.h

@@ -37 +37 @@
       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;