Ignore:
Timestamp:
03/13/19 16:40:18 (5 years ago)
Author:
Sanahuja Guillaume
Message:

vrpnlite support

File:
1 edited

Legend:

Unmodified
Added
Removed
  • 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  }
Note: See TracChangeset for help on using the changeset viewer.