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

Timestamp:

02/13/17 15:02:54 (7 years ago)

Author:

Sanahuja Guillaume

Message:

vrpn: improved timeout

File:

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

trunk/lib/FlairSensorActuator/src/VrpnClient_impl.cpp

@@ -42 +42 @@
 
 VrpnClient_impl::VrpnClient_impl(VrpnClient *self, std::string name,
-                                 std::string address, uint16_t us_period) {
-  this->us_period = us_period;
+                                 std::string address) {
   this->self = self;
   serialport = NULL;
+        isConnected=false;
 
   mutex = new Mutex(self, name);
@@ -162 +162 @@
 
 void VrpnClient_impl::Run(void) {
+        struct timeval timeout;
+        timeout.tv_sec=0;
+        timeout.tv_usec=100000;
+                        
   while (!self->ToBeStopped()) {
     if (UseXbee()) {
@@ -200 +204 @@
               t.quat[i] = quat[i];
             if (fabs(pos[0] > 10) || fabs(pos[1] > 10) || fabs(pos[2] > 10)) {
-              printf("prob pos %f %f %f\n", pos[0], pos[1], pos[2]);
+              Printf("prob pos %f %f %f\n", pos[0], pos[1], pos[2]);
             } else {
               // self->Printf("%i %f %f %f
@@ -210 +214 @@
         }
       }
-    } else {
-      connection->mainloop();
-      mutex->GetMutex();
-      for (unsigned int i = 0; i < trackables.size(); i++)
-        trackables.at(i)->mainloop();
-      mutex->ReleaseMutex();
-
-      self->SleepUS(us_period);
+    } else {//!UseXbee()
+                        if(connection->connected()==vrpn_true && !isConnected) {
+                                isConnected=true;
+                                Printf("VRPN connected\n");
+                        }
+                        if(connection->connected()==vrpn_false && isConnected) {
+                                isConnected=false;
+                                Printf("VRPN disconnected\n");
+                        }
+                        
+                        //timeout in mainloop is not well handled if not connected...
+                        if(isConnected) {
+                                //this is when trackables callbacks are called:
+                                if(connection->mainloop(&timeout)!=0) {
+                                        self->Warn("connection dropped\n");
+                                }
+                                //printf("%lld\n",GetTime()/(1000*1000));
+                                mutex->GetMutex();
+                                for (unsigned int i = 0; i < trackables.size(); i++)
+                                        trackables.at(i)->mainloop();
+                                mutex->ReleaseMutex();
+                        } else {
+                                connection->mainloop();
+                                self->SleepMS(10);
+                        }
     }
   }