Ignore:
Timestamp:
09/25/19 15:29:26 (2 years ago)
Author:
Sanahuja Guillaume
Message:

use less bandwidth in vprnlite

File:
1 edited

Legend:

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

    r309 r330  
    4040
    4141VrpnClient_impl::VrpnClient_impl(VrpnClient *self, string name,
    42                                  std::string address) {
     42                                 std::string address,flair::sensor::VrpnClient::ConnectionType_t connectionType) {
    4343  this->self = self;
    4444  this->address = address;
     45  this->connectionType = connectionType;
    4546  isConnected=false;
    46   connectionType=VrpnClient::Vrpn;
    47 
    48   connection = vrpn_get_connection_by_name(address.c_str());
     47 
     48  if(connectionType==VrpnClient::Vrpn) {
     49    connection = vrpn_get_connection_by_name(address.c_str());
     50    Printf("Connecting to VRPN server on %s\n",address.c_str());
     51  } else if(connectionType==VrpnClient::VrpnLite) {
     52    dataSocket =new UdpSocket(getFrameworkManager(), "data_socket", address);
     53    Printf("Connecting to VRPN-lite server on %s\n",address.c_str());
     54  } else {
     55    self->Err("Bad connection type, try using naother constructor\n");
     56  }
    4957
    5058  CommonConstructor(name);
    51  
    52   Printf("Connecting to VRPN server on %s\n",address.c_str());
    53 }
    54 
    55 VrpnClient_impl::VrpnClient_impl(VrpnClient *self, string name,
    56                                  uint16_t port) {
    57   this->self = self;
    58         isConnected=false;
    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 }
     59}
     60
     61
    6762
    6863VrpnClient_impl::VrpnClient_impl(VrpnClient *self, string name,
     
    132127    trackables.push_back(obj);
    133128    mutex->ReleaseMutex();
     129  } else if (connectionType==VrpnClient::VrpnLite) {
     130    if(liteObjects.size()<0xffff) {
     131      liteObject_t tmp;
     132      tmp.vrpnobject = obj;
     133      tmp.id = liteObjects.size();
     134      mutex->GetMutex();
     135      liteObjects.push_back(tmp);
     136      mutex->ReleaseMutex();
     137      //Printf("%i %s\n",tmp.id,obj->self->ObjectName().c_str());
     138     
     139      char char_array[obj->self->ObjectName().length() + 2];//id coded on 16bits
     140      strcpy(char_array, obj->self->ObjectName().c_str());
     141      uint16_t* idPtr=(uint16_t*)&char_array[obj->self->ObjectName().length()];
     142      *idPtr=tmp.id;
     143      dataSocket->HostToNetwork((char*)idPtr,sizeof(uint16_t));
     144      dataSocket->SendMessage(char_array,obj->self->ObjectName().length() + 2);
     145    }else {
     146      self->Warn("too much trackables for vrpnlite connection, not adding %s\n",obj->self->ObjectName().c_str());
     147    }
    134148  } else {
    135     self->Warn("AddTrackable called but not in vrpn mode\n");
     149    self->Warn("AddTrackable called but not in vrpn mode nor in vrpnlite mode\n");
    136150  }
    137151}
     
    146160    mutex->ReleaseMutex();
    147161  } else {
    148     self->Warn("AddTrackable called but not in vrpnlite nor in xbee mode\n");
     162    self->Warn("AddTrackable called but not in xbee mode\n");
    149163  }
    150164}
     
    153167  mutex->GetMutex();
    154168  if (connectionType==VrpnClient::Vrpn) {
    155     for (vector<VrpnObject_impl *>::iterator it = trackables.begin();
    156          it < trackables.end(); it++) {
     169    for (vector<VrpnObject_impl *>::iterator it = trackables.begin();it < trackables.end(); it++) {
    157170      if (*it == obj) {
    158171        trackables.erase(it);
     
    162175  }
    163176  if (connectionType==VrpnClient::VrpnLite || connectionType==VrpnClient::Xbee) {
    164     for (vector<liteObject_t>::iterator it = liteObjects.begin();
    165          it < liteObjects.end(); it++) {
     177    for (vector<liteObject_t>::iterator it = liteObjects.begin();it < liteObjects.end(); it++) {
    166178      if ((*it).vrpnobject == obj) {
    167179        liteObjects.erase(it);
     
    243255            //printf("%lld\n",GetTime()/(1000*1000));
    244256            mutex->GetMutex();
    245             for (unsigned int i = 0; i < trackables.size(); i++)
    246                 trackables.at(i)->tracker->mainloop();
     257            for (unsigned int i = 0; i < trackables.size(); i++) {
     258             // Printf("tracker %i\n",i);
     259              trackables.at(i)->tracker->mainloop();
     260              //Printf("tracker %i ok\n",i);
     261            }
    247262            mutex->ReleaseMutex();
    248263        } else {
     
    251266        }
    252267    }else if(connectionType==VrpnClient::VrpnLite) {
    253       vrpn_TRACKERCB t;
    254       float pos[3];
    255       float quat[4];
     268      mutex->GetMutex();
     269   
     270      int16_t pos[3];
     271      int16_t quat[4];
    256272      Time time;
    257       uint8_t id;
    258       char datas[sizeof(id) + sizeof(pos)+sizeof(quat)+ sizeof(time)];
     273      char datas[liteObjects.size()*(sizeof(pos)+sizeof(quat))+ sizeof(time)];
     274      char *datasPtr=datas;
     275     
    259276      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]));
     277      if(rcv!=sizeof(datas)) {
     278        if(rcv>0) Printf("discarding message (size %i/%i)\n",rcv,sizeof(datas));
     279        mutex->ReleaseMutex();
     280        continue; 
     281      }
     282     
     283      memcpy(&time, datasPtr+sizeof(datas)-sizeof(time), sizeof(time));
    268284      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];
     285     
     286      for (vector<liteObject_t>::iterator it = liteObjects.begin();it < liteObjects.end(); it++) {
     287        memcpy(pos, datasPtr, sizeof(pos));
     288        datasPtr+=sizeof(pos);
     289        memcpy(quat,datasPtr, sizeof(quat));
     290        datasPtr+=sizeof(quat);
     291       
     292        for(int i=0;i<3;i++) dataSocket->NetworkToHost((char*)(&pos[i]),sizeof(pos[i]));
     293        for(int i=0;i<4;i++) dataSocket->NetworkToHost((char*)(&quat[i]),sizeof(quat[i]));
     294       
     295        vrpn_TRACKERCB t;
     296        for (int i = 0; i < 3; i++) t.pos[i] = ConvertPosition(pos[i]);
    273297        // warning: t.quat is defined as (qx,qy,qz,qw), which is different from
    274298        // 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];
     299        t.quat[0] = ConvertQuaternion(quat[1]);
     300        t.quat[1] = ConvertQuaternion(quat[2]);
     301        t.quat[2] = ConvertQuaternion(quat[3]);
     302        t.quat[3] = ConvertQuaternion(quat[0]);
    279303        t.msg_time.tv_sec=time/((Time)1000000000);
    280304        t.msg_time.tv_usec=(time%((Time)1000000000))/((Time)1000);
    281305        //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       }
     306        VrpnObject_impl::handle_pos((void*)(it->vrpnobject), t);
     307      }
     308     
    284309      mutex->ReleaseMutex();
    285310    }
    286311  }
    287312}
     313
     314float VrpnClient_impl::ConvertPosition(int16_t value) const {
     315  return (float)value/1000.;
     316}
     317
     318float VrpnClient_impl::ConvertQuaternion(int16_t value) const {
     319  return (float)value/32767.;
     320}
Note: See TracChangeset for help on using the changeset viewer.