Changeset 309 in flair-src


Ignore:
Timestamp:
Mar 13, 2019, 4:40:18 PM (3 years ago)
Author:
Sanahuja Guillaume
Message:

vrpnlite support

Location:
trunk/lib/FlairSensorActuator/src
Files:
6 edited

Legend:

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

    r294 r309  
    4444    singleton = this;
    4545  }
    46 
     46 
    4747  pimpl_ = new VrpnClient_impl(this, name, address);
    4848}
    4949
     50
     51VrpnClient::VrpnClient(std::string name,
     52             uint16_t port,  uint8_t priority)
     53  : Thread(getFrameworkManager(), name, priority) {
     54        if (singleton != NULL) {
     55    SimpleWarn("VrpnClient should be instanced only one time!\n");
     56    SimpleWarn("Next calls to GetVrpnClient() will return the first created VrpnClient (%s)\n",singleton->ObjectName().c_str());
     57  } else {
     58    singleton = this;
     59  }
     60
     61  pimpl_ = new VrpnClient_impl(this, name, port);
     62}             
     63             
    5064VrpnClient::VrpnClient(string name,
    5165                       SerialPort *serialport, uint16_t us_period,
     
    6983}
    7084
     85VrpnClient::ConnectionType_t VrpnClient::ConnectionType(void) const {
     86  return pimpl_->connectionType;
     87}
     88
    7189Layout *VrpnClient::GetLayout(void) const {
    7290  return (Layout *)(pimpl_->setup_tab);
     
    7593TabWidget *VrpnClient::GetTabWidget(void) const { return pimpl_->tab; }
    7694
    77 bool VrpnClient::UseXbee(void) const { return pimpl_->UseXbee(); }
    78 
    7995void VrpnClient::Run(void) { pimpl_->Run(); }
    8096
  • trunk/lib/FlairSensorActuator/src/VrpnClient.h

    r136 r309  
    4545  * \brief Constructor
    4646  *
    47   * Construct a VrpnClient. Connection is done by IP.
     47  * Construct a VrpnClient. Connection is done by IP to a vrpn server.
    4848  *
    4949  * \param name name
     
    5454             std::string address,  uint8_t priority);
    5555
     56  /*!
     57  * \brief Constructor
     58  *
     59  * Construct a VrpnClient. Connection is done by IP to a vrpn-lite server (see tools/VrpnLite in flair-src)
     60  *
     61  * \param name name
     62  * \param port server port
     63  * \param priority priority of the Thread
     64  */
     65  VrpnClient(std::string name,
     66             uint16_t port,  uint8_t priority);
     67             
    5668  /*!
    5769  * \brief Constructor
     
    8799  */
    88100  gui::TabWidget *GetTabWidget(void) const;
    89 
    90   /*!
    91   * \brief Is XBee used?
    92   *
    93   * \return true if connection is based on XBee modem
    94   */
    95   bool UseXbee(void) const;
     101 
     102  typedef enum { Vrpn, VrpnLite, Xbee } ConnectionType_t;
     103 
     104  ConnectionType_t ConnectionType(void) const;
    96105
    97106private:
  • trunk/lib/FlairSensorActuator/src/VrpnClient_impl.cpp

    r294 r309  
    3030#include <Quaternion.h>
    3131#include <Mutex.h>
     32#include <UdpSocket.h>
    3233#include <math.h>
    3334
     
    3839using namespace flair::sensor;
    3940
    40 VrpnClient_impl::VrpnClient_impl(VrpnClient *self, std::string name,
     41VrpnClient_impl::VrpnClient_impl(VrpnClient *self, string name,
    4142                                 std::string address) {
    4243  this->self = self;
    4344  this->address = address;
    44   serialport = NULL;
     45  isConnected=false;
     46  connectionType=VrpnClient::Vrpn;
     47
     48  connection = vrpn_get_connection_by_name(address.c_str());
     49
     50  CommonConstructor(name);
     51 
     52  Printf("Connecting to VRPN server on %s\n",address.c_str());
     53}
     54
     55VrpnClient_impl::VrpnClient_impl(VrpnClient *self, string name,
     56                                 uint16_t port) {
     57  this->self = self;
    4558        isConnected=false;
    46 
     59  connectionType=VrpnClient::VrpnLite;
     60
     61  dataSocket =new UdpSocket(getFrameworkManager(), "data_socket", port);
     62
     63  CommonConstructor(name);
     64 
     65  Printf("Connecting to VRPN-lite server on port %i\n",port);
     66}
     67
     68VrpnClient_impl::VrpnClient_impl(VrpnClient *self, string name,
     69                                 SerialPort *serialport, uint16_t us_period) {
     70  this->us_period = us_period;
     71  this->self = self;
     72  this->serialport = serialport;
     73  connectionType=VrpnClient::Xbee;
     74
     75  serialport->SetBaudrate(111111);
     76  serialport->SetRxTimeout(us_period * 1000);
     77
     78  CommonConstructor(name);
     79 
     80  Printf("Connecting to VRPN server through xbee on %s\n",serialport->ObjectName().c_str());
     81}
     82
     83void VrpnClient_impl::CommonConstructor(std::string name) {
    4784  mutex = new Mutex(self, name);
    48 
    49   connection = vrpn_get_connection_by_name(address.c_str());
    50 
     85 
    5186  // station sol
    5287  main_tab = new Tab(getFrameworkManager()->GetTabWidget(), name);
     
    5691  rotation_1 = new OneAxisRotation(setup_tab->NewRow(), "post rotation 1",OneAxisRotation::PreRotation);
    5792  rotation_2 = new OneAxisRotation(setup_tab->NewRow(), "post rotation 2",OneAxisRotation::PreRotation);
    58  
    59   Printf("Connecting to VRPN server on %s\n",address.c_str());
    60 }
    61 
    62 VrpnClient_impl::VrpnClient_impl(VrpnClient *self, std::string name,
    63                                  SerialPort *serialport, uint16_t us_period) {
    64   this->us_period = us_period;
    65   this->self = self;
    66   this->serialport = serialport;
    67   connection = NULL;
    68   mutex = new Mutex(self, name);
    69 
    70   serialport->SetBaudrate(111111);
    71   serialport->SetRxTimeout(us_period * 1000);
    72 
    73   // station sol
    74   main_tab = new Tab(getFrameworkManager()->GetTabWidget(), name);
    75   tab = new TabWidget(main_tab->NewRow(), name);
    76   setup_tab = new Tab(tab, "Reglages");
    77 
    78   rotation_1 = new OneAxisRotation(setup_tab->NewRow(), "post rotation 1",OneAxisRotation::PreRotation);
    79   rotation_2 = new OneAxisRotation(setup_tab->NewRow(), "post rotation 2",OneAxisRotation::PreRotation);
    80  
    81   Printf("Connecting to VRPN server on %s\n",serialport->ObjectName().c_str());
    8293}
    8394
    8495VrpnClient_impl::~VrpnClient_impl() {
    85   if (!UseXbee()) {
     96  if (connectionType==VrpnClient::Vrpn) {
    8697    // on fait une copie car le delete touche a trackables_copy via
    8798    // RemoveTrackable
     
    90101      delete trackables_copy.at(i)->self;
    91102    // trackables.clear();
    92   } else {
     103  } else if(connectionType==VrpnClient::Xbee || connectionType==VrpnClient::VrpnLite) {
    93104    // on fait une copie car le delete touche a xbee_objects_copy via
    94105    // RemoveTrackable
    95     vector<xbee_object> xbee_objects_copy = xbee_objects;
    96     for (unsigned int i = 0; i < xbee_objects_copy.size(); i++)
    97       delete xbee_objects_copy.at(i).vrpnobject->self;
     106    vector<liteObject_t> liteObjects_copy = liteObjects;
     107    for (unsigned int i = 0; i < liteObjects_copy.size(); i++)
     108      delete liteObjects_copy.at(i).vrpnobject->self;
    98109  }
    99110
    100111  delete main_tab;
    101112
    102   if (!UseXbee()) {
     113  if (connectionType==VrpnClient::Vrpn) {
    103114    // it will automatically delete connection
    104115    connection->removeReference();
     
    117128
    118129void VrpnClient_impl::AddTrackable(VrpnObject_impl *obj) {
    119   mutex->GetMutex();
    120   trackables.push_back(obj);
    121   mutex->ReleaseMutex();
     130  if (connectionType==VrpnClient::Vrpn) {
     131    mutex->GetMutex();
     132    trackables.push_back(obj);
     133    mutex->ReleaseMutex();
     134  } else {
     135    self->Warn("AddTrackable called but not in vrpn mode\n");
     136  }
     137}
     138
     139void VrpnClient_impl::AddTrackable(VrpnObject_impl *obj, uint8_t id) {
     140  if (connectionType==VrpnClient::VrpnLite || connectionType==VrpnClient::Xbee) {
     141    liteObject_t tmp;
     142    tmp.vrpnobject = obj;
     143    tmp.id = id;
     144    mutex->GetMutex();
     145    liteObjects.push_back(tmp);
     146    mutex->ReleaseMutex();
     147  } else {
     148    self->Warn("AddTrackable called but not in vrpnlite nor in xbee mode\n");
     149  }
    122150}
    123151
    124152void VrpnClient_impl::RemoveTrackable(VrpnObject_impl *obj) {
    125153  mutex->GetMutex();
    126   for (vector<VrpnObject_impl *>::iterator it = trackables.begin();
    127        it < trackables.end(); it++) {
    128     if (*it == obj) {
    129       trackables.erase(it);
    130       break;
     154  if (connectionType==VrpnClient::Vrpn) {
     155    for (vector<VrpnObject_impl *>::iterator it = trackables.begin();
     156         it < trackables.end(); it++) {
     157      if (*it == obj) {
     158        trackables.erase(it);
     159        break;
     160      }
    131161    }
    132162  }
    133   for (vector<xbee_object>::iterator it = xbee_objects.begin();
    134        it < xbee_objects.end(); it++) {
    135     if ((*it).vrpnobject == obj) {
    136       xbee_objects.erase(it);
    137       break;
     163  if (connectionType==VrpnClient::VrpnLite || connectionType==VrpnClient::Xbee) {
     164    for (vector<liteObject_t>::iterator it = liteObjects.begin();
     165         it < liteObjects.end(); it++) {
     166      if ((*it).vrpnobject == obj) {
     167        liteObjects.erase(it);
     168        break;
     169      }
    138170    }
    139171  }
    140172  mutex->ReleaseMutex();
    141 }
    142 
    143 void VrpnClient_impl::AddTrackable(VrpnObject_impl *obj, uint8_t id) {
    144   xbee_object tmp;
    145   tmp.vrpnobject = obj;
    146   tmp.id = id;
    147   mutex->GetMutex();
    148   xbee_objects.push_back(tmp);
    149   mutex->ReleaseMutex();
    150 }
    151 
    152 bool VrpnClient_impl::UseXbee(void) {
    153   if (connection == NULL) {
    154     return true;
    155   } else {
    156     return false;
    157   }
    158173}
    159174
     
    164179                       
    165180  while (!self->ToBeStopped()) {
    166     if (UseXbee()) {
     181    if (connectionType==VrpnClient::Xbee) {
    167182      ssize_t read = 0;
    168183      uint8_t response[38] = {0};
     
    193208
    194209          mutex->GetMutex();
    195           if (id < xbee_objects.size()) {
     210          if (id < liteObjects.size()) {
    196211            memcpy(pos, &response[9], sizeof(pos));
    197212            memcpy(quat, &response[9] + sizeof(pos), sizeof(quat));
     
    205220              // self->Printf("%i %f %f %f
    206221              // %f\n",id,pos[0],pos[1],pos[2],(float)self->GetTime()/(1000*1000));
    207               VrpnObject_impl::handle_pos(xbee_objects.at(id).vrpnobject, t);
     222              VrpnObject_impl::handle_pos(liteObjects.at(id).vrpnobject, t);
    208223            }
    209224          }
     
    211226        }
    212227      }
    213     } else {//!UseXbee()
     228    } else if(connectionType==VrpnClient::Vrpn) {
    214229        if(connection->connected()==vrpn_true && !isConnected) {
    215230            isConnected=true;
     
    235250            self->SleepMS(10);
    236251        }
     252    }else if(connectionType==VrpnClient::VrpnLite) {
     253      vrpn_TRACKERCB t;
     254      float pos[3];
     255      float quat[4];
     256      Time time;
     257      uint8_t id;
     258      char datas[sizeof(id) + sizeof(pos)+sizeof(quat)+ sizeof(time)];
     259      int rcv=dataSocket->RecvMessage(datas,sizeof(datas),50*1000*1000);
     260      if(rcv!=sizeof(datas)) continue; 
     261      id = datas[0];
     262      memcpy(pos, datas+sizeof(id), sizeof(pos));
     263      memcpy(quat, datas +sizeof(id)+ sizeof(pos), sizeof(quat));
     264      memcpy(&time, datas+sizeof(id) + sizeof(pos)+sizeof(quat), sizeof(time));
     265     
     266      for(int i=0;i<3;i++) dataSocket->NetworkToHost((char*)(&pos[i]),sizeof(pos[i]));
     267      for(int i=0;i<4;i++) dataSocket->NetworkToHost((char*)(&quat[i]),sizeof(quat[i]));
     268      dataSocket->NetworkToHost((char*)(&time),sizeof(time));
     269
     270      mutex->GetMutex();
     271      if (id < liteObjects.size()) {
     272        for (int i = 0; i < 3; i++) t.pos[i] = pos[i];
     273        // warning: t.quat is defined as (qx,qy,qz,qw), which is different from
     274        // flair::core::Quaternion
     275        t.quat[0] = quat[1];
     276        t.quat[1] = quat[2];
     277        t.quat[2] = quat[3];
     278        t.quat[3] = quat[0];
     279        t.msg_time.tv_sec=time/((Time)1000000000);
     280        t.msg_time.tv_usec=(time%((Time)1000000000))/((Time)1000);
     281        //Printf("%i %lld %lld %lld\n",id,time,t.msg_time.tv_sec,t.msg_time.tv_usec);
     282        VrpnObject_impl::handle_pos(liteObjects.at(id).vrpnobject, t);
     283      }
     284      mutex->ReleaseMutex();
    237285    }
    238286  }
  • trunk/lib/FlairSensorActuator/src/VrpnObject.h

    r286 r309  
    5050  * \brief Constructor
    5151  *
    52   * Construct a VrpnObject. Connection is done by IP.
     52  * Construct a VrpnObject. Connection is done by IP. (vrpn)
    5353  *
    5454  * \param name VRPN object name, should be the same as defined in the server
     
    6262  * \brief Constructor
    6363  *
    64   * Construct a VrpnObject. Connection is done by xbee.
     64  * Construct a VrpnObject. Connection is done by xbee or vrpnlite (see tools/VrpnLite in flair-src)
    6565  *
    6666  * \param name name
    67   * \param id VRPN object id, should be the same as defined in the xbee bridge
     67  * \param id VRPN object id, should be the same as defined in the xbee bridge or vrpnlite tool
    6868  * \param tab Tab for the user interface
    6969  * \param client VrpnClient of the connection, if unspecified, use the default one
     
    8888  * \brief Get Last Packet Time
    8989  *
    90   * \return Time of last received packe
     90  * \return Time of last received packet
    9191  */
    9292  core::Time GetLastPacketTime(void) const;
  • trunk/lib/FlairSensorActuator/src/VrpnObject_impl.cpp

    r286 r309  
    4848                return;
    4949        }
    50   if (id == -1 && parent->UseXbee()) {
     50  if (id == -1 && GetVrpnClient()->ConnectionType()==VrpnClient::Xbee) {
    5151    self->Err("erreur aucun identifiant specifie pour la connexion Xbee\n");
    5252  }
    53   if (id != -1 && !parent->UseXbee()) {
    54     self->Warn(
    55         "identifiant pour la connexion Xbee ignore car pas en mode Xbee\n");
     53  if (id == -1 && GetVrpnClient()->ConnectionType()==VrpnClient::VrpnLite) {
     54    self->Err("erreur aucun identifiant specifie pour la connexion VrpnLite\n");
     55  }
     56  if (id != -1 && GetVrpnClient()->ConnectionType()==VrpnClient::Vrpn) {
     57    self->Warn("identifiant pour la connexion ignore car inutile en mode Vrpn\n");
    5658  }
    5759 
     
    8486  z_plot->AddCurve(output->Element(6));
    8587 
    86   if (parent->UseXbee()) {
    87     tracker = NULL;
     88  if (GetVrpnClient()->ConnectionType()==VrpnClient::Xbee) {
    8889    parent->pimpl_->AddTrackable(this, id);
    89   } else {
     90  } else if(GetVrpnClient()->ConnectionType()==VrpnClient::Vrpn){
    9091    tracker = new vrpn_Tracker_Remote(name.c_str(), parent->pimpl_->connection);
    9192    tracker->register_change_handler(this, handle_pos);
    9293    tracker->shutup = true;
    9394    parent->pimpl_->AddTrackable(this);
     95  } else if(GetVrpnClient()->ConnectionType()==VrpnClient::VrpnLite){
     96    parent->pimpl_->AddTrackable(this, id);
    9497  }
    9598 
     
    99102VrpnObject_impl::~VrpnObject_impl(void) {
    100103  parent->pimpl_->RemoveTrackable(this);
    101   if (tracker != NULL) {// normal
     104  if (GetVrpnClient()->ConnectionType()==VrpnClient::Vrpn) {
    102105    tracker->unregister_change_handler(this, handle_pos);
    103106    delete tracker;
  • trunk/lib/FlairSensorActuator/src/unexported/VrpnClient_impl.h

    r294 r309  
    2929class Mutex;
    3030class SerialPort;
     31class UdpSocket;
    3132}
    3233namespace gui {
     
    5051  VrpnClient_impl(flair::sensor::VrpnClient *self, std::string name,
    5152                  flair::core::SerialPort *serialport, uint16_t us_period);
     53  VrpnClient_impl(flair::sensor::VrpnClient *self, std::string name,
     54                  uint16_t port);
    5255  ~VrpnClient_impl();
    5356  void AddTrackable(VrpnObject_impl *obj);    // normal
     
    5659  void ComputeRotations(flair::core::Vector3Df &point);
    5760  void ComputeRotations(flair::core::Quaternion &quat);
    58   bool UseXbee(void);
    5961  void Run(void);
    6062  flair::gui::Tab *setup_tab;
    6163  flair::gui::TabWidget *tab;
    6264  vrpn_Connection *connection;
     65  flair::sensor::VrpnClient::ConnectionType_t connectionType;
    6366
    6467private:
     68  void CommonConstructor(std::string name);
    6569  flair::sensor::VrpnClient *self;
    6670  flair::core::Mutex *mutex;
    6771  uint16_t us_period;
    6872  std::vector<VrpnObject_impl *> trackables;
    69   typedef struct xbee_object {
     73  typedef struct liteObject_t {
    7074    VrpnObject_impl *vrpnobject;
    7175    uint8_t id;
    72   } xbee_object;
     76  } liteObject_t;
    7377
    74   std::vector<xbee_object> xbee_objects;
     78  std::vector<liteObject_t> liteObjects;
    7579  flair::gui::Tab *main_tab;
    7680  flair::core::OneAxisRotation *rotation_1, *rotation_2;
     
    7882        bool isConnected;//only for ip connection, not for xbee
    7983  std::string address;
     84  flair::core::UdpSocket* dataSocket;
     85 
    8086};
    8187
Note: See TracChangeset for help on using the changeset viewer.