Changeset 330 in flair-src for trunk/tools


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

use less bandwidth in vprnlite

Location:
trunk/tools/VrpnLite/src
Files:
3 edited

Legend:

Unmodified
Added
Removed
  • trunk/tools/VrpnLite/src/VrpnLite.cpp

    r317 r330  
    2525using namespace flair::sensor;
    2626
    27 VrpnLite::VrpnLite(string clientAddress,string serveurAddress): Thread(getFrameworkManager(),"VrpnLite",90) {
    28   vrpnclient=new VrpnClient("vrpn", serveurAddress,80);
    29   VrpnObject* vrpnobject = new VrpnObject("x4_0",vrpnclient->GetTabWidget(),vrpnclient);
    30   //VrpnObject* vrpnobject2 = new VrpnObject("target",vrpnclient->GetTabWidget(),vrpnclient);
    31  
    32   vrpnobjects.push_back(vrpnobject);
    33   //vrpnobjects.push_back(vrpnobject2);
    34  
    35   dataSocket = new UdpSocket(this,"client socket",clientAddress);
    36  
     27VrpnLite::VrpnLite(int vrpnLitePort,string vrpnServerAddress): Thread(getFrameworkManager(),"VrpnLite",90) {
     28  vrpnclient=new VrpnClient("vrpn", vrpnServerAddress,80);
     29  dataSocket = new UdpSocket(this,"client socket",vrpnLitePort);
    3730  vrpnclient->Start();
    3831}
     
    4235
    4336void VrpnLite::Run(void) {
     37  Time dataSocketTimeout;
     38  char msg[256];
     39 
    4440  while (!ToBeStopped()) {
    4541   
    46     WaitUpdate(vrpnobjects.at(0));//tood improve this wait, to be sure all vrpnobject are up to date; or one thread by object?
    47     //could be also lighter to send only one frame with all objects
    48     int i=0;
    49     for (vector<VrpnObject*>::iterator it = vrpnobjects.begin();it < vrpnobjects.end(); it++) {
    50       SendObject(*it,i);
    51       i++;
     42    if(vrpnobjects.size()>0) {
     43      //wait for last one to be sure all are up to date
     44      if(WaitUpdate(vrpnobjects.at(vrpnobjects.size()-1),100000000)) {
     45          SendObjects();
     46      }
     47      dataSocketTimeout=TIME_NONBLOCK;
     48    } else {
     49      dataSocketTimeout=100000000;
    5250    }
    5351   
     52    ssize_t rcv=dataSocket->RecvMessage(msg,sizeof(msg),dataSocketTimeout);
     53    if(rcv>0) {
     54      string object=msg;
     55      uint16_t* idPtr=(uint16_t*)&msg[rcv-2];
     56      dataSocket->HostToNetwork((char*)idPtr,sizeof(uint16_t));
     57      //assume we receive it in the good order
     58      if(*idPtr==vrpnobjects.size()) {
     59        Printf("adding object %s with id %i\n",object.c_str(),*idPtr);
     60        VrpnObject* vrpnobject = new VrpnObject(object,vrpnclient->GetTabWidget());
     61        vrpnobjects.push_back(vrpnobject);
     62      }else {
     63        Err("adding object %s failed, expected id %i, got %i\n",object.c_str(),vrpnobjects.size(),*idPtr);
     64      }
     65    }
    5466  }
    5567}
    5668
    57 void VrpnLite::SendObject(const VrpnObject* vrpnobject,uint8_t id) const{
     69void VrpnLite::SendObjects(void) const{
     70  int16_t position[3];
     71  int16_t quaternion[4];
     72  Time time;
     73  char datas[vrpnobjects.size()*(sizeof(position)+sizeof(quaternion))+ sizeof(time)];
     74  char *datasPtr=datas;
     75 
     76  for (vector<VrpnObject*>::const_iterator it = vrpnobjects.begin();it < vrpnobjects.end(); it++) {
    5877    Vector3Df objectPosition;
    5978    Quaternion objectQuaternion;
    60    
     79    const VrpnObject* vrpnobject=*it;
     80
    6181    vrpnobject->GetPosition(objectPosition);
    6282    vrpnobject->GetQuaternion(objectQuaternion);
    63     Time time=vrpnobject->GetLastPacketTime();
    64    
    65     float position[3];
    66     position[0]=objectPosition.x;
    67     position[1]=objectPosition.y;
    68     position[2]=objectPosition.z;
    69     float quaternion[4];
    70     quaternion[0]=objectQuaternion.q0;
    71     quaternion[1]=objectQuaternion.q1;
    72     quaternion[2]=objectQuaternion.q2;
    73     quaternion[3]=objectQuaternion.q3;
    74    
     83    time=vrpnobject->GetLastPacketTime();
     84
     85    position[0]=ConvertPosition(objectPosition.x);
     86    position[1]=ConvertPosition(objectPosition.y);
     87    position[2]=ConvertPosition(objectPosition.z);
     88    quaternion[0]=ConvertQuaternion(objectQuaternion.q0);
     89    quaternion[1]=ConvertQuaternion(objectQuaternion.q1);
     90    quaternion[2]=ConvertQuaternion(objectQuaternion.q2);
     91    quaternion[3]=ConvertQuaternion(objectQuaternion.q3);
     92
    7593    for(int i=0;i<3;i++) dataSocket->HostToNetwork((char*)(&position[i]),sizeof(position[i]));
    7694    for(int i=0;i<4;i++) dataSocket->HostToNetwork((char*)(&quaternion[i]),sizeof(quaternion[i]));
    77     dataSocket->HostToNetwork((char*)(&time),sizeof(Time));
    78    
    79     char datas[sizeof(id) + sizeof(position)+sizeof(quaternion)+ sizeof(time)];
    80    
    81     datas[0]=id;
    82     memcpy(datas+sizeof(id),position, sizeof(position));
    83     memcpy(datas +sizeof(id)+ sizeof(position),quaternion, sizeof(quaternion));
    84     memcpy(datas+sizeof(id) + sizeof(position)+sizeof(quaternion),&time, sizeof(time));
    85    
    86     dataSocket->SendMessage(datas,sizeof(datas));
     95
     96    memcpy(datasPtr,position, sizeof(position));
     97    datasPtr+=sizeof(position);
     98    memcpy(datasPtr,quaternion, sizeof(quaternion));
     99    datasPtr+=sizeof(quaternion);
     100  }
     101 
     102  dataSocket->HostToNetwork((char*)(&time),sizeof(Time));
     103  memcpy(datasPtr,&time, sizeof(time));//only one time for all VrpnObject; suppose it is the same!
     104  dataSocket->SendMessage(datas,sizeof(datas));
    87105}
     106
     107int16_t VrpnLite::ConvertQuaternion(float value) const{
     108  int16_t tmp;
     109  tmp=value*32767.;
     110  if(value<-1) {
     111    tmp=-32767;
     112    Warn("position value is %f, saturating it to %i\n",value,tmp);
     113  }
     114  if(value>1) {
     115    tmp=32767;
     116    Warn("position value is %f, saturating it to %i\n",value,tmp);
     117  }
     118  return tmp;
     119}
     120
     121int16_t VrpnLite::ConvertPosition(float value) const{
     122  int16_t tmp;
     123  tmp=value*1000;
     124  if(value<-32.768) {
     125    tmp=-32768;
     126    Warn("position value is %f, saturating it to %i\n",value,tmp);
     127  }
     128  if(value>32.767) {
     129    tmp=32767;
     130    Warn("position value is %f, saturating it to %i\n",value,tmp);
     131  }
     132  return tmp;
     133}
  • trunk/tools/VrpnLite/src/VrpnLite.h

    r308 r330  
    3030class VrpnLite: public flair::core::Thread {
    3131  public:
    32       VrpnLite(std::string clientAddress,std::string serveurAddress);
     32      VrpnLite(int vrpnLitePort,std::string vrpnServerAddress);
    3333      ~VrpnLite();
    3434
     
    3838      std::vector<flair::sensor::VrpnObject*> vrpnobjects;
    3939      flair::core::UdpSocket* dataSocket;
    40       void SendObject(const flair::sensor::VrpnObject* vrpnobject,uint8_t id) const;
    41      
     40      void SendObjects(void) const;
     41      int16_t ConvertPosition(float value) const;
     42      int16_t ConvertQuaternion(float value) const;
    4243};
    4344
  • trunk/tools/VrpnLite/src/main.cpp

    r308 r330  
    2222
    2323string xml_file;
    24 string clientAddress,serverAddress;
    25 int port;
     24string vrpnServerAddress;
     25int gcsPort,vrpnLitePort;
    2626
    2727void parseOptions(int argc, char **argv);
     
    3232  FrameworkManager *manager;
    3333  manager = new FrameworkManager("vrpnforwarder");
    34   manager->SetupConnection("127.0.0.1", port);
     34  manager->SetupConnection("127.0.0.1", gcsPort);
    3535  manager->SetupUserInterface(xml_file);
    3636 
    37   VrpnLite* vrpnlite=new VrpnLite(clientAddress,serverAddress);
     37  VrpnLite* vrpnlite=new VrpnLite(vrpnLitePort,vrpnServerAddress);
    3838 
    3939  vrpnlite->Start();
     
    4747    CmdLine cmd("Command description message", ' ', "0.1");
    4848
    49     ValueArg<string> clientaddressArg("c", "caddress","client address", true,
    50                                 "127.0.0.1:3884", "string");
    51     cmd.add(clientaddressArg);
     49    ValueArg<int> vrpnLitePortArg("v", "vport","vrpn lite port", true,
     50                                3884, "int");
     51    cmd.add(vrpnLitePortArg);
    5252   
    5353    ValueArg<string> serveraddressArg("s", "saddress","server address", true,
     
    5555    cmd.add(serveraddressArg);
    5656
    57     ValueArg<int> portArg("p", "port","local port used to connect to the ground station",
     57    ValueArg<int> gcsPortArg("p", "port","local port used to connect to the ground station",
    5858                          false, 9000, "int");
    59     cmd.add(portArg);
     59    cmd.add(gcsPortArg);
    6060
    6161
     
    6666    cmd.parse(argc, argv);
    6767
    68     clientAddress = clientaddressArg.getValue();
    69     serverAddress = serveraddressArg.getValue();
    70     port = portArg.getValue();
     68    vrpnLitePort = vrpnLitePortArg.getValue();
     69    vrpnServerAddress = serveraddressArg.getValue();
     70    gcsPort = gcsPortArg.getValue();
    7171    xml_file = xmlArg.getValue();
    7272
Note: See TracChangeset for help on using the changeset viewer.