Changeset 309 in flair-src for trunk/lib/FlairSensorActuator/src

Timestamp:

Mar 13, 2019, 4:40:18 PM (6 years ago)

Author:

Sanahuja Guillaume

Message:

vrpnlite support

Location:

trunk/lib/FlairSensorActuator/src

Files:

• VrpnClient.cpp (modified) (3 diffs)
• VrpnClient.h (modified) (3 diffs)
• VrpnClient_impl.cpp (modified) (10 diffs)
• VrpnObject.h (modified) (3 diffs)
• VrpnObject_impl.cpp (modified) (3 diffs)
• unexported/VrpnClient_impl.h (modified) (4 diffs)

trunk/lib/FlairSensorActuator/src/VrpnClient.cpp

@@ -44 +44 @@
     singleton = this;
   }
-
+  
   pimpl_ = new VrpnClient_impl(this, name, address);
 }
 
+
+VrpnClient::VrpnClient(std::string name,
+             uint16_t port,  uint8_t priority)
+  : Thread(getFrameworkManager(), name, priority) {
+        if (singleton != NULL) {
+    SimpleWarn("VrpnClient should be instanced only one time!\n");
+    SimpleWarn("Next calls to GetVrpnClient() will return the first created VrpnClient (%s)\n",singleton->ObjectName().c_str());
+  } else {
+    singleton = this;
+  }
+
+  pimpl_ = new VrpnClient_impl(this, name, port);
+}             
+             
 VrpnClient::VrpnClient(string name,
                        SerialPort *serialport, uint16_t us_period,
@@ -69 +83 @@
 }
 
+VrpnClient::ConnectionType_t VrpnClient::ConnectionType(void) const {
+  return pimpl_->connectionType;
+}
+
 Layout *VrpnClient::GetLayout(void) const {
   return (Layout *)(pimpl_->setup_tab);
@@ -75 +93 @@
 TabWidget *VrpnClient::GetTabWidget(void) const { return pimpl_->tab; }
 
-bool VrpnClient::UseXbee(void) const { return pimpl_->UseXbee(); }
-
 void VrpnClient::Run(void) { pimpl_->Run(); }
 

trunk/lib/FlairSensorActuator/src/VrpnClient.h

@@ -45 +45 @@
   * \brief Constructor
   *
-  * Construct a VrpnClient. Connection is done by IP.
+  * Construct a VrpnClient. Connection is done by IP to a vrpn server.
   *
   * \param name name
@@ -54 +54 @@
              std::string address,  uint8_t priority);
 
+  /*!
+  * \brief Constructor
+  *
+  * Construct a VrpnClient. Connection is done by IP to a vrpn-lite server (see tools/VrpnLite in flair-src)
+  *
+  * \param name name
+  * \param port server port
+  * \param priority priority of the Thread
+  */
+  VrpnClient(std::string name,
+             uint16_t port,  uint8_t priority);
+             
   /*!
   * \brief Constructor
@@ -87 +99 @@
   */
   gui::TabWidget *GetTabWidget(void) const;
-
-  /*!
-  * \brief Is XBee used?
-  *
-  * \return true if connection is based on XBee modem
-  */
-  bool UseXbee(void) const;
+  
+  typedef enum { Vrpn, VrpnLite, Xbee } ConnectionType_t;
+  
+  ConnectionType_t ConnectionType(void) const;
 
 private:

trunk/lib/FlairSensorActuator/src/VrpnClient_impl.cpp

@@ -30 +30 @@
 #include <Quaternion.h>
 #include <Mutex.h>
+#include <UdpSocket.h>
 #include <math.h>
 
@@ -38 +39 @@
 using namespace flair::sensor;
 
-VrpnClient_impl::VrpnClient_impl(VrpnClient *self, std::string name,
+VrpnClient_impl::VrpnClient_impl(VrpnClient *self, string name,
                                  std::string address) {
   this->self = self;
   this->address = address;
-  serialport = NULL;
+  isConnected=false;
+  connectionType=VrpnClient::Vrpn;
+
+  connection = vrpn_get_connection_by_name(address.c_str());
+
+  CommonConstructor(name);
+  
+  Printf("Connecting to VRPN server on %s\n",address.c_str());
+}
+
+VrpnClient_impl::VrpnClient_impl(VrpnClient *self, string name,
+                                 uint16_t port) {
+  this->self = self;
         isConnected=false;
-
+  connectionType=VrpnClient::VrpnLite;
+
+  dataSocket =new UdpSocket(getFrameworkManager(), "data_socket", port);
+
+  CommonConstructor(name);
+  
+  Printf("Connecting to VRPN-lite server on port %i\n",port);
+}
+
+VrpnClient_impl::VrpnClient_impl(VrpnClient *self, string name,
+                                 SerialPort *serialport, uint16_t us_period) {
+  this->us_period = us_period;
+  this->self = self;
+  this->serialport = serialport;
+  connectionType=VrpnClient::Xbee;
+
+  serialport->SetBaudrate(111111);
+  serialport->SetRxTimeout(us_period * 1000);
+
+  CommonConstructor(name);
+  
+  Printf("Connecting to VRPN server through xbee on %s\n",serialport->ObjectName().c_str());
+}
+
+void VrpnClient_impl::CommonConstructor(std::string name) {
   mutex = new Mutex(self, name);
-
-  connection = vrpn_get_connection_by_name(address.c_str());
-
+  
   // station sol
   main_tab = new Tab(getFrameworkManager()->GetTabWidget(), name);
@@ -56 +91 @@
   rotation_1 = new OneAxisRotation(setup_tab->NewRow(), "post rotation 1",OneAxisRotation::PreRotation);
   rotation_2 = new OneAxisRotation(setup_tab->NewRow(), "post rotation 2",OneAxisRotation::PreRotation);
-  
-  Printf("Connecting to VRPN server on %s\n",address.c_str());
-}
-
-VrpnClient_impl::VrpnClient_impl(VrpnClient *self, std::string name,
-                                 SerialPort *serialport, uint16_t us_period) {
-  this->us_period = us_period;
-  this->self = self;
-  this->serialport = serialport;
-  connection = NULL;
-  mutex = new Mutex(self, name);
-
-  serialport->SetBaudrate(111111);
-  serialport->SetRxTimeout(us_period * 1000);
-
-  // station sol
-  main_tab = new Tab(getFrameworkManager()->GetTabWidget(), name);
-  tab = new TabWidget(main_tab->NewRow(), name);
-  setup_tab = new Tab(tab, "Reglages");
-
-  rotation_1 = new OneAxisRotation(setup_tab->NewRow(), "post rotation 1",OneAxisRotation::PreRotation);
-  rotation_2 = new OneAxisRotation(setup_tab->NewRow(), "post rotation 2",OneAxisRotation::PreRotation);
-  
-  Printf("Connecting to VRPN server on %s\n",serialport->ObjectName().c_str());
 }
 
 VrpnClient_impl::~VrpnClient_impl() {
-  if (!UseXbee()) {
+  if (connectionType==VrpnClient::Vrpn) {
     // on fait une copie car le delete touche a trackables_copy via
     // RemoveTrackable
@@ -90 +101 @@
       delete trackables_copy.at(i)->self;
     // trackables.clear();
-  } else {
+  } else if(connectionType==VrpnClient::Xbee || connectionType==VrpnClient::VrpnLite) {
     // on fait une copie car le delete touche a xbee_objects_copy via
     // RemoveTrackable
-    vector<xbee_object> xbee_objects_copy = xbee_objects;
-    for (unsigned int i = 0; i < xbee_objects_copy.size(); i++)
-      delete xbee_objects_copy.at(i).vrpnobject->self;
+    vector<liteObject_t> liteObjects_copy = liteObjects;
+    for (unsigned int i = 0; i < liteObjects_copy.size(); i++)
+      delete liteObjects_copy.at(i).vrpnobject->self;
   }
 
   delete main_tab;
 
-  if (!UseXbee()) {
+  if (connectionType==VrpnClient::Vrpn) {
     // it will automatically delete connection
     connection->removeReference();
@@ -117 +128 @@
 
 void VrpnClient_impl::AddTrackable(VrpnObject_impl *obj) {
-  mutex->GetMutex();
-  trackables.push_back(obj);
-  mutex->ReleaseMutex();
+  if (connectionType==VrpnClient::Vrpn) {
+    mutex->GetMutex();
+    trackables.push_back(obj);
+    mutex->ReleaseMutex();
+  } else {
+    self->Warn("AddTrackable called but not in vrpn mode\n");
+  }
+}
+
+void VrpnClient_impl::AddTrackable(VrpnObject_impl *obj, uint8_t id) {
+  if (connectionType==VrpnClient::VrpnLite || connectionType==VrpnClient::Xbee) {
+    liteObject_t tmp;
+    tmp.vrpnobject = obj;
+    tmp.id = id;
+    mutex->GetMutex();
+    liteObjects.push_back(tmp);
+    mutex->ReleaseMutex();
+  } else {
+    self->Warn("AddTrackable called but not in vrpnlite nor in xbee mode\n");
+  }
 }
 
 void VrpnClient_impl::RemoveTrackable(VrpnObject_impl *obj) {
   mutex->GetMutex();
-  for (vector<VrpnObject_impl *>::iterator it = trackables.begin();
-       it < trackables.end(); it++) {
-    if (*it == obj) {
-      trackables.erase(it);
-      break;
+  if (connectionType==VrpnClient::Vrpn) {
+    for (vector<VrpnObject_impl *>::iterator it = trackables.begin();
+         it < trackables.end(); it++) {
+      if (*it == obj) {
+        trackables.erase(it);
+        break;
+      }
     }
   }
-  for (vector<xbee_object>::iterator it = xbee_objects.begin();
-       it < xbee_objects.end(); it++) {
-    if ((*it).vrpnobject == obj) {
-      xbee_objects.erase(it);
-      break;
+  if (connectionType==VrpnClient::VrpnLite || connectionType==VrpnClient::Xbee) {
+    for (vector<liteObject_t>::iterator it = liteObjects.begin();
+         it < liteObjects.end(); it++) {
+      if ((*it).vrpnobject == obj) {
+        liteObjects.erase(it);
+        break;
+      }
     }
   }
   mutex->ReleaseMutex();
-}
-
-void VrpnClient_impl::AddTrackable(VrpnObject_impl *obj, uint8_t id) {
-  xbee_object tmp;
-  tmp.vrpnobject = obj;
-  tmp.id = id;
-  mutex->GetMutex();
-  xbee_objects.push_back(tmp);
-  mutex->ReleaseMutex();
-}
-
-bool VrpnClient_impl::UseXbee(void) {
-  if (connection == NULL) {
-    return true;
-  } else {
-    return false;
-  }
 }
 
@@ -164 +179 @@
                         
   while (!self->ToBeStopped()) {
-    if (UseXbee()) {
+    if (connectionType==VrpnClient::Xbee) {
       ssize_t read = 0;
       uint8_t response[38] = {0};
@@ -193 +208 @@
 
           mutex->GetMutex();
-          if (id < xbee_objects.size()) {
+          if (id < liteObjects.size()) {
             memcpy(pos, &response[9], sizeof(pos));
             memcpy(quat, &response[9] + sizeof(pos), sizeof(quat));
@@ -205 +220 @@
               // self->Printf("%i %f %f %f
               // %f\n",id,pos[0],pos[1],pos[2],(float)self->GetTime()/(1000*1000));
-              VrpnObject_impl::handle_pos(xbee_objects.at(id).vrpnobject, t);
+              VrpnObject_impl::handle_pos(liteObjects.at(id).vrpnobject, t);
             }
           }
@@ -211 +226 @@
         }
       }
-    } else {//!UseXbee()
+    } else if(connectionType==VrpnClient::Vrpn) {
         if(connection->connected()==vrpn_true && !isConnected) {
             isConnected=true;
@@ -235 +250 @@
             self->SleepMS(10);
         }
+    }else if(connectionType==VrpnClient::VrpnLite) {
+      vrpn_TRACKERCB t;
+      float pos[3];
+      float quat[4];
+      Time time;
+      uint8_t id;
+      char datas[sizeof(id) + sizeof(pos)+sizeof(quat)+ sizeof(time)];
+      int rcv=dataSocket->RecvMessage(datas,sizeof(datas),50*1000*1000);
+      if(rcv!=sizeof(datas)) continue;  
+      id = datas[0];
+      memcpy(pos, datas+sizeof(id), sizeof(pos));
+      memcpy(quat, datas +sizeof(id)+ sizeof(pos), sizeof(quat));
+      memcpy(&time, datas+sizeof(id) + sizeof(pos)+sizeof(quat), sizeof(time));
+      
+      for(int i=0;i<3;i++) dataSocket->NetworkToHost((char*)(&pos[i]),sizeof(pos[i]));
+      for(int i=0;i<4;i++) dataSocket->NetworkToHost((char*)(&quat[i]),sizeof(quat[i]));
+      dataSocket->NetworkToHost((char*)(&time),sizeof(time));
+
+      mutex->GetMutex();
+      if (id < liteObjects.size()) {
+        for (int i = 0; i < 3; i++) t.pos[i] = pos[i];
+        // warning: t.quat is defined as (qx,qy,qz,qw), which is different from
+        // flair::core::Quaternion
+        t.quat[0] = quat[1];
+        t.quat[1] = quat[2];
+        t.quat[2] = quat[3];
+        t.quat[3] = quat[0];
+        t.msg_time.tv_sec=time/((Time)1000000000);
+        t.msg_time.tv_usec=(time%((Time)1000000000))/((Time)1000);
+        //Printf("%i %lld %lld %lld\n",id,time,t.msg_time.tv_sec,t.msg_time.tv_usec);
+        VrpnObject_impl::handle_pos(liteObjects.at(id).vrpnobject, t);
+      }
+      mutex->ReleaseMutex();
     }
   }

trunk/lib/FlairSensorActuator/src/VrpnObject.h

@@ -50 +50 @@
   * \brief Constructor
   *
-  * Construct a VrpnObject. Connection is done by IP.
+  * Construct a VrpnObject. Connection is done by IP. (vrpn)
   *
   * \param name VRPN object name, should be the same as defined in the server
@@ -62 +62 @@
   * \brief Constructor
   *
-  * Construct a VrpnObject. Connection is done by xbee.
+  * Construct a VrpnObject. Connection is done by xbee or vrpnlite (see tools/VrpnLite in flair-src)
   *
   * \param name name
-  * \param id VRPN object id, should be the same as defined in the xbee bridge
+  * \param id VRPN object id, should be the same as defined in the xbee bridge or vrpnlite tool
   * \param tab Tab for the user interface
   * \param client VrpnClient of the connection, if unspecified, use the default one
@@ -88 +88 @@
   * \brief Get Last Packet Time
   *
-  * \return Time of last received packe
+  * \return Time of last received packet
   */
   core::Time GetLastPacketTime(void) const;

trunk/lib/FlairSensorActuator/src/VrpnObject_impl.cpp

@@ -48 +48 @@
                 return;
         }
-  if (id == -1 && parent->UseXbee()) {
+  if (id == -1 && GetVrpnClient()->ConnectionType()==VrpnClient::Xbee) {
     self->Err("erreur aucun identifiant specifie pour la connexion Xbee\n");
   }
-  if (id != -1 && !parent->UseXbee()) {
-    self->Warn(
-        "identifiant pour la connexion Xbee ignore car pas en mode Xbee\n");
+  if (id == -1 && GetVrpnClient()->ConnectionType()==VrpnClient::VrpnLite) {
+    self->Err("erreur aucun identifiant specifie pour la connexion VrpnLite\n");
+  }
+  if (id != -1 && GetVrpnClient()->ConnectionType()==VrpnClient::Vrpn) {
+    self->Warn("identifiant pour la connexion ignore car inutile en mode Vrpn\n");
   }
   
@@ -84 +86 @@
   z_plot->AddCurve(output->Element(6));
   
-  if (parent->UseXbee()) {
-    tracker = NULL;
+  if (GetVrpnClient()->ConnectionType()==VrpnClient::Xbee) {
     parent->pimpl_->AddTrackable(this, id);
-  } else {
+  } else if(GetVrpnClient()->ConnectionType()==VrpnClient::Vrpn){
     tracker = new vrpn_Tracker_Remote(name.c_str(), parent->pimpl_->connection);
     tracker->register_change_handler(this, handle_pos);
     tracker->shutup = true;
     parent->pimpl_->AddTrackable(this);
+  } else if(GetVrpnClient()->ConnectionType()==VrpnClient::VrpnLite){
+    parent->pimpl_->AddTrackable(this, id);
   }
   
@@ -99 +102 @@
 VrpnObject_impl::~VrpnObject_impl(void) {
   parent->pimpl_->RemoveTrackable(this);
-  if (tracker != NULL) {// normal
+  if (GetVrpnClient()->ConnectionType()==VrpnClient::Vrpn) {
     tracker->unregister_change_handler(this, handle_pos);
     delete tracker;

trunk/lib/FlairSensorActuator/src/unexported/VrpnClient_impl.h

@@ -29 +29 @@
 class Mutex;
 class SerialPort;
+class UdpSocket;
 }
 namespace gui {
@@ -50 +51 @@
   VrpnClient_impl(flair::sensor::VrpnClient *self, std::string name,
                   flair::core::SerialPort *serialport, uint16_t us_period);
+  VrpnClient_impl(flair::sensor::VrpnClient *self, std::string name,
+                  uint16_t port);
   ~VrpnClient_impl();
   void AddTrackable(VrpnObject_impl *obj);    // normal
@@ -56 +59 @@
   void ComputeRotations(flair::core::Vector3Df &point);
   void ComputeRotations(flair::core::Quaternion &quat);
-  bool UseXbee(void);
   void Run(void);
   flair::gui::Tab *setup_tab;
   flair::gui::TabWidget *tab;
   vrpn_Connection *connection;
+  flair::sensor::VrpnClient::ConnectionType_t connectionType;
 
 private:
+  void CommonConstructor(std::string name);
   flair::sensor::VrpnClient *self;
   flair::core::Mutex *mutex;
   uint16_t us_period;
   std::vector<VrpnObject_impl *> trackables;
-  typedef struct xbee_object {
+  typedef struct liteObject_t {
     VrpnObject_impl *vrpnobject;
     uint8_t id;
-  } xbee_object;
+  } liteObject_t;
 
-  std::vector<xbee_object> xbee_objects;
+  std::vector<liteObject_t> liteObjects;
   flair::gui::Tab *main_tab;
   flair::core::OneAxisRotation *rotation_1, *rotation_2;
@@ -78 +82 @@
         bool isConnected;//only for ip connection, not for xbee
   std::string address;
+  flair::core::UdpSocket* dataSocket;
+  
 };